Make VelXY Dterm LPF user configurable

src/main/fc/settings.yaml

@@ -1076,6 +1076,10 @@ groups:
         condition: USE_NAV
         min: 0
         max: 255
+      - name: nav_mc_vel_xy_dterm_lpf_hz
+        field: navVelXyDTermLpfHz
+        min: 0
+        max: 100
       - name: nav_fw_pos_z_p
         field: bank_fw.pid[PID_POS_Z].P
         condition: USE_NAV

src/main/flight/pid.c

@@ -109,7 +109,7 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_
 
 STATIC_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -204,6 +204,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingItermLimitOnStickPosition = 0.5f,
 
         .loiter_direction = NAV_LOITER_RIGHT,
+        .navVelXyDTermLpfHz = 10,
 );
 
 void pidInit(void)

src/main/flight/pid.h

@@ -108,6 +108,7 @@ typedef struct pidProfile_s {
     float       fixedWingItermLimitOnStickPosition;   //Do not allow Iterm to grow when stick position is above this point
 
     uint8_t     loiter_direction;               // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
+    float       navVelXyDTermLpfHz;
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {

src/main/navigation/navigation.c

@@ -1655,7 +1655,11 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
         pid->last_input = measurement;
     }
 
-    newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, NAV_DTERM_CUT_HZ, dt) * gainScaler;
+    if (pid->dTermLpfHz > 0) {
+        newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt) * gainScaler;
+    } else {
+        newDerivative = pid->param.kD * newDerivative;
+    }
 
     if (pidFlags & PID_ZERO_INTEGRATOR) {
         pid->integrator = 0.0f;
@@ -1714,7 +1718,7 @@ void navPidReset(pidController_t *pid)
     pid->output_constrained = 0.0f;
 }
 
-void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF)
+void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz)
 {
     pid->param.kP = _kP;
     pid->param.kI = _kI;
@@ -1730,7 +1734,7 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF
         pid->param.kI = 0.0;
         pid->param.kT = 0.0;
     }
-
+    pid->dTermLpfHz = _dTermLpfHz;
     navPidReset(pid);
 }
 
@@ -3038,7 +3042,8 @@ void navigationUsePIDs(void)
         navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
                                                (float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f,
-                                               (float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f
+                                               (float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f,
+                                               pidProfile()->navVelXyDTermLpfHz
         );
     }
 
@@ -3048,25 +3053,33 @@ void navigationUsePIDs(void)
     navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f,
                                         (float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f,
                                         (float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
-                                        0.0f);
+                                        0.0f,
+                                        NAV_DTERM_CUT_HZ
+    );
 
     // Initialize surface tracking PID
     navPidInit(&posControl.pids.surface, 2.0f,
                                          0.0f,
                                          0.0f,
-                                         0.0f);
+                                         0.0f,
+                                         NAV_DTERM_CUT_HZ
+    );
 
     /** Airplane PIDs */
     // Initialize fixed wing PID controllers
     navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
-                                        0.0f);
+                                        0.0f,
+                                        NAV_DTERM_CUT_HZ
+    );
 
     navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
-                                        0.0f);
+                                        0.0f,
+                                        NAV_DTERM_CUT_HZ
+    );
 }
 
 void navigationInit(void)

src/main/navigation/navigation.h

@@ -253,6 +253,7 @@ typedef struct {
     bool reset;
     pidControllerParam_t param;
     pt1Filter_t dterm_filter_state;     // last derivative for low-pass filter
+    float dTermLpfHz;                   // dTerm low pass filter cutoff frequency
     float integrator;                   // integrator value
     float last_input;                   // last input for derivative
 

src/main/navigation/navigation_private.h

@@ -33,7 +33,7 @@
 #define NAV_FW_CONTROL_MONITORING_RATE      2
 #define NAV_FW_PITCH_CUTOFF_FREQENCY_HZ     2       // low-pass filter on Z (pitch angle) for fixed wing
 #define NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ     10      // low-pass filter on roll correction for fixed wing
-#define NAV_DTERM_CUT_HZ                    10
+#define NAV_DTERM_CUT_HZ                    10.0f
 #define NAV_ACCELERATION_XY_MAX             980.0f  // cm/s/s       // approx 45 deg lean angle
 
 #define INAV_SURFACE_MAX_DISTANCE           40
@@ -357,7 +357,7 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);
 float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags);
 float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler);
 void navPidReset(pidController_t *pid);
-void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF);
+void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz);
 void navPInit(pController_t *p, float _kP);
 
 bool isThrustFacingDownwards(void);