diff --git a/src/main/config/config.c b/src/main/config/config.c index 5e551db867..9137edfe05 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -177,7 +177,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D8[PIDVEL] = 1; pidProfile->dterm_lpf_hz = 0; // filtering ON by default - pidProfile->deltaFromGyro = 1; + pidProfile->deltaMethod = DELTA_FROM_MEASUREMENT; pidProfile->airModeInsaneAcrobilityFactor = 0; pidProfile->P_f[ROLL] = 1.1f; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 6ce8ea824f..e4bf8991b1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -128,7 +128,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa { float RateError, AngleRate, gyroRate; float ITerm,PTerm,DTerm; - static float lastError[3], lastGyroRate[3]; + static float lastErrorForDelta[3]; static float previousDelta[3][DELTA_TOTAL_SAMPLES]; float delta, deltaSum; int axis, deltaCount; @@ -215,12 +215,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa ITerm = errorGyroIf[axis]; //-----calculate D-term - if (!pidProfile->deltaFromGyro) { - delta = RateError - lastError[axis]; - lastError[axis] = RateError; + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateError - lastErrorForDelta[axis]; + lastErrorForDelta[axis] = RateError; } else { - delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastGyroRate[axis] = gyroRate; + delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastErrorForDelta[axis] = gyroRate; } // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference @@ -269,9 +269,8 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat int axis, deltaCount; int32_t PTerm, ITerm, DTerm, delta, deltaSum; - static int32_t lastError[3] = { 0, 0, 0 }; + static int32_t lastErrorForDelta[3] = { 0, 0, 0 }; static int32_t previousDelta[3][DELTA_TOTAL_SAMPLES]; - static int32_t lastGyroRate[3]; int32_t AngleRateTmp, RateError, gyroRate; int8_t horizonLevelStrength = 100; @@ -360,12 +359,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat ITerm = errorGyroI[axis] >> 13; //-----calculate D-term - if (!pidProfile->deltaFromGyro) { // quick and dirty solution for testing - delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastError[axis] = RateError; + if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { + delta = RateError - lastErrorForDelta[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastErrorForDelta[axis] = RateError; } else { - delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 - lastGyroRate[axis] = gyroRate; + delta = -(gyroRate - lastErrorForDelta[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 + lastErrorForDelta[axis] = gyroRate; } // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 42b4188922..308c894df4 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -42,6 +42,11 @@ typedef enum { PID_COUNT } pidControllerType_e; +typedef enum { + DELTA_FROM_ERROR = 0, + DELTA_FROM_MEASUREMENT +} pidDeltaType_e; + #define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2) typedef struct pidProfile_s { @@ -60,7 +65,7 @@ typedef struct pidProfile_s { uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor float dterm_lpf_hz; // Delta Filter in hz - uint8_t deltaFromGyro; // Alternative delta Calculation + uint8_t deltaMethod; // Alternative delta Calculation #ifdef GTUNE uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 5bea5bea40..0e92af5f86 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -400,6 +400,10 @@ static const char * const lookupTableMagHardware[] = { "AK8963" }; +static const char * const lookupDeltaMethod[] = { + "ERROR", "MEASUREMENT" +}; + typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -424,6 +428,7 @@ typedef enum { TABLE_ACC_HARDWARE, TABLE_BARO_HARDWARE, TABLE_MAG_HARDWARE, + TABLE_DELTA_METHOD, } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -442,7 +447,8 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }, { lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) }, { lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) }, - { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) } + { lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) }, + { lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) } }; #define VALUE_TYPE_OFFSET 0 @@ -664,7 +670,8 @@ const clivalue_t valueTable[] = { { "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } }, { "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } }, - { "delta_from_gyro", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaFromGyro, .config.lookup = { TABLE_OFF_ON } }, + { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, + { "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, @@ -704,7 +711,6 @@ const clivalue_t valueTable[] = { { "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } }, { "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } }, - { "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "acro_plus_factor", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 100 } }, #ifdef BLACKBOX