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

More Filter Config Options 

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
Show Stats 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)
{
return gyroFIRCoeff_1000[filter_level];
return gyroFIRCoeff_1000[filter_level-1];
}
// 9 Tap FIR filter as described here:

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

@ -133,7 +133,7 @@ static uint32_t activeFeaturesLatch = 0;
static uint8_t currentControlRateProfileIndex = 0;
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)
{
@ -175,7 +175,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->I8[PIDVEL] = 45;
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->yaw_pterm_cut_hz = 50;
@ -399,7 +399,7 @@ static void resetConf(void)
masterConfig.current_profile_index = 0; // default profile
masterConfig.dcm_kp = 2500; // 1.0 * 10000
masterConfig.dcm_ki = 0; // 0.003 * 10000
masterConfig.gyro_lpf = 1; // 1KHZ or 8KHZ
masterConfig.gyro_lpf = 1; // 188HZ
resetAccelerometerTrims(&masterConfig.accZero);
@ -694,7 +694,9 @@ void activateConfig(void)
&currentProfile->pidProfile
);
useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf));
if (currentProfile->pidProfile.gyro_soft_lpf) {
useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf));
}
#ifdef TELEMETRY
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;
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
static float lastError[3];
float delta;
static float delta1[3], delta2[3];
float delta, deltaSum;
int axis;
float horizonLevelStrength = 1;
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.
delta *= (1.0f / dT);
// Dterm low pass
if (pidProfile->dterm_cut_hz) {
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
if (!pidProfile->gyro_soft_lpf) {
// add moving average here to reduce noise
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
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;
int axis;
int32_t delta;
int32_t delta, deltaSum;
static int32_t delta1[3], delta2[3];
int32_t PTerm, ITerm, DTerm;
static int32_t lastError[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.
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6;
// Dterm delta low pass
if (pidProfile->dterm_cut_hz) {
delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
if (!pidProfile->gyro_soft_lpf) {
// add moving average here to reduce noise
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
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[] = {
"LOW", "MEDIUM", "HIGH"
"OFF", "LOW", "MEDIUM", "HIGH"
};
static const char * const lookupTableGyroSampling[] = {
"8KHZ",
"1KHZ"
static const char * const lookupTableGyroLpf[] = {
"OFF",
"188HZ",
"98HZ",
"42HZ"
};
typedef struct lookupTableEntry_s {
@ -377,7 +379,7 @@ typedef enum {
TABLE_PID_CONTROLLER,
TABLE_SERIAL_RX,
TABLE_GYRO_FILTER,
TABLE_GYRO_SAMPLING,
TABLE_GYRO_LPF,
} lookupTableIndex_e;
static const lookupTableEntry_t lookupTables[] = {
@ -394,7 +396,7 @@ static const lookupTableEntry_t lookupTables[] = {
{ lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
{ lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) },
{ lookupTableGyroSampling, sizeof(lookupTableGyroSampling) / sizeof(char *) }
{ lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }
};
#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 } },
{ "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 } },
{ "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 } },