Add setpoint derivative feed forward term to PID controller

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, pidProfile()->dterm_soft_notch_cutoff); //pidProfile()->dterm_notch_cutoff
+        sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
+        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);
-                pidProfileMutable()->dterm_soft_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
+                sbufReadU16(src);
+                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