Attenuate VEL_XY when UAV is traveling at high speed

src/main/fc/settings.yaml

@@ -1203,6 +1203,18 @@ groups:
         field: navVelXyDTermLpfHz
         min: 0
         max: 100
+      - name: nav_mc_vel_xy_dterm_dyn_scale
+        field: navVelXyDtermDynScale
+        min: 0
+        max: 1
+      - name: nav_mc_vel_xy_dterm_dyn_scale_min_at
+        field: navVelXyDtermDynScaleMinAt
+        min: 0
+        max: 1
+      - name: nav_mc_vel_xy_dterm_dyn_scale_max_at
+        field: navVelXyDtermDynScaleMaxAt
+        min: 0
+        max: 1
       - name: nav_fw_pos_z_p
         field: bank_fw.pid[PID_POS_Z].P
         condition: USE_NAV

src/main/flight/pid.c

@@ -246,6 +246,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 
         .loiter_direction = NAV_LOITER_RIGHT,
         .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ,
+        .navVelXyDtermDynScale = 0.2f,
+        .navVelXyDtermDynScaleMinAt = 0.6f,
+        .navVelXyDtermDynScaleMaxAt = 0.2f,
         .iterm_relax_type = ITERM_RELAX_SETPOINT,
         .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT,
         .iterm_relax = ITERM_RELAX_RP,

src/main/flight/pid.h

@@ -137,7 +137,9 @@ typedef struct pidProfile_s {
 
     uint8_t     loiter_direction;               // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
     float       navVelXyDTermLpfHz;
-
+    float       navVelXyDtermDynScale;          // VEL_XY dynamic Dterm scale: Dterm will be attenuated by this value when velocity is at navVelXyDtermMaxAt and 1.0 when velocity is at navVelXyDtermMinAt   
+    float       navVelXyDtermDynScaleMinAt;     // VEL_XY dynamic Dterm scale: Dterm will be scaled down fully to navVelXyDtermDynScale when velocity in setpoint and measurement is above this value. It is a factor of max. velocity processed by the controller
+    float       navVelXyDtermDynScaleMaxAt;     // VEL_XY dynamic Dterm scale: Dterm will be not scaled when velocity in setpoint and measurement is below this value. It is a factor of max. velocity processed by the controller
     uint8_t iterm_relax_type;               // Specifies type of relax algorithm
     uint8_t iterm_relax_cutoff;             // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
     uint8_t iterm_relax;                    // Enable iterm suppression during stick input

src/main/navigation/navigation.c

@@ -1711,8 +1711,17 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
 // Implementation of PID with back-calculation I-term anti-windup
 // Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
 // http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
-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)
-{
+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,
+    const float dTermScaler
+) {
     float newProportional, newDerivative, newFeedForward;
     float error = setpoint - measurement;
 
@@ -1736,11 +1745,13 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
     }
 
     if (pid->dTermLpfHz > 0) {
-        newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt) * gainScaler;
+        newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt);
     } else {
         newDerivative = pid->param.kD * newDerivative;
     }
 
+    newDerivative = newDerivative * gainScaler * dTermScaler;
+
     if (pidFlags & PID_ZERO_INTEGRATOR) {
         pid->integrator = 0.0f;
     }
@@ -1780,7 +1791,7 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
 
 float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags)
 {
-    return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f);
+    return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f, 1.0f);
 }
 
 void navPidReset(pidController_t *pid)

src/main/navigation/navigation_fixedwing.c

@@ -134,7 +134,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
 
     // PID controller to translate energy balance error [J] into pitch angle [decideg]
-    float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv);
+    float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv, 1.0f);
     targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, NAV_FW_PITCH_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
 
     // Reconstrain pitch angle ( >0 - climb, <0 - dive)

src/main/navigation/navigation_multicopter.c

@@ -437,7 +437,7 @@ static float getVelocityExpoAttenuationFactor(float velTotal, float velMax)
     return 1.0f - posControl.posResponseExpo * (1.0f - (velScale * velScale));  // x^3 expo factor
 }
 
-static void updatePositionVelocityController_MC(void)
+static void updatePositionVelocityController_MC(const float maxSpeed)
 {
     const float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
     const float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
@@ -446,9 +446,6 @@ static void updatePositionVelocityController_MC(void)
     float newVelX = posErrorX * posControl.pids.pos[X].param.kP;
     float newVelY = posErrorY * posControl.pids.pos[Y].param.kP;
 
-    // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s
-    const float maxSpeed = getActiveWaypointSpeed();
-
     // Scale velocity to respect max_speed
     float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY));
     if (newVelTotal > maxSpeed) {
@@ -472,12 +469,20 @@ static void updatePositionVelocityController_MC(void)
 #endif
 }
 
-static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit)
+static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed)
 {
 
+    const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x;
+    const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y;
+    const float measurementXY = sqrtf(powf(measurementX, 2)+powf(measurementY, 2));
+
+    const float setpointX = posControl.desiredState.vel.x;
+    const float setpointY = posControl.desiredState.vel.y;
+    const float setpointXY = sqrtf(powf(setpointX, 2)+powf(setpointY, 2));
+
     // Calculate velocity error
-    const float velErrorX = posControl.desiredState.vel.x - navGetCurrentActualPositionAndVelocity()->vel.x;
-    const float velErrorY = posControl.desiredState.vel.y - navGetCurrentActualPositionAndVelocity()->vel.y;
+    const float velErrorX = setpointX - measurementX;
+    const float velErrorY = setpointY - measurementY;
 
     // Calculate XY-acceleration limit according to velocity error limit
     float accelLimitX, accelLimitY;
@@ -508,11 +513,55 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
 
     // TODO: Verify if we need jerk limiting after all
 
+    /*
+     * This PID controller has dynamic dTerm scale. It's less active when controller
+     * is tracking setpoint at high speed. Full dTerm is required only for position hold,
+     * acceleration and deceleration
+     * Scale down dTerm with 2D speed
+     */
+    
+    //Normalize the setpoint and measurement between 0.0f and 1.0f where 1.0f is currently used max speed
+    const float setpointNormalized = constrainf(scaleRangef(fabsf(setpointXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f);
+    const float measurementNormalized = constrainf(scaleRangef(fabsf(measurementXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f);
+
+    /* 
+     * Map normalized speed of setpoint and measurement between 1.0f and nav_mc_vel_xy_dterm_dyn_scale
+     * 1.0f means that Dterm is not attenuated
+     * navVelXyDtermDynScale means full allowed attenuation
+     * 
+     * Dterm can be attenuated when both setpoint and measurement is high - UAV is moving close to desired velocity
+     */
+    const float setpointScale = constrainf(scaleRangef(setpointNormalized, pidProfile()->navVelXyDtermDynScaleMinAt, pidProfile()->navVelXyDtermDynScaleMaxAt, 1.0f, pidProfile()->navVelXyDtermDynScale), pidProfile()->navVelXyDtermDynScale, 1.0f);
+    const float measurementScale = constrainf(scaleRangef(measurementNormalized, pidProfile()->navVelXyDtermDynScaleMinAt, pidProfile()->navVelXyDtermDynScaleMaxAt, 1.0f, pidProfile()->navVelXyDtermDynScale), pidProfile()->navVelXyDtermDynScale, 1.0f);
+
+    //Use the higher scaling factor from the two above
+    const float dtermScale = MAX(setpointScale, measurementScale);
+
     // Apply PID with output limiting and I-term anti-windup
     // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit
     // Thus we don't need to do anything else with calculated acceleration
-    float newAccelX = navPidApply2(&posControl.pids.vel[X], posControl.desiredState.vel.x, navGetCurrentActualPositionAndVelocity()->vel.x, US2S(deltaMicros), accelLimitXMin, accelLimitXMax, 0);
-    float newAccelY = navPidApply2(&posControl.pids.vel[Y], posControl.desiredState.vel.y, navGetCurrentActualPositionAndVelocity()->vel.y, US2S(deltaMicros), accelLimitYMin, accelLimitYMax, 0);
+    float newAccelX = navPidApply3(
+        &posControl.pids.vel[X], 
+        setpointX, 
+        measurementX, 
+        US2S(deltaMicros), 
+        accelLimitXMin, 
+        accelLimitXMax, 
+        0,      // Flags
+        1.0f,   // Total gain scale
+        dtermScale    // Additional dTerm scale
+    );
+    float newAccelY = navPidApply3(
+        &posControl.pids.vel[Y], 
+        setpointY, 
+        measurementY, 
+        US2S(deltaMicros), 
+        accelLimitYMin, 
+        accelLimitYMax, 
+        0,      // Flags
+        1.0f,   // Total gain scale
+        dtermScale    // Additional dTerm scale
+    );
 
     int32_t maxBankAngle = DEGREES_TO_DECIDEGREES(navConfig()->mc.max_bank_angle);
 
@@ -589,8 +638,10 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
             if (!bypassPositionController) {
                 // Update position controller
                 if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
-                    updatePositionVelocityController_MC();
-                    updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX);
+                    // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s
+                    const float maxSpeed = getActiveWaypointSpeed(); 
+                    updatePositionVelocityController_MC(maxSpeed);
+                    updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed);
                 }
                 else {
                     resetMulticopterPositionController();

src/main/navigation/navigation_private.h

@@ -373,7 +373,17 @@ extern navigationPosControl_t posControl;
 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);
+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,
+    const float dTermScaler
+);
 void navPidReset(pidController_t *pid);
 void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz);