Revised multicopter navigation acceleration attenuation settings

docs/Settings.md

@@ -3612,23 +3612,23 @@ Maximum D-term attenution percentage for horizontal velocity PID controller (Mul
 
 ---
 
-### nav_mc_vel_xy_dterm_attenuation_end
+### nav_mc_vel_xy_dterm_attenuation_end_speed
 
-A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum
+Horizontal speed at which nav_mc_vel_xy_dterm_attenuation reaches maximum [m/s]
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 60 | 0 | 100 |
+| 10 | 0 | 100 |
 
 ---
 
-### nav_mc_vel_xy_dterm_attenuation_start
+### nav_mc_vel_xy_dterm_attenuation_start_speed
 
-A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins
+Horizontal speed at which nav_mc_vel_xy_dterm_attenuation begins [m/s]
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 10 | 0 | 100 |
+| 5 | 0 | 100 |
 
 ---
 

src/main/cms/cms_menu_imu.c

@@ -460,6 +460,8 @@ static const OSD_Entry cmsx_menuMechanicsEntries[] =
     OSD_SETTING_ENTRY("ITERM RELAX", SETTING_MC_ITERM_RELAX),
     OSD_SETTING_ENTRY("ITERM CUTOFF", SETTING_MC_ITERM_RELAX_CUTOFF),
     OSD_SETTING_ENTRY("CD LPF", SETTING_MC_CD_LPF_HZ),
+    OSD_SETTING_ENTRY("MC DTERM ATT START", SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_SPEED),
+    OSD_SETTING_ENTRY("MC DTERM ATT END", SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_SPEED),
 
     OSD_BACK_AND_END_ENTRY,
 };

src/main/fc/settings.yaml

@@ -2039,15 +2039,15 @@ groups:
         min: 0
         max: 100
         default_value: 90
-      - 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
+      - name: nav_mc_vel_xy_dterm_attenuation_start_speed
+        description: "Horizontal speed at which nav_mc_vel_xy_dterm_attenuation begins [m/s]"
+        default_value: 5
         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
+      - name: nav_mc_vel_xy_dterm_attenuation_end_speed
+        description: "Horizontal speed at which nav_mc_vel_xy_dterm_attenuation reaches maximum [m/s]"
+        default_value: 10
         field: navVelXyDtermAttenuationEnd
         min: 0
         max: 100
@@ -2295,7 +2295,7 @@ groups:
         field: w_z_surface_p
         min: 0
         max: 100
-        default_value: 3.5 
+        default_value: 3.5
       - name: inav_w_z_surface_v
         description: "Weight of rangefinder measurements in estimated climb rate. Setting is used on both airplanes and multirotors when rangefinder is present and Surface mode enabled"
         field: w_z_surface_v
@@ -4108,7 +4108,7 @@ groups:
     type: navFwAutolandConfig_t
     headers: ["navigation/navigation.h"]
     condition: USE_FW_AUTOLAND
-    members: 
+    members:
       - name: nav_fw_land_approach_length
         description: "Length of the final approach"
         default_value: 35000

src/main/flight/pid.c

@@ -273,8 +273,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 
         .navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT,
         .navVelXyDtermAttenuation = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_DEFAULT,
-        .navVelXyDtermAttenuationStart = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_DEFAULT,
-        .navVelXyDtermAttenuationEnd = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_DEFAULT,
+        .navVelXyDtermAttenuationStart = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_SPEED_DEFAULT,
+        .navVelXyDtermAttenuationEnd = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_SPEED_DEFAULT,
         .iterm_relax_cutoff = SETTING_MC_ITERM_RELAX_CUTOFF_DEFAULT,
         .iterm_relax = SETTING_MC_ITERM_RELAX_DEFAULT,
 

src/main/navigation/navigation.c

@@ -4641,8 +4641,8 @@ void navigationUsePIDs(void)
      * Set coefficients used in MC VEL_XY
      */
     multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f;
-    multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart / 100.0f;
-    multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd / 100.0f;
+    multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart * 100.0f;
+    multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd * 100.0f;
 
 #ifdef USE_MR_BRAKING_MODE
     multicopterPosXyCoefficients.breakingBoostFactor = (float) navConfig()->mc.braking_boost_factor / 100.0f;

src/main/navigation/navigation_multicopter.c

@@ -551,20 +551,23 @@ static float computeNormalizedVelocity(const float value, const float maxValue)
 }
 
 static float computeVelocityScale(
-    const float value,
-    const float maxValue,
+    float activeSpeed,
     const float attenuationFactor,
-    const float attenuationStart,
-    const float attenuationEnd
+    const float attenuationStartVel,
+    const float attenuationEndVel
 )
 {
-    const float normalized = computeNormalizedVelocity(value, maxValue);
+    activeSpeed -= attenuationStartVel;
+    if (activeSpeed <= 0.0f) {
+        return 0.0f;
+    }
 
-    float scale = scaleRangef(normalized, attenuationStart, attenuationEnd, 0, attenuationFactor);
-    return constrainf(scale, 0, attenuationFactor);
+    const float normalized = computeNormalizedVelocity(activeSpeed, attenuationEndVel - attenuationStartVel);
+    float scale = scaleRangef(normalized, 0.0f, 1.0f, 0.0f, attenuationFactor);
+    return constrainf(scale, 0.0f, attenuationFactor);
 }
 
-static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed)
+static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit)
 {
     const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x;
     const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y;
@@ -615,14 +618,12 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
      */
     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
@@ -734,7 +735,7 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
             // Get max speed for current NAV mode
             float maxSpeed = getActiveSpeed();
             updatePositionVelocityController_MC(maxSpeed);
-            updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed);
+            updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX);
 
             navDesiredVelocity[X] = constrain(lrintf(posControl.desiredState.vel.x), -32678, 32767);
             navDesiredVelocity[Y] = constrain(lrintf(posControl.desiredState.vel.y), -32678, 32767);