Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

More Filter Config Options

Browse source 
refactor

Bump EEPROM Version
This commit is contained in:
borisbstyle 2015-12-10 01:44:05 +01:00
parent 6d51ab6e53
commit e490564815
4 changed files with 46 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/common/filter.c
View file

@ -40,7 +40,7 @@ static int8_t gyroFIRCoeff_1000[3][9] = { { 0, 0, 12, 23, 40, 51, 52, 40, 38 },
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level) int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level)
{ {
return gyroFIRCoeff_1000[filter_level]; return gyroFIRCoeff_1000[filter_level-1];
} }
// 9 Tap FIR filter as described here: // 9 Tap FIR filter as described here:

8
src/main/config/config.c
View file

@ -133,7 +133,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0; static uint8_t currentControlRateProfileIndex = 0;
controlRateConfig_t *currentControlRateProfile; controlRateConfig_t *currentControlRateProfile;
static const uint8_t EEPROM_CONF_VERSION = 115; static const uint8_t EEPROM_CONF_VERSION = 116;
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
{ {
@ -175,7 +175,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->I8[PIDVEL] = 45; pidProfile->I8[PIDVEL] = 45;
pidProfile->D8[PIDVEL] = 1; pidProfile->D8[PIDVEL] = 1;
pidProfile->gyro_soft_lpf = 0; // LOW filtering by default pidProfile->gyro_soft_lpf = 1; // LOW filtering by default
pidProfile->dterm_cut_hz = 40; pidProfile->dterm_cut_hz = 40;
pidProfile->yaw_pterm_cut_hz = 50; pidProfile->yaw_pterm_cut_hz = 50;
@ -399,7 +399,7 @@ static void resetConf(void)
masterConfig.current_profile_index = 0; // default profile masterConfig.current_profile_index = 0; // default profile
masterConfig.dcm_kp = 2500; // 1.0 * 10000 masterConfig.dcm_kp = 2500; // 1.0 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000 masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 1; // 1KHZ or 8KHZ masterConfig.gyro_lpf = 1; // 188HZ
resetAccelerometerTrims(&masterConfig.accZero); resetAccelerometerTrims(&masterConfig.accZero);
@ -694,7 +694,9 @@ void activateConfig(void)
&currentProfile->pidProfile &currentProfile->pidProfile
); );
if (currentProfile->pidProfile.gyro_soft_lpf) {
useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf)); useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf));
}
#ifdef TELEMETRY #ifdef TELEMETRY
telemetryUseConfig(&masterConfig.telemetryConfig); telemetryUseConfig(&masterConfig.telemetryConfig);

40
src/main/flight/pid.c
View file

@ -96,7 +96,8 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
float ITerm,PTerm,DTerm; float ITerm,PTerm,DTerm;
int32_t stickPosAil, stickPosEle, mostDeflectedPos; int32_t stickPosAil, stickPosEle, mostDeflectedPos;
static float lastError[3]; static float lastError[3];
float delta; static float delta1[3], delta2[3];
float delta, deltaSum;
int axis; int axis;
float horizonLevelStrength = 1; float horizonLevelStrength = 1;
static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
@ -198,12 +199,21 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// would be scaled by different dt each time. Division by dT fixes that. // would be scaled by different dt each time. Division by dT fixes that.
delta *= (1.0f / dT); delta *= (1.0f / dT);
// Dterm low pass if (!pidProfile->gyro_soft_lpf) {
if (pidProfile->dterm_cut_hz) { // add moving average here to reduce noise
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); deltaSum = (delta1[axis] + delta2[axis] + delta) / 3;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
} else {
deltaSum = delta;
} }
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f); // Dterm low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = constrainf(deltaSum * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
// -----calculate total PID output // -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000); axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
@ -229,7 +239,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
int32_t errorAngle; int32_t errorAngle;
int axis; int axis;
int32_t delta; int32_t delta, deltaSum;
static int32_t delta1[3], delta2[3];
int32_t PTerm, ITerm, DTerm; int32_t PTerm, ITerm, DTerm;
static int32_t lastError[3] = { 0, 0, 0 }; static int32_t lastError[3] = { 0, 0, 0 };
static int32_t previousErrorGyroI[3] = { 0, 0, 0 }; static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
@ -335,12 +346,21 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// would be scaled by different dt each time. Division by dT fixes that. // would be scaled by different dt each time. Division by dT fixes that.
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6; delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
// Dterm delta low pass if (!pidProfile->gyro_soft_lpf) {
if (pidProfile->dterm_cut_hz) { // add moving average here to reduce noise
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT); deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
} else {
deltaSum = delta * 2;
} }
DTerm = (delta * 2 * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8; // Multiplied by 2 to approximately match old scaling // Dterm delta low pass
if (pidProfile->dterm_cut_hz) {
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
}
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
// -----calculate total PID output // -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm; axisPID[axis] = PTerm + ITerm + DTerm;

16
src/main/io/serial_cli.c
View file

@ -348,12 +348,14 @@ static const char * const lookupTableSerialRX[] = {
}; };
static const char * const lookupTableGyroFilter[] = { static const char * const lookupTableGyroFilter[] = {
"LOW", "MEDIUM", "HIGH" "OFF", "LOW", "MEDIUM", "HIGH"
}; };
static const char * const lookupTableGyroSampling[] = { static const char * const lookupTableGyroLpf[] = {
"8KHZ", "OFF",
"1KHZ" "188HZ",
"98HZ",
"42HZ"
}; };
typedef struct lookupTableEntry_s { typedef struct lookupTableEntry_s {
@ -377,7 +379,7 @@ typedef enum {
TABLE_PID_CONTROLLER, TABLE_PID_CONTROLLER,
TABLE_SERIAL_RX, TABLE_SERIAL_RX,
TABLE_GYRO_FILTER, TABLE_GYRO_FILTER,
TABLE_GYRO_SAMPLING, TABLE_GYRO_LPF,
} lookupTableIndex_e; } lookupTableIndex_e;
static const lookupTableEntry_t lookupTables[] = { static const lookupTableEntry_t lookupTables[] = {
@ -394,7 +396,7 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) }, { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
{ lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) }, { lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) },
{ lookupTableGyroSampling, sizeof(lookupTableGyroSampling) / sizeof(char *) } { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }
}; };
#define VALUE_TYPE_OFFSET 0 #define VALUE_TYPE_OFFSET 0
@ -545,7 +547,7 @@ const clivalue_t valueTable[] = {
{ "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } }, { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } },
{ "gyro_sampling", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_SAMPLING } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } },
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, .config.minmax = { 0, 128 } },
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } }, { "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_kp, .config.minmax = { 0, 50000 } },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } }, { "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, &masterConfig.dcm_ki, .config.minmax = { 0, 50000 } },