From 70054d59b0b1b066e8ee4735677a8971d501b6f8 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 8 Jul 2020 22:19:28 +0200 Subject: [PATCH 1/3] Move gyro EKF from gyro to PID --- src/main/flight/kalman.c | 10 ++-------- src/main/flight/kalman.h | 3 +-- src/main/flight/pid.c | 11 ++++++++++- src/main/sensors/gyro.c | 14 -------------- 4 files changed, 13 insertions(+), 25 deletions(-) diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c index 9e672a4a6a..2e88777d73 100755 --- a/src/main/flight/kalman.c +++ b/src/main/flight/kalman.c @@ -29,7 +29,6 @@ FILE_COMPILE_FOR_SPEED #include "build/debug.h" kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT]; -float setPoint[XYZ_AXIS_COUNT]; static void gyroKalmanInitAxis(kalman_t *filter) { @@ -43,11 +42,6 @@ static void gyroKalmanInitAxis(kalman_t *filter) filter->inverseN = 1.0f / (float)(filter->w); } -void gyroKalmanSetSetpoint(uint8_t axis, float rate) -{ - setPoint[axis] = rate; -} - void gyroKalmanInitialize(void) { gyroKalmanInitAxis(&kalmanFilterStateRate[X]); @@ -114,13 +108,13 @@ static void updateAxisVariance(kalman_t *kalmanState, float rate) kalmanState->r = squirt * VARIANCE_SCALE; } -float gyroKalmanUpdate(uint8_t axis, float input) +float gyroKalmanUpdate(uint8_t axis, float input, float setpoint) { updateAxisVariance(&kalmanFilterStateRate[axis], input); DEBUG_SET(DEBUG_KALMAN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain - return kalman_process(&kalmanFilterStateRate[axis], input, setPoint[axis]); + return kalman_process(&kalmanFilterStateRate[axis], input, setpoint); } #endif \ No newline at end of file diff --git a/src/main/flight/kalman.h b/src/main/flight/kalman.h index f493c1f628..0a27ca2514 100755 --- a/src/main/flight/kalman.h +++ b/src/main/flight/kalman.h @@ -50,5 +50,4 @@ typedef struct kalman } kalman_t; void gyroKalmanInitialize(void); -float gyroKalmanUpdate(uint8_t axis, float input); -void gyroKalmanSetSetpoint(uint8_t axis, float rate); +float gyroKalmanUpdate(uint8_t axis, float input, float setpoint); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a9e621285e..791163627a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -967,8 +967,11 @@ void FAST_CODE pidController(float dT) // Limit desired rate to something gyro can measure reliably pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + #ifdef USE_GYRO_KALMAN - gyroKalmanSetSetpoint(axis, pidState[axis].rateTarget); + if (gyroConfig()->kalmanEnabled) { + pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget); + } #endif } @@ -1105,6 +1108,12 @@ void pidInit(void) } else { pidControllerApplyFn = nullRateController; } + +#ifdef USE_GYRO_KALMAN + if (gyroConfig()->kalmanEnabled) { + gyroKalmanInitialize(); + } +#endif } const pidBank_t * pidBank(void) { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index e11b2f5d00..7db921eb73 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -72,7 +72,6 @@ FILE_COMPILE_FOR_SPEED #include "flight/gyroanalyse.h" #include "flight/rpm_filter.h" #include "flight/dynamic_gyro_notch.h" -#include "flight/kalman.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -312,11 +311,6 @@ bool gyroInit(void) } gyroInitFilters(); -#ifdef USE_GYRO_KALMAN - if (gyroConfig()->kalmanEnabled) { - gyroKalmanInitialize(); - } -#endif #ifdef USE_DYNAMIC_FILTERS dynamicGyroNotchFiltersInit(&dynamicGyroNotchState); gyroDataAnalyseStateInit( @@ -460,14 +454,6 @@ void FAST_CODE NOINLINE gyroUpdate() gyro.gyroADCf[axis] = gyroADCf; } -#ifdef USE_GYRO_KALMAN - if (gyroConfig()->kalmanEnabled) { - gyro.gyroADCf[X] = gyroKalmanUpdate(X, gyro.gyroADCf[X]); - gyro.gyroADCf[Y] = gyroKalmanUpdate(Y, gyro.gyroADCf[Y]); - gyro.gyroADCf[Z] = gyroKalmanUpdate(Z, gyro.gyroADCf[Z]); - } -#endif - #ifdef USE_DYNAMIC_FILTERS if (dynamicGyroNotchState.enabled) { gyroDataAnalyse(&gyroAnalyseState); From eebb5f644fd2591aa544c50ff05b64da1de08248 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 8 Jul 2020 22:27:17 +0200 Subject: [PATCH 2/3] Allow separate debug of PID measurement --- src/main/build/debug.h | 3 ++- src/main/fc/settings.yaml | 2 +- src/main/flight/kalman.c | 2 +- src/main/flight/pid.c | 1 + 4 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/build/debug.h b/src/main/build/debug.h index a5a5e90ea6..6fd91b473d 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -72,6 +72,7 @@ typedef enum { DEBUG_DYNAMIC_FILTER_FREQUENCY, DEBUG_IRLOCK, DEBUG_CD, - DEBUG_KALMAN, + DEBUG_KALMAN_GAIN, + DEBUG_PID_MEASUREMENT, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ce545ab777..f01b915ccc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -84,7 +84,7 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c index 2e88777d73..70855b46b8 100755 --- a/src/main/flight/kalman.c +++ b/src/main/flight/kalman.c @@ -112,7 +112,7 @@ float gyroKalmanUpdate(uint8_t axis, float input, float setpoint) { updateAxisVariance(&kalmanFilterStateRate[axis], input); - DEBUG_SET(DEBUG_KALMAN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain + DEBUG_SET(DEBUG_KALMAN_GAIN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain return kalman_process(&kalmanFilterStateRate[axis], input, setpoint); } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 791163627a..e04178dc19 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -973,6 +973,7 @@ void FAST_CODE pidController(float dT) pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget); } #endif + DEBUG_SET(DEBUG_PID_MEASUREMENT, axis, pidState[axis].gyroRate); } // Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK From e4752db5badc31cb8b7dfab464c138219a63b675 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 30 Jul 2020 11:06:26 +0200 Subject: [PATCH 3/3] Move Kalman setting to PID profile --- src/main/fc/settings.yaml | 46 +++++++++++++++++++++++---------------- src/main/flight/kalman.c | 16 +++++++------- src/main/flight/kalman.h | 2 +- src/main/flight/pid.c | 12 ++++++---- src/main/flight/pid.h | 4 ++++ src/main/sensors/gyro.c | 6 +---- src/main/sensors/gyro.h | 4 ---- 7 files changed, 49 insertions(+), 41 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 94b7cdb750..f49b8774fc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -229,25 +229,6 @@ groups: condition: USE_DYNAMIC_FILTERS min: 30 max: 1000 - - name: gyro_kalman_enabled - condition: USE_GYRO_KALMAN - field: kalmanEnabled - type: bool - - name: gyro_kalman_q - field: kalman_q - condition: USE_GYRO_KALMAN - min: 1 - max: 16000 - - name: gyro_kalman_w - field: kalman_w - condition: USE_GYRO_KALMAN - min: 1 - max: 40 - - name: gyro_kalman_sharpness - field: kalman_sharpness - condition: USE_GYRO_KALMAN - min: 1 - max: 16000 - name: gyro_to_use condition: USE_DUAL_GYRO min: 0 @@ -1802,6 +1783,33 @@ groups: field: controlDerivativeLpfHz min: 0 max: 200 + - name: setpoint_kalman_enabled + description: "Enable Kalman filter on the PID controller setpoint" + default_value: "OFF" + condition: USE_GYRO_KALMAN + field: kalmanEnabled + type: bool + - name: setpoint_kalman_q + description: "Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds" + default_value: "100" + field: kalman_q + condition: USE_GYRO_KALMAN + min: 1 + max: 16000 + - name: setpoint_kalman_w + description: "Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response" + default_value: "4" + field: kalman_w + condition: USE_GYRO_KALMAN + min: 1 + max: 40 + - name: setpoint_kalman_sharpness + description: "Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets" + default_value: "100" + field: kalman_sharpness + condition: USE_GYRO_KALMAN + min: 1 + max: 16000 - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c index 70855b46b8..99b0a00b2b 100755 --- a/src/main/flight/kalman.c +++ b/src/main/flight/kalman.c @@ -30,23 +30,23 @@ FILE_COMPILE_FOR_SPEED kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT]; -static void gyroKalmanInitAxis(kalman_t *filter) +static void gyroKalmanInitAxis(kalman_t *filter, uint16_t q, uint16_t w, uint16_t sharpness) { memset(filter, 0, sizeof(kalman_t)); - filter->q = gyroConfig()->kalman_q * 0.03f; //add multiplier to make tuning easier + filter->q = q * 0.03f; //add multiplier to make tuning easier filter->r = 88.0f; //seeding R at 88.0f filter->p = 30.0f; //seeding P at 30.0f filter->e = 1.0f; - filter->s = gyroConfig()->kalman_sharpness / 10.0f; - filter->w = gyroConfig()->kalman_w * 8; + filter->s = sharpness / 10.0f; + filter->w = w * 8; filter->inverseN = 1.0f / (float)(filter->w); } -void gyroKalmanInitialize(void) +void gyroKalmanInitialize(uint16_t q, uint16_t w, uint16_t sharpness) { - gyroKalmanInitAxis(&kalmanFilterStateRate[X]); - gyroKalmanInitAxis(&kalmanFilterStateRate[Y]); - gyroKalmanInitAxis(&kalmanFilterStateRate[Z]); + gyroKalmanInitAxis(&kalmanFilterStateRate[X], q, w, sharpness); + gyroKalmanInitAxis(&kalmanFilterStateRate[Y], q, w, sharpness); + gyroKalmanInitAxis(&kalmanFilterStateRate[Z], q, w, sharpness); } float kalman_process(kalman_t *kalmanState, float input, float target) diff --git a/src/main/flight/kalman.h b/src/main/flight/kalman.h index 0a27ca2514..17b714b8ff 100755 --- a/src/main/flight/kalman.h +++ b/src/main/flight/kalman.h @@ -49,5 +49,5 @@ typedef struct kalman uint16_t w; } kalman_t; -void gyroKalmanInitialize(void); +void gyroKalmanInitialize(uint16_t q, uint16_t w, uint16_t sharpness); float gyroKalmanUpdate(uint8_t axis, float input, float setpoint); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8c0486c6a5..a3a638b50b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -155,7 +155,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 13); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 14); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -276,6 +276,10 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .pidControllerType = PID_TYPE_AUTO, .navFwPosHdgPidsumLimit = PID_SUM_LIMIT_YAW_DEFAULT, .controlDerivativeLpfHz = 30, + .kalman_q = 100, + .kalman_w = 4, + .kalman_sharpness = 100, + .kalmanEnabled = 0, ); bool pidInitFilters(void) @@ -976,7 +980,7 @@ void FAST_CODE pidController(float dT) pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); #ifdef USE_GYRO_KALMAN - if (gyroConfig()->kalmanEnabled) { + if (pidProfile()->kalmanEnabled) { pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget); } #endif @@ -1113,8 +1117,8 @@ void pidInit(void) pidResetTPAFilter(); #ifdef USE_GYRO_KALMAN - if (gyroConfig()->kalmanEnabled) { - gyroKalmanInitialize(); + if (pidProfile()->kalmanEnabled) { + gyroKalmanInitialize(pidProfile()->kalman_q, pidProfile()->kalman_w, pidProfile()->kalman_sharpness); } #endif } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c686266150..d105f67ae2 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -151,6 +151,10 @@ typedef struct pidProfile_s { uint16_t navFwPosHdgPidsumLimit; uint8_t controlDerivativeLpfHz; + uint16_t kalman_q; + uint16_t kalman_w; + uint16_t kalman_sharpness; + uint8_t kalmanEnabled; } pidProfile_t; typedef struct pidAutotuneConfig_s { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 7db921eb73..d9cd15891d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -101,7 +101,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 10); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = GYRO_LPF_42HZ, // 42HZ value is defined for Invensense/TDK gyros @@ -120,10 +120,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .dynamicGyroNotchQ = 120, .dynamicGyroNotchMinHz = 150, .dynamicGyroNotchEnabled = 0, - .kalman_q = 100, - .kalman_w = 4, - .kalman_sharpness = 100, - .kalmanEnabled = 0, ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 39e04c8480..b709294b55 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -74,10 +74,6 @@ typedef struct gyroConfig_s { uint16_t dynamicGyroNotchQ; uint16_t dynamicGyroNotchMinHz; uint8_t dynamicGyroNotchEnabled; - uint16_t kalman_q; - uint16_t kalman_w; - uint16_t kalman_sharpness; - uint8_t kalmanEnabled; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig);