rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Change defaults // Add optional filter config

This commit is contained in:
borisbstyle 2016-08-01 23:37:52 +02:00
parent 75c139faf2
commit ca703b1ff1
5 changed files with 36 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/main/config/config.c
View File

@ -240,13 +240,13 @@ static void resetPidProfile(pidProfile_t *pidProfile)
// Betaflight PID controller parameters
pidProfile->ptermSetpointWeight = 75;
pidProfile->dtermSetpointWeight = 0;
pidProfile->dtermSetpointWeight = 200;
pidProfile->pidMaxVelocity = 1000;
pidProfile->pidMaxVelocityYaw = 50;
pidProfile->toleranceBand = 15;
pidProfile->toleranceBand = 20;
pidProfile->toleranceBandReduction = 40;
pidProfile->zeroCrossAllowanceCount = 2;
pidProfile->itermThrottleGain = 10;
pidProfile->itermThrottleGain = 0;
#ifdef GTUNE
pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune.
@ -475,11 +475,12 @@ static void resetConf(void)
masterConfig.gyro_sync_denom = 4;
masterConfig.pid_process_denom = 2;
#endif
masterConfig.gyro_soft_lpf_hz = 100;
masterConfig.gyro_soft_type = GYRO_FILTER_PT1;
masterConfig.gyro_soft_lpf_hz = 80;
masterConfig.gyro_soft_notch_hz = 0;
masterConfig.gyro_soft_notch_q = 5;
masterConfig.debug_mode = 0;
masterConfig.debug_mode = DEBUG_NONE;
resetAccelerometerTrims(&masterConfig.accZero);
@ -527,7 +528,7 @@ static void resetConf(void)
masterConfig.rxConfig.rssi_channel = 0;
masterConfig.rxConfig.rssi_scale = RSSI_SCALE_DEFAULT;
masterConfig.rxConfig.rssi_ppm_invert = 0;
masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_OFF;
masterConfig.rxConfig.rcSmoothing = RC_SMOOTHING_AUTO;
masterConfig.rxConfig.rcSmoothInterval = 9;
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
masterConfig.rxConfig.max_aux_channel = MAX_AUX_CHANNELS;
@ -742,7 +743,7 @@ void activateConfig(void)
&currentProfile->pidProfile
);
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q);
gyroUseConfig(&masterConfig.gyroConfig, masterConfig.gyro_soft_lpf_hz, masterConfig.gyro_soft_notch_hz, masterConfig.gyro_soft_notch_q, masterConfig.gyro_soft_type);
#ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig);

1
src/main/config/config_master.h
View File

@ -58,6 +58,7 @@ typedef struct master_t {
uint8_t acc_for_fast_looptime; // shorten acc processing time by using 1 out of 9 samples. For combination with fast looptimes.
uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
uint8_t gyro_sync_denom; // Gyro sample divider
uint8_t gyro_soft_type; // Gyro Filter Type
uint8_t gyro_soft_lpf_hz; // Biquad gyro lpf hz
uint16_t gyro_soft_notch_hz; // Biquad gyro notch hz
uint8_t gyro_soft_notch_q; // Biquad gyro notch quality

7
src/main/io/serial_cli.c
View File

@ -497,6 +497,10 @@ static const char * const lookupTableRcSmoothing[] = {
"OFF", "DEFAULT", "AUTO", "MANUAL"
};
static const char * const lookupTableGyroSoftLowpassType[] = {
"NORMAL", "HIGH"
};
typedef struct lookupTableEntry_s {
const char * const *values;
const uint8_t valueCount;
@ -534,6 +538,7 @@ typedef enum {
TABLE_MOTOR_PWM_PROTOCOL,
TABLE_DELTA_METHOD,
TABLE_RC_SMOOTHING,
TABLE_GYRO_LOWPASS_TYPE,
#ifdef OSD
TABLE_OSD,
#endif
@ -571,6 +576,7 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) },
{ lookupTableDeltaMethod, sizeof(lookupTableDeltaMethod) / sizeof(char *) },
{ lookupTableRcSmoothing, sizeof(lookupTableRcSmoothing) / sizeof(char *) },
{ lookupTableGyroSoftLowpassType, sizeof(lookupTableGyroSoftLowpassType) / sizeof(char *) },
#ifdef OSD
{ lookupTableOsdType, sizeof(lookupTableOsdType) / sizeof(char *) },
#endif
@ -742,6 +748,7 @@ const clivalue_t valueTable[] = {
{ "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } },
{ "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_sync_denom, .config.minmax = { 1, 8 } },
{ "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_soft_type, .config.lookup = { TABLE_GYRO_LOWPASS_TYPE } },
{ "gyro_lowpass", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_lpf_hz, .config.minmax = { 0, 255 } },
{ "gyro_notch_hz", VAR_UINT16 | MASTER_VALUE, &masterConfig.gyro_soft_notch_hz, .config.minmax = { 0, 500 } },
{ "gyro_notch_q", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyro_soft_notch_q, .config.minmax = { 1, 100 } },

17
src/main/sensors/gyro.c
View File

@ -47,17 +47,21 @@ static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static const gyroConfig_t *gyroConfig;
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT];
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfType;
static uint16_t gyroSoftNotchHz;
static uint8_t gyroSoftNotchQ;
static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
static float gyroDt;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q)
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q, uint8_t gyro_soft_lpf_type)
{
gyroConfig = gyroConfigToUse;
gyroSoftLpfHz = gyro_soft_lpf_hz;
gyroSoftNotchHz = gyro_soft_notch_hz;
gyroSoftNotchQ = gyro_soft_notch_q;
gyroSoftLpfType = gyro_soft_lpf_type;
}
void gyroInit(void)
@ -65,7 +69,10 @@ void gyroInit(void)
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterNotch[axis], gyroSoftNotchHz, gyro.targetLooptime, ((float) gyroSoftNotchQ) / 10, FILTER_NOTCH);
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
if (gyroSoftLpfType == GYRO_FILTER_BIQUAD)
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
else
gyroDt = gyro.targetLooptime / 1000.0f;
}
}
}
@ -175,7 +182,11 @@ void gyroUpdate(void)
debug[axis*2 + 1] = lrintf(sample);
}
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);;
if (gyroSoftLpfType == GYRO_FILTER_BIQUAD) {
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], sample);
} else {
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], sample, gyroSoftLpfHz, gyroDt);
}
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
} else {

7
src/main/sensors/gyro.h
View File

@ -31,6 +31,11 @@ typedef enum {
GYRO_MAX = GYRO_FAKE
} gyroSensor_e;
typedef enum {
GYRO_FILTER_PT1 = 0,
GYRO_FILTER_BIQUAD,
} gyroFilter_e;
extern gyro_t gyro;
extern int32_t gyroADC[XYZ_AXIS_COUNT];
@ -40,7 +45,7 @@ typedef struct gyroConfig_s {
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
} gyroConfig_t;
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q);
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse, uint8_t gyro_soft_lpf_hz, uint16_t gyro_soft_notch_hz, uint8_t gyro_soft_notch_q, uint8_t gyro_soft_lpf_type);
void gyroSetCalibrationCycles(void);
void gyroInit(void);
void gyroUpdate(void);