Add setpoint derivative feed forward term to PID controller (#5642)

* Add setpoint derivative feed forward term to PID controller

* Drop Dterm setpoint weight, FIR filter and dterm notch

* Fix FALCORE build

* Rename Multirotor FeedForward to Control Derivative

* Apply Control Derivative results

* Free CCM on F3

* Rename CLI name

* Remove Omnibus F3 support

* Update CLI.md and rename setting

docs/Board - OMNIBUS.md

@@ -1,5 +1,7 @@
 # Board - OMNIBUS F3
 
+> This board is not supported in recent INAV releases
+
 ## Hardware Features
 
 Refer to the product web page:

docs/Cli.md

@@ -399,12 +399,15 @@ A shorter form is also supported to enable and disable functions using `serial <
 | mc_p_pitch | 40 | Multicopter rate stabilisation P-gain for PITCH               |
 | mc_i_pitch | 30 | Multicopter rate stabilisation I-gain for PITCH               |
 | mc_d_pitch | 23 | Multicopter rate stabilisation D-gain for PITCH               |
+| mc_cd_pitch | 60 | Multicopter Control Derivative gain for PITCH                |
 | mc_p_roll  | 40 | Multicopter rate stabilisation P-gain for ROLL                |
 | mc_i_roll  | 30 | Multicopter rate stabilisation I-gain for ROLL                |
 | mc_d_roll  | 23 | Multicopter rate stabilisation D-gain for ROLL                |
+| mc_cd_roll | 60 | Multicopter Control Derivative gain for ROLL                |
 | mc_p_yaw   | 85 | Multicopter rate stabilisation P-gain for YAW                 |
 | mc_i_yaw   | 45 | Multicopter rate stabilisation I-gain for YAW                 |
 | mc_d_yaw   | 0  | Multicopter rate stabilisation D-gain for YAW                 |
+| mc_cd_yaw | 60 | Multicopter Control Derivative gain for YAW                  |
 | mc_p_level | 20 | Multicopter attitude stabilisation P-gain                     |
 | mc_i_level | 15 | Multicopter attitude stabilisation low-pass filter cutoff     |
 | mc_d_level | 75 | Multicopter attitude stabilisation HORIZON transition point   |
@@ -505,7 +508,6 @@ A shorter form is also supported to enable and disable functions using `serial <
 | baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default  should allow the noisiest baro to complete calibration [cm]. |
 | mc_airmode_type | STICK_CENTER | Defines the Airmode state handling type for Multirotors. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `mc_airmode_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER** mode. |
 | mc_airmode_threshold | 1300 | Defines airmode THROTTLE activation threshold when `mc_airmode_type` **THROTTLE_THRESHOLD** is used |
-| use_dterm_fir_filter | ON | Setting to **OFF** disabled extra filter on Dterm. **OFF** offers faster Dterm and better inflight performance with a cost of being more sensitive to gyro noise. Small and relatively clean multirotors (7 inches and below) are suggested to use **OFF** setting. If motors are getting too hot, switch back to **ON** |
 | sim_ground_station_number | Empty string | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. |
 | sim_pin | Empty string | PIN code for the SIM module |
 | sim_transmit_interval | 60 | Text message transmission interval in seconds for SIM module. Minimum value: 10 |
@@ -521,3 +523,4 @@ A shorter form is also supported to enable and disable functions using `serial <
 | antigravity_accelerator | 1   | |
 | antigravity_cutoff_lpf_hz | 15    | Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain |
 | sim_pin   |   | PIN for GSM card module |
+| mc_cd_lpf_hz | 30 | Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky |

make/release.mk

@@ -10,7 +10,7 @@ RELEASE_TARGETS += KAKUTEF4 KAKUTEF4V2 KAKUTEF7 KAKUTEF7MINI KAKUTEF7HDV
 
 RELEASE_TARGETS += SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO SPRACINGF7DUAL
 
-RELEASE_TARGETS += OMNIBUS AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2 AIRBOTF7 OMNIBUSF7NANOV7
+RELEASE_TARGETS += AIRBOTF4 ASGARD32F4 ASGARD32F7 FIREWORKSV2 AIRBOTF7 OMNIBUSF7NANOV7
 RELEASE_TARGETS += OMNIBUSF4 OMNIBUSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 DYSF4PRO DYSF4PROV2
 RELEASE_TARGETS += OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS
 RELEASE_TARGETS += OMNIBUSF7 OMNIBUSF7V2 OMNIBUSF7NXT YUPIF7

make/targets.mk

@@ -66,7 +66,7 @@ endif
 
 GROUP_1_TARGETS := AIRHEROF3 AIRHEROF3_QUAD LUX_RACE SPARKY REVO SPARKY2 COLIBRI FALCORE FF_F35_LIGHTNING FF_FORTINIF4 FF_PIKOF4 FF_PIKOF4OSD
 GROUP_2_TARGETS := SPRACINGF3 SPRACINGF3EVO SPRACINGF3EVO_1SS SPRACINGF3MINI SPRACINGF4EVO CLRACINGF4AIR CLRACINGF4AIRV2 BEEROTORF4 BETAFLIGHTF3 BETAFLIGHTF4 PIKOBLX
-GROUP_3_TARGETS := OMNIBUS AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO FIREWORKSV2 SPARKY2 MATEKF405 OMNIBUSF7 DYSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 OMNIBUSF7NXT OMNIBUSF7V2 ASGARD32F4
+GROUP_3_TARGETS := AIRBOTF4 BLUEJAYF4 OMNIBUSF4 OMNIBUSF4PRO FIREWORKSV2 SPARKY2 MATEKF405 OMNIBUSF7 DYSF4PRO OMNIBUSF4PRO_LEDSTRIPM5 OMNIBUSF7NXT OMNIBUSF7V2 ASGARD32F4
 GROUP_4_TARGETS := ANYFC ANYFCF7 ANYFCF7_EXTERNAL_BARO ALIENFLIGHTNGF7 PIXRACER YUPIF7 MATEKF405SE MATEKF411 MATEKF722 MATEKF405OSD MATEKF405_SERVOS6 NOX
 GROUP_5_TARGETS := ASGARD32F7 CLRACINGF4AIRV3 DALRCF405 DALRCF722DUAL DYSF4PROV2 F4BY FISHDRONEF4 FOXEERF405 FOXEERF722DUAL FRSKYF3 FRSKYF4 FURYF3 FURYF3_SPIFLASH FURYF4OSD
 GROUP_6_TARGETS := MAMBAF405 OMNIBUSF4V3 OMNIBUSF4V3_S6_SS OMNIBUSF4V3_S5S6_SS OMNIBUSF4V3_S5_S6_2SS AIKONF4

src/main/blackbox/blackbox.c

@@ -1717,8 +1717,6 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_type", "%d",                  pidProfile()->dterm_lpf_type);
         BLACKBOX_PRINT_HEADER_LINE("dterm_lpf2_hz", "%d",                   pidProfile()->dterm_lpf2_hz);
         BLACKBOX_PRINT_HEADER_LINE("dterm_lpf2_type", "%d",                 pidProfile()->dterm_lpf2_type);
-        BLACKBOX_PRINT_HEADER_LINE("dterm_notch_hz", "%d",                  pidProfile()->dterm_soft_notch_hz);
-        BLACKBOX_PRINT_HEADER_LINE("dterm_notch_cutoff", "%d",              pidProfile()->dterm_soft_notch_cutoff);
         BLACKBOX_PRINT_HEADER_LINE("deadband", "%d",                        rcControlsConfig()->deadband);
         BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d",                    rcControlsConfig()->yaw_deadband);
         BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d",                        gyroConfig()->gyro_lpf);
@@ -1747,7 +1745,6 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE("acc_notch_hz", "%d",                    accelerometerConfig()->acc_notch_hz);
         BLACKBOX_PRINT_HEADER_LINE("acc_notch_cutoff", "%d",                accelerometerConfig()->acc_notch_cutoff);
         BLACKBOX_PRINT_HEADER_LINE("gyro_stage2_lowpass_hz", "%d",          gyroConfig()->gyro_stage2_lowpass_hz);
-        BLACKBOX_PRINT_HEADER_LINE("dterm_setpoint_weight", "%f",           (double)pidProfile()->dterm_setpoint_weight);
         BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d",                     pidProfile()->pidSumLimit);
         BLACKBOX_PRINT_HEADER_LINE("pidSumLimitYaw", "%d",                  pidProfile()->pidSumLimitYaw);
         BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitYaw", "%d",        pidProfile()->axisAccelerationLimitYaw);

src/main/build/debug.h

@@ -71,5 +71,6 @@ typedef enum {
     DEBUG_DYNAMIC_FILTER,
     DEBUG_DYNAMIC_FILTER_FREQUENCY,
     DEBUG_IRLOCK,
+    DEBUG_CD,
     DEBUG_COUNT
 } debugType_e;

src/main/common/filter.c

@@ -231,40 +231,4 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
     filter->x2 = x2;
     filter->y1 = y1;
     filter->y2 = y2;
-}
+}
-
-/*
- * FIR filter
- */
-void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
-{
-    filter->buf = buf;
-    filter->bufLength = bufLength;
-    filter->coeffs = coeffs;
-    filter->coeffsLength = coeffsLength;
-    memset(filter->buf, 0, sizeof(float) * filter->bufLength);
-}
-
-/*
- * FIR filter initialisation
- * If FIR filter is just used for averaging, coeffs can be set to NULL
- */
-void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs)
-{
-    firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
-}
-
-void firFilterUpdate(firFilter_t *filter, float input)
-{
-    memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(float));
-    filter->buf[0] = input;
-}
-
-float firFilterApply(const firFilter_t *filter)
-{
-    float ret = 0.0f;
-    for (int ii = 0; ii < filter->coeffsLength; ++ii) {
-        ret += filter->coeffs[ii] * filter->buf[ii];
-    }
-    return ret;
-}

src/main/common/filter.h

@@ -81,8 +81,3 @@ float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
 float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
-
-void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
-void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
-void firFilterUpdate(firFilter_t *filter, float input);
-float firFilterApply(const firFilter_t *filter);

src/main/fc/config.c

@@ -170,9 +170,6 @@ void validateAndFixConfig(void)
     if (gyroConfig()->gyro_notch_cutoff >= gyroConfig()->gyro_notch_hz) {
         gyroConfigMutable()->gyro_notch_hz = 0;
     }
-    if (pidProfile()->dterm_soft_notch_cutoff >= pidProfile()->dterm_soft_notch_hz) {
-        pidProfileMutable()->dterm_soft_notch_hz = 0;
-    }
     if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
         accelerometerConfigMutable()->acc_notch_hz = 0;
     }

src/main/fc/fc_msp.c

@@ -1157,8 +1157,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
         sbufWriteU16(dst, gyroConfig()->gyro_notch_hz);
         sbufWriteU16(dst, gyroConfig()->gyro_notch_cutoff);
-        sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz); //BF: pidProfile()->dterm_notch_hz
+        sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
-        sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff); //pidProfile()->dterm_notch_cutoff
+        sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff
 
         sbufWriteU16(dst, 0); //BF: masterConfig.gyro_soft_notch_hz_2
         sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_2
@@ -1176,7 +1176,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
         sbufWriteU8(dst, 0); //BF: pidProfile()->deltaMethod
         sbufWriteU8(dst, 0); //BF: pidProfile()->vbatPidCompensation
         sbufWriteU8(dst, 0); //BF: pidProfile()->setpointRelaxRatio
-        sbufWriteU8(dst, constrain(pidProfile()->dterm_setpoint_weight * 100, 0, 255));
+        sbufWriteU8(dst, 0);
         sbufWriteU16(dst, pidProfile()->pidSumLimit);
         sbufWriteU8(dst, 0); //BF: pidProfile()->itermThrottleGain
 
@@ -2044,8 +2044,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
                 return MSP_RESULT_ERROR;
             }
             if (dataSize >= 13) {
-                pidProfileMutable()->dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500);
+                sbufReadU16(src);
-                pidProfileMutable()->dterm_soft_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
+                sbufReadU16(src);
                 pidInitFilters();
             } else {
                 return MSP_RESULT_ERROR;
@@ -2082,7 +2082,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             sbufReadU8(src); //BF: pidProfileMutable()->deltaMethod
             sbufReadU8(src); //BF: pidProfileMutable()->vbatPidCompensation
             sbufReadU8(src); //BF: pidProfileMutable()->setpointRelaxRatio
-            pidProfileMutable()->dterm_setpoint_weight = constrainf(sbufReadU8(src) / 100.0f, 0.0f, 2.0f);
+            sbufReadU8(src);
             pidProfileMutable()->pidSumLimit = sbufReadU16(src);
             sbufReadU8(src); //BF: pidProfileMutable()->itermThrottleGain
 

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"]
+      "IRLOCK", "CD"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -969,6 +969,10 @@ groups:
         field: bank_mc.pid[PID_PITCH].D
         min: 0
         max: 200
+      - name: mc_cd_pitch
+        field: bank_mc.pid[PID_PITCH].FF
+        min: 0
+        max: 200
       - name: mc_p_roll
         field: bank_mc.pid[PID_ROLL].P
         min: 0
@@ -981,6 +985,10 @@ groups:
         field: bank_mc.pid[PID_ROLL].D
         min: 0
         max: 200
+      - name: mc_cd_roll
+        field: bank_mc.pid[PID_ROLL].FF
+        min: 0
+        max: 200
       - name: mc_p_yaw
         field: bank_mc.pid[PID_YAW].P
         min: 0
@@ -993,6 +1001,10 @@ groups:
         field: bank_mc.pid[PID_YAW].D
         min: 0
         max: 200
+      - name: mc_cd_yaw
+        field: bank_mc.pid[PID_YAW].FF
+        min: 0
+        max: 200
       - name: mc_p_level
         field: bank_mc.pid[PID_LEVEL].P
         min: 0
@@ -1073,15 +1085,9 @@ groups:
       - name: dterm_lpf2_type
         field: dterm_lpf2_type
         table: filter_type
-      - name: use_dterm_fir_filter
-        field: use_dterm_fir_filter
-        type: bool
       - name: yaw_lpf_hz
         min: 0
         max: 200
-      - name: dterm_setpoint_weight
-        min: 0
-        max: 2
       - name: fw_iterm_throw_limit
         field: fixedWingItermThrowLimit
         min: FW_ITERM_THROW_LIMIT_MIN
@@ -1102,14 +1108,6 @@ groups:
         field: fixedWingItermLimitOnStickPosition
         min: 0
         max: 1
-      - name: dterm_notch_hz
-        field: dterm_soft_notch_hz
-        min: 0
-        max: 500
-      - name: dterm_notch_cutoff
-        field: dterm_soft_notch_cutoff
-        min: 1
-        max: 500
       - name: pidsum_limit
         field: pidSumLimit
         min: PID_SUM_LIMIT_MIN
@@ -1283,6 +1281,10 @@ groups:
       - name: pid_type
         field: pidControllerType
         table: pidTypeTable
+      - name: mc_cd_lpf_hz
+        field: controlDerivativeLpfHz
+        min: 0
+        max: 200
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t

src/main/flight/pid.c

@@ -63,6 +63,7 @@ typedef struct {
     float kI;   // Integral gain
     float kD;   // Derivative gain
     float kFF;  // Feed-forward gain
+    float kCD;  // Control Derivative
     float kT;   // Back-calculation tracking gain
 
     float gyroRate;
@@ -85,22 +86,23 @@ typedef struct {
     pt1Filter_t ptermLpfState;
     filter_t dtermLpfState;
     filter_t dtermLpf2State;
-    // Dterm notch filtering
+
-    biquadFilter_t deltaNotchFilter;
     float stickPosition;
 
-#ifdef USE_D_BOOST
     float previousRateTarget;
     float previousRateGyro;
+
+#ifdef USE_D_BOOST
     pt1Filter_t dBoostLpf;
     biquadFilter_t dBoostGyroLpf;
 #endif
     uint16_t pidSumLimit;
     filterApply4FnPtr ptermFilterApplyFn;
     bool itermLimitActive;
+
+    biquadFilter_t rateTargetFilter;
 } pidState_t;
 
-STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn;
 STATIC_FASTRAM bool pidFiltersConfigured = false;
 static EXTENDED_FASTRAM float headingHoldCosZLimit;
 static EXTENDED_FASTRAM int16_t headingHoldTarget;
@@ -115,8 +117,7 @@ FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT];
 int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_D[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_Setpoint[FLIGHT_DYNAMICS_INDEX_COUNT];
 #endif
 
-STATIC_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
+static EXTENDED_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
-
 static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
 static EXTENDED_FASTRAM uint8_t itermRelax;
 static EXTENDED_FASTRAM uint8_t itermRelaxType;
@@ -148,15 +149,16 @@ typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t
 static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 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, 12);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 13);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
             .pid = {
-                [PID_ROLL] =    { 40, 30, 23, 0 },
+                [PID_ROLL] =    { 40, 30, 23, 60 },
-                [PID_PITCH] =   { 40, 30, 23, 0 },
+                [PID_PITCH] =   { 40, 30, 23, 60 },
-                [PID_YAW] =     { 85, 45, 0, 0 },
+                [PID_YAW] =     { 85, 45, 0, 60 },
                 [PID_LEVEL] = {
                     .P = 20,    // Self-level strength
                     .I = 15,    // Self-leveing low-pass frequency (0 - disabled)
@@ -230,15 +232,11 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
             }
         },
 
-        .dterm_soft_notch_hz = 0,
-        .dterm_soft_notch_cutoff = 1,
         .dterm_lpf_type = 1, //Default to BIQUAD
         .dterm_lpf_hz = 40,
         .dterm_lpf2_type = 1, //Default to BIQUAD
         .dterm_lpf2_hz = 0,   // Off by default
         .yaw_lpf_hz = 0,
-        .dterm_setpoint_weight = 1.0f,
-        .use_dterm_fir_filter = 1,
 
         .itermWindupPointPercent = 50,       // Percent
 
@@ -270,6 +268,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF,
         .pidControllerType = PID_TYPE_AUTO,
         .navFwPosHdgPidsumLimit = PID_SUM_LIMIT_YAW_DEFAULT,
+        .controlDerivativeLpfHz = 30,
 );
 
 bool pidInitFilters(void)
@@ -280,38 +279,6 @@ bool pidInitFilters(void)
         return false;
     }
 
-    static float dtermCoeffs[PID_GYRO_RATE_BUF_LENGTH];
-
-    if (pidProfile()->use_dterm_fir_filter) {
-        // Calculate derivative using 5-point noise-robust differentiators without time delay (one-sided or forward filters)
-        // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
-        // h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8
-        dtermCoeffs[0] = 5.0f/8;
-        dtermCoeffs[1] = 2.0f/8;
-        dtermCoeffs[2] = -8.0f/8;
-        dtermCoeffs[3] = -2.0f/8;
-        dtermCoeffs[4] = 3.0f/8;
-    } else {
-        //simple d(t) - d(t-1) differentiator 
-        dtermCoeffs[0] = 1.0f;
-        dtermCoeffs[1] = -1.0f;
-        dtermCoeffs[2] = 0.0f;
-        dtermCoeffs[3] = 0.0f;
-        dtermCoeffs[4] = 0.0f;
-    }
-
-    for (int axis = 0; axis < 3; ++ axis) {
-        firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
-    }
-
-    notchFilterApplyFn = nullFilterApply;
-    if (pidProfile()->dterm_soft_notch_hz != 0) {
-        notchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
-        for (int axis = 0; axis < 3; ++ axis) {
-            biquadFilterInitNotch(&pidState[axis].deltaNotchFilter, refreshRate, pidProfile()->dterm_soft_notch_hz, pidProfile()->dterm_soft_notch_cutoff);
-        }
-    }
-
     // Init other filters
     if (pidProfile()->dterm_lpf_hz) {
         for (int axis = 0; axis < 3; ++ axis) {
@@ -348,6 +315,12 @@ bool pidInitFilters(void)
     }
 #endif
 
+    if (pidProfile()->controlDerivativeLpfHz) {
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            biquadFilterInitLPF(&pidState[axis].rateTargetFilter, pidProfile()->controlDerivativeLpfHz, getLooptime());
+        }
+    }
+
     pidFiltersConfigured = true;
 
     return true;
@@ -496,6 +469,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
             pidState[axis].kI  = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER  * tpaFactor;
             pidState[axis].kD  = 0.0f;
             pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor;
+            pidState[axis].kCD = 0.0f;
             pidState[axis].kT  = 0.0f;
         }
         else {
@@ -503,6 +477,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
             pidState[axis].kP  = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA;
             pidState[axis].kI  = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER;
             pidState[axis].kD  = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA;
+            pidState[axis].kCD = pidBank()->pid[axis].FF / FP_PID_RATE_D_FF_MULTIPLIER * axisTPA;
             pidState[axis].kFF = 0.0f;
 
             // Tracking anti-windup requires P/I/D to be all defined which is only true for MC
@@ -591,7 +566,7 @@ bool isFixedWingItermLimitActive(float stickPosition)
      * Iterm anti windup whould be active only when pilot controls the rotation
      * velocity directly, not when ANGLE or HORIZON are used
      */
-    if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
+    if (levelingEnabled) {
         return false;
     }
 
@@ -689,9 +664,6 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
         dBoost = scaleRangef(acceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostFactor);
         dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
         dBoost = constrainf(dBoost, 1.0f, dBoostFactor);
-
-        pidState->previousRateTarget = pidState->rateTarget;
-        pidState->previousRateGyro = pidState->gyroRate;
     } 
 
     return dBoost;
@@ -709,34 +681,38 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
     const float rateError = pidState->rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT);
 
+    const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget;
+    const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta);
+
+    /*
+     * Compute Control Derivative
+     * CD is enabled only when ANGLE and HORIZON modes are not enabled!
+     */
+    float newCDTerm;
+    if (levelingEnabled) {
+        newCDTerm = 0.0F;
+    } else {
+        newCDTerm = rateTargetDeltaFiltered * (pidState->kCD / dT);
+    }
+    DEBUG_SET(DEBUG_CD, axis, newCDTerm);
+
     // Calculate new D-term
     float newDTerm;
     if (pidState->kD == 0) {
         // optimisation for when D8 is zero, often used by YAW axis
         newDTerm = 0;
     } else {
-        // Calculate delta for Dterm calculation. Apply filters before derivative to minimize effects of dterm kick
+        float delta = pidState->previousRateGyro - pidState->gyroRate;
-        const float deltaFiltered = pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate;
 
-        firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
+        delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
-        newDTerm = firFilterApply(&pidState->gyroRateFilter);
+        delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
-
-        // Apply D-term notch
-        newDTerm = notchFilterApplyFn(&pidState->deltaNotchFilter, newDTerm);
-
-        // Apply additional lowpass
-        newDTerm = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, newDTerm);
-        newDTerm = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, newDTerm);
 
         // Calculate derivative
-        newDTerm =  newDTerm * (pidState->kD / dT) * applyDBoost(pidState, dT);
+        newDTerm =  delta * (pidState->kD / dT) * applyDBoost(pidState, dT);
-
-        // Additionally constrain D
-        newDTerm = constrainf(newDTerm, -300.0f, 300.0f);
     }
 
     // TODO: Get feedback from mixer on available correction range for each axis
-    const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf;
+    const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
     const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
 
     float itermErrorRate = rateError;
@@ -760,6 +736,9 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
     axisPID_D[axis] = newDTerm;
     axisPID_Setpoint[axis] = pidState->rateTarget;
 #endif
+
+    pidState->previousRateTarget = pidState->rateTarget;
+    pidState->previousRateGyro = pidState->gyroRate;
 }
 
 void updateHeadingHoldTarget(int16_t heading)
@@ -995,6 +974,9 @@ void FAST_CODE pidController(float dT)
         pidLevel(&pidState[FD_ROLL], FD_ROLL, horizonRateMagnitude, dT);
         pidLevel(&pidState[FD_PITCH], FD_PITCH, horizonRateMagnitude, dT);
         canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with ANGLE/HORIZON
+        levelingEnabled = true;
+    } else {
+        levelingEnabled = false;
     }
 
     if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {

src/main/flight/pid.h

@@ -45,6 +45,7 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
 #define FP_PID_RATE_P_MULTIPLIER    31.0f
 #define FP_PID_RATE_I_MULTIPLIER    4.0f
 #define FP_PID_RATE_D_MULTIPLIER    1905.0f
+#define FP_PID_RATE_D_FF_MULTIPLIER   7270.0f
 #define FP_PID_LEVEL_P_MULTIPLIER   6.56f       // Level P gain units is [1/sec] and angle error is [deg] => [deg/s]
 #define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f
 
@@ -104,17 +105,12 @@ typedef struct pidProfile_s {
     pidBank_t bank_fw;
     pidBank_t bank_mc;
 
-    uint16_t dterm_soft_notch_hz;           // Dterm Notch frequency
-    uint16_t dterm_soft_notch_cutoff;       // Dterm Notch Cutoff frequency
-    
     uint8_t dterm_lpf_type;                 // Dterm LPF type: PT1, BIQUAD
     uint16_t dterm_lpf_hz;                  
     
     uint8_t dterm_lpf2_type;                // Dterm LPF type: PT1, BIQUAD
     uint16_t dterm_lpf2_hz;                 
     
-    uint8_t use_dterm_fir_filter;           // Use classical INAV FIR differentiator. Very noise robust, can be quite slowish
-
     uint8_t yaw_lpf_hz;
 
     uint8_t heading_hold_rate_limit;        // Maximum rotation rate HEADING_HOLD mode can feed to yaw rate PID controller
@@ -126,7 +122,6 @@ typedef struct pidProfile_s {
 
     int16_t max_angle_inclination[ANGLE_INDEX_COUNT];       // Max possible inclination (roll and pitch axis separately
 
-    float dterm_setpoint_weight;
     uint16_t pidSumLimit;
     uint16_t pidSumLimitYaw;
 
@@ -151,6 +146,7 @@ typedef struct pidProfile_s {
     uint8_t antigravityCutoff;
 
     uint16_t navFwPosHdgPidsumLimit;
+    uint8_t controlDerivativeLpfHz;
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {

src/main/io/osd_dji_hd.c

@@ -568,8 +568,8 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
             sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);                // BF: currentPidProfile->yaw_lowpass_hz
             sbufWriteU16(dst, gyroConfig()->gyro_notch_hz);             // BF: gyroConfig()->gyro_soft_notch_hz_1
             sbufWriteU16(dst, gyroConfig()->gyro_notch_cutoff);         // BF: gyroConfig()->gyro_soft_notch_cutoff_1
-            sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz);       // BF: currentPidProfile->dterm_notch_hz
+            sbufWriteU16(dst, 0);                                       // BF: currentPidProfile->dterm_notch_hz
-            sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff);   // BF: currentPidProfile->dterm_notch_cutoff
+            sbufWriteU16(dst, 1);                                       // BF: currentPidProfile->dterm_notch_cutoff
             sbufWriteU16(dst, 0);                                       // BF: gyroConfig()->gyro_soft_notch_hz_2
             sbufWriteU16(dst, 1);                                       // BF: gyroConfig()->gyro_soft_notch_cutoff_2
             sbufWriteU8(dst, 0);                                        // BF: currentPidProfile->dterm_filter_type

src/main/target/FALCORE/config.c

@@ -167,9 +167,6 @@ void targetConfiguration(void)
     pidProfileMutable()->max_angle_inclination[FD_PITCH] = 300;
     pidProfileMutable()->dterm_lpf_hz = 70;
     pidProfileMutable()->yaw_lpf_hz = 35;
-    pidProfileMutable()->dterm_setpoint_weight = 0;
-    pidProfileMutable()->dterm_soft_notch_hz = 0;
-    pidProfileMutable()->dterm_soft_notch_cutoff = 1;
     pidProfileMutable()->pidSumLimit = 500;
     pidProfileMutable()->axisAccelerationLimitRollPitch = 0;
     pidProfileMutable()->axisAccelerationLimitYaw = 10000;