Changed control logic

docs/Settings.md

@@ -3604,21 +3604,21 @@ Maximum D-term attenution percentage for horizontal velocity PID controller (Mul
 
 ### nav_mc_vel_xy_dterm_attenuation_end
 
-A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum
+Horizontal velocity 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
 
-A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins
+Horizontal velocity 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

@@ -437,7 +437,7 @@ 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("ACCEL ATTENUATION", SETTING_NAV_MC_VEL_XY_ACCEL_ATTENUATE),
+    OSD_SETTING_ENTRY("MC DTERM ATT START", SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START),
 
     OSD_BACK_AND_END_ENTRY,
 };

src/main/fc/settings.yaml

@@ -2064,14 +2064,14 @@ groups:
         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
+        description: "Horizontal velocity 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
+        description: "Horizontal velocity at which nav_mc_vel_xy_dterm_attenuation reaches maximum [m/s]"
+        default_value: 10
         field: navVelXyDtermAttenuationEnd
         min: 0
         max: 100

src/main/navigation/navigation.c

@@ -4274,8 +4274,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

@@ -552,20 +552,22 @@ 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);
-
-    float scale = scaleRangef(normalized, attenuationStart, attenuationEnd, 0, attenuationFactor);
-    return constrainf(scale, 0, attenuationFactor);
+    activeSpeed -= attenuationStartVel;
+    if (activeSpeed <= 0.0f) {
+        return 0.0f;
+    }
+    const float normalized = computeNormalizedVelocity(activeSpeed, attenuationEndVel);
+    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;
@@ -616,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
@@ -742,7 +742,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);