From d7d07c1c5c91766a29a8590d818b50fbc2d0d76c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 7 Apr 2016 23:52:27 +0100 Subject: [PATCH] Changed DTerm calculation to always use delta from measurement method. --- src/main/flight/pid.c | 17 ++++++++--------- src/main/flight/pid.h | 1 - src/main/flight/pid_luxfloat.c | 12 +++--------- src/main/flight/pid_mw23.c | 12 +++--------- src/main/flight/pid_mwrewrite.c | 12 +++--------- src/main/io/serial_cli.c | 8 -------- 6 files changed, 17 insertions(+), 45 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 31c5ce70c..eeee3b131 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -88,15 +88,15 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .P8[PIDALT] = 50, .I8[PIDALT] = 0, .D8[PIDALT] = 0, - .P8[PIDPOS] = 15, // POSHOLD_P * 100 - .I8[PIDPOS] = 0, // POSHOLD_I * 100 + .P8[PIDPOS] = 15, // POSHOLD_P * 100 + .I8[PIDPOS] = 0, // POSHOLD_I * 100 .D8[PIDPOS] = 0, - .P8[PIDPOSR] = 34, // POSHOLD_RATE_P * 10 - .I8[PIDPOSR] = 14, // POSHOLD_RATE_I * 100 - .D8[PIDPOSR] = 53, // POSHOLD_RATE_D * 1000 - .P8[PIDNAVR] = 25, // NAV_P * 10 - .I8[PIDNAVR] = 33, // NAV_I * 100 - .D8[PIDNAVR] = 83, // NAV_D * 1000 + .P8[PIDPOSR] = 34, // POSHOLD_RATE_P * 10 + .I8[PIDPOSR] = 14, // POSHOLD_RATE_I * 100 + .D8[PIDPOSR] = 53, // POSHOLD_RATE_D * 1000 + .P8[PIDNAVR] = 25, // NAV_P * 10 + .I8[PIDNAVR] = 33, // NAV_I * 100 + .D8[PIDNAVR] = 83, // NAV_D * 1000 .P8[PIDLEVEL] = 20, .I8[PIDLEVEL] = 10, .D8[PIDLEVEL] = 100, @@ -107,7 +107,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .yaw_p_limit = YAW_P_LIMIT_MAX, .dterm_cut_hz = 0, - .deltaMethod = 1, ); void pidResetITerm(void) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index b0991d8a1..f0943eb5e 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -58,7 +58,6 @@ typedef struct pidProfile_s { uint8_t I8[PID_ITEM_COUNT]; uint8_t D8[PID_ITEM_COUNT]; uint8_t pidController; - uint8_t deltaMethod; // Alternative delta calculation. Delta from gyro might give smoother results uint16_t yaw_p_limit; // set P term limit (fixed value was 300) uint16_t dterm_cut_hz; // dterm filtering } pidProfile_t; diff --git a/src/main/flight/pid_luxfloat.c b/src/main/flight/pid_luxfloat.c index fb47fb0c8..2788a6dd9 100644 --- a/src/main/flight/pid_luxfloat.c +++ b/src/main/flight/pid_luxfloat.c @@ -103,15 +103,9 @@ STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProf // optimisation for when D8 is zero, often used by YAW axis DTerm = 0; } else { - float delta; - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = rateError - lastErrorForDelta[axis]; - lastErrorForDelta[axis] = rateError; - } else { - // Delta from measurement - delta = -(gyroRate - lastErrorForDelta[axis]); - lastErrorForDelta[axis] = gyroRate; - } + // Delta from measurement + float delta = -(gyroRate - lastErrorForDelta[axis]); + lastErrorForDelta[axis] = gyroRate; // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // would be scaled by different dt each time. Division by dT fixes that. delta *= (1.0f / dT); diff --git a/src/main/flight/pid_mw23.c b/src/main/flight/pid_mw23.c index b1c4a21a2..d64b3f283 100644 --- a/src/main/flight/pid_mw23.c +++ b/src/main/flight/pid_mw23.c @@ -142,14 +142,9 @@ void pidMultiWii23(const pidProfile_t *pidProfile, const controlRateConfig_t *co PTerm -= ((int32_t)gyroError * dynP8[axis]) >> 6; // 32 bits is needed for calculation //-----calculate D-term based on the configured approach (delta from measurement or deltafromError) - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = error - lastErrorForDelta[axis]; - lastErrorForDelta[axis] = error; - } else { /* Delta from measurement */ - delta = -(gyroError - lastErrorForDelta[axis]); - lastErrorForDelta[axis] = gyroError; - } - + // Delta from measurement + delta = -(gyroError - lastErrorForDelta[axis]); + lastErrorForDelta[axis] = gyroError; if (pidProfile->dterm_cut_hz) { // Dterm delta low pass DTerm = delta; @@ -160,7 +155,6 @@ void pidMultiWii23(const pidProfile_t *pidProfile, const controlRateConfig_t *co delta2[axis] = delta1[axis]; delta1[axis] = delta; } - DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5; // 32 bits is needed for calculation axisPID[axis] = PTerm + ITerm + DTerm; diff --git a/src/main/flight/pid_mwrewrite.c b/src/main/flight/pid_mwrewrite.c index a27e5bf6f..b3b543f19 100644 --- a/src/main/flight/pid_mwrewrite.c +++ b/src/main/flight/pid_mwrewrite.c @@ -103,15 +103,9 @@ STATIC_UNIT_TESTED int16_t pidMultiWiiRewriteCore(int axis, const pidProfile_t * // optimisation for when D8 is zero, often used by YAW axis DTerm = 0; } else { - int32_t delta; - if (pidProfile->deltaMethod == DELTA_FROM_ERROR) { - delta = rateError - lastErrorForDelta[axis]; - lastErrorForDelta[axis] = rateError; - } else { - // Delta from measurement - delta = -(gyroRate - lastErrorForDelta[axis]); - lastErrorForDelta[axis] = gyroRate; - } + // Delta from measurement + int32_t delta = -(gyroRate - lastErrorForDelta[axis]); + lastErrorForDelta[axis] = gyroRate; // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // would be scaled by different dt each time. Division by dT fixes that. delta = (delta * ((uint16_t)0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 009b6068a..f8b9b661e 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -390,10 +390,6 @@ static const char * const lookupTableGyroLpf[] = { "10HZ" }; -static const char * const lookupDeltaMethod[] = { - "ERROR", "MEASUREMENT" -}; - typedef struct lookupTableEntry_s { const char * const *values; const uint8_t valueCount; @@ -416,7 +412,6 @@ typedef enum { TABLE_SERIAL_RX, TABLE_GYRO_FILTER, TABLE_GYRO_LPF, - TABLE_DELTA_METHOD, } lookupTableIndex_e; static const lookupTableEntry_t lookupTables[] = { @@ -436,7 +431,6 @@ static const lookupTableEntry_t lookupTables[] = { { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, { lookupTableGyroFilter, sizeof(lookupTableGyroFilter) / sizeof(char *) }, { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) }, - { lookupDeltaMethod, sizeof(lookupDeltaMethod) / sizeof(char *) } }; #define VALUE_TYPE_OFFSET 0 @@ -671,8 +665,6 @@ const clivalue_t valueTable[] = { { "mag_declination", VAR_INT16 | PROFILE_VALUE, .config.minmax = { -18000, 18000 } , PG_COMPASS_CONFIGURATION, offsetof(compassConfig_t, mag_declination)}, #endif - { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DELTA_METHOD } , PG_PID_PROFILE, offsetof(pidProfile_t, deltaMethod)}, - { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_PID_CONTROLLER } , PG_PID_PROFILE, offsetof(pidProfile_t, pidController)}, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { PID_MIN, PID_MAX } , PG_PID_PROFILE, offsetof(pidProfile_t, P8[FD_PITCH])}, -- 2.11.4.GIT