Refactor attenuation factor computation

src/main/fc/settings.yaml

@@ -1651,14 +1651,20 @@ groups:
         min: 0
         max: 100
       - name: nav_mc_vel_xy_dterm_attenuation
+        description: "Maximum D-term attenution percentage for horizontal velocity PID controller (Multirotor). It allows to smooth the PosHold CRUISE, WP and RTH when Multirotor is traveling at full speed. Dterm is not attenuated at low speeds, breaking and accelerating."
+        default_value: "90"
         field: navVelXyDtermAttenuation
         min: 0
         max: 100
       - name: nav_mc_vel_xy_dterm_attenuation_start
+        description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins"
+        default_value: 10"
         field: navVelXyDtermAttenuationStart
         min: 0
         max: 100
       - name: nav_mc_vel_xy_dterm_attenuation_end
+        description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum"
+        default_value: "60"  
         field: navVelXyDtermAttenuationEnd
         min: 0
         max: 100

src/main/flight/pid.c

@@ -258,8 +258,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 
         .loiter_direction = NAV_LOITER_RIGHT,
         .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ,
-        .navVelXyDtermAttenuation = 80,
-        .navVelXyDtermAttenuationStart = 20,
+        .navVelXyDtermAttenuation = 90,
+        .navVelXyDtermAttenuationStart = 10,
         .navVelXyDtermAttenuationEnd = 60,
         .iterm_relax_type = ITERM_RELAX_SETPOINT,
         .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT,

src/main/navigation/navigation_multicopter.c

@@ -469,6 +469,25 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
 #endif
 }
 
+static float computeNormalizedVelocity(const float value, const float maxValue)
+{
+    return constrainf(scaleRangef(fabsf(value), 0, maxValue, 0.0f, 1.0f), 0.0f, 1.0f);
+}
+
+static float computeVelocityScale(
+    const float value, 
+    const float maxValue, 
+    const float attenuationFactor,
+    const float attenuationStart,
+    const float attenuationEnd
+)
+{
+    const float normalized = computeNormalizedVelocity(value, maxValue);
+
+    float scale = scaleRangef(normalized, attenuationStart, attenuationEnd, 0, attenuationFactor);
+    return constrainf(scale, 0, attenuationFactor);
+}
+
 static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed)
 {    
     const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x;
@@ -517,29 +536,23 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
      * 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(posControl.actualState.velXY), 0, maxSpeed, 0.0f, 1.0f), 0.0f, 1.0f);
-
-    DEBUG_SET(DEBUG_ALWAYS, 0, setpointNormalized * 100);
-    DEBUG_SET(DEBUG_ALWAYS, 1, measurementNormalized * 100);
-
-    /* 
-     * Dterm can be attenuated when both setpoint and measurement is high - UAV is moving close to desired velocity
-     */
-    float setpointScale = scaleRangef(setpointNormalized, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd, 0, multicopterPosXyCoefficients.dTermAttenuation);
-    setpointScale = constrainf(setpointScale, 0, multicopterPosXyCoefficients.dTermAttenuation);
-
-    float measurementScale = scaleRangef(measurementNormalized, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd, 0, multicopterPosXyCoefficients.dTermAttenuation);
-    measurementScale = constrainf(measurementScale, 0, multicopterPosXyCoefficients.dTermAttenuation);
-
-    DEBUG_SET(DEBUG_ALWAYS, 2, setpointScale * 100);
-    DEBUG_SET(DEBUG_ALWAYS, 3, measurementScale * 100);
+    const float setpointScale = computeVelocityScale(
+        setpointXY, 
+        maxSpeed, 
+        multicopterPosXyCoefficients.dTermAttenuation,
+        multicopterPosXyCoefficients.dTermAttenuationStart,
+        multicopterPosXyCoefficients.dTermAttenuationEnd
+    );
+    const float measurementScale = computeVelocityScale(
+        posControl.actualState.velXY, 
+        maxSpeed, 
+        multicopterPosXyCoefficients.dTermAttenuation,
+        multicopterPosXyCoefficients.dTermAttenuationStart,
+        multicopterPosXyCoefficients.dTermAttenuationEnd
+    );
 
     //Choose smaller attenuation factor and convert from attenuation to scale
     const float dtermScale = 1.0f - MIN(setpointScale, measurementScale);
-    DEBUG_SET(DEBUG_ALWAYS, 4, dtermScale * 100);
 
     // 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