diff --git a/src/main/build/debug.h b/src/main/build/debug.h index a9bbf6cb23..e385bf0bb1 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -72,7 +72,8 @@ typedef enum { DEBUG_DYNAMIC_FILTER_FREQUENCY, DEBUG_IRLOCK, DEBUG_CD, - DEBUG_KALMAN, + DEBUG_KALMAN_GAIN, + DEBUG_PID_MEASUREMENT, DEBUG_SPM_CELLS, // Smartport master FLVSS DEBUG_SPM_VS600, // Smartport master VS600 VTX DEBUG_SPM_VARIO, // Smartport master variometer diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 389b0b0218..f49b8774fc 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", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -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 9e672a4a6a..99b0a00b2b 100755 --- a/src/main/flight/kalman.c +++ b/src/main/flight/kalman.c @@ -29,30 +29,24 @@ 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) +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 gyroKalmanSetSetpoint(uint8_t axis, float rate) +void gyroKalmanInitialize(uint16_t q, uint16_t w, uint16_t sharpness) { - setPoint[axis] = rate; -} - -void gyroKalmanInitialize(void) -{ - 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) @@ -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 + DEBUG_SET(DEBUG_KALMAN_GAIN, 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..17b714b8ff 100755 --- a/src/main/flight/kalman.h +++ b/src/main/flight/kalman.h @@ -49,6 +49,5 @@ typedef struct kalman uint16_t w; } kalman_t; -void gyroKalmanInitialize(void); -float gyroKalmanUpdate(uint8_t axis, float input); -void gyroKalmanSetSetpoint(uint8_t axis, float rate); +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 88dedf1814..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) @@ -974,9 +978,13 @@ 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 (pidProfile()->kalmanEnabled) { + 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 @@ -1108,6 +1116,11 @@ void pidInit(void) } pidResetTPAFilter(); +#ifdef USE_GYRO_KALMAN + if (pidProfile()->kalmanEnabled) { + gyroKalmanInitialize(pidProfile()->kalman_q, pidProfile()->kalman_w, pidProfile()->kalman_sharpness); + } +#endif } const pidBank_t * pidBank(void) { 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 e11b2f5d00..d9cd15891d 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" @@ -102,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 @@ -121,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) @@ -312,11 +307,6 @@ bool gyroInit(void) } gyroInitFilters(); -#ifdef USE_GYRO_KALMAN - if (gyroConfig()->kalmanEnabled) { - gyroKalmanInitialize(); - } -#endif #ifdef USE_DYNAMIC_FILTERS dynamicGyroNotchFiltersInit(&dynamicGyroNotchState); gyroDataAnalyseStateInit( @@ -460,14 +450,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); 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);