first cut

docs/Settings.md

@@ -3572,6 +3572,16 @@ P gain of altitude PID controller (Multirotor)
 
 ---
 
+### nav_mc_vel_xy_accel_attenuate
+
+Adjustment factor to attenuate horizonal acceleration response as target speed is reached Reduce setting to prevent overshooting target speed.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 100 | 10 | 100 |
+
+---
+
 ### nav_mc_vel_xy_d
 
 D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target.

src/main/cms/cms_menu_imu.c

@@ -437,6 +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_BACK_AND_END_ENTRY,
 };

src/main/fc/settings.yaml

@@ -192,7 +192,7 @@ tables:
     enum: gpsBaudRate_e
   - name: nav_mc_althold_throttle
     values: ["STICK", "MID_STICK", "HOVER"]
-    enum: navMcAltHoldThrottle_e    
+    enum: navMcAltHoldThrottle_e
   - name: led_pin_pwm_mode
     values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"]
     enum: led_pin_pwm_mode_e
@@ -1186,7 +1186,7 @@ groups:
         min: 0
         max: 200
 
-      
+
 
   - name: PG_REVERSIBLE_MOTORS_CONFIG
     type: reversibleMotorsConfig_t
@@ -2040,6 +2040,12 @@ groups:
         min: 0
         max: 255
         default_value: 40
+      - name: nav_mc_vel_xy_accel_attenuate
+        description: "Adjustment factor to attenuate horizonal acceleration response as target speed is reached Reduce setting to prevent overshooting target speed."
+        field: mc_vel_xy_accel_attenuate
+        default_value: 100
+        min: 10
+        max: 100
       - name: nav_mc_heading_p
         description: "P gain of Heading Hold controller (Multirotor)"
         default_value: 60
@@ -3586,7 +3592,7 @@ groups:
         min: 1000
         max: 5000
         default_value: 1500
-  
+
       - name: osd_switch_indicator_zero_name
         description: "Character to use for OSD switch incicator 0."
         field: osd_switch_indicator0_name

src/main/flight/pid.c

@@ -278,6 +278,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .navVelXyDtermAttenuationEnd = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_DEFAULT,
         .iterm_relax_cutoff = SETTING_MC_ITERM_RELAX_CUTOFF_DEFAULT,
         .iterm_relax = SETTING_MC_ITERM_RELAX_DEFAULT,
+        .mc_vel_xy_accel_attenuate = SETTING_NAV_MC_VEL_XY_ACCEL_ATTENUATE_DEFAULT,
 
 #ifdef USE_D_BOOST
         .dBoostMin = SETTING_D_BOOST_MIN_DEFAULT,
@@ -838,7 +839,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
 
     pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT)
                              + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
-    
+
     if (pidProfile()->pidItermLimitPercent != 0){
         float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f;
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);

src/main/flight/pid.h

@@ -132,6 +132,7 @@ typedef struct pidProfile_s {
     uint8_t navVelXyDtermAttenuationEnd;    // VEL_XY dynamic Dterm scale: Dterm will be fully attenuated at this percent of max velocity
     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
+    uint8_t mc_vel_xy_accel_attenuate;
 
 #ifdef USE_D_BOOST
     float dBoostMin;

src/main/navigation/navigation_multicopter.c

@@ -632,6 +632,13 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
     //Choose smaller attenuation factor and convert from attenuation to scale
     const float dtermScale = 1.0f - MIN(setpointScale, measurementScale);
 
+    // Apply accel attenuation factor
+    const float speedError = setpointXY != 0.0f ? fabsf(1.0f - (posControl.actualState.velXY / setpointXY)) : 1.0f;
+    float gainScale = 1.0f;
+    if (speedError < 0.25f) {
+        gainScale = (scaleRangef(speedError, 0.0f, 0.25f, pidProfile()->mc_vel_xy_accel_attenuate, 100.0f)) / 100.0f;
+    }
+
     // 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
@@ -643,7 +650,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
         accelLimitXMin,
         accelLimitXMax,
         0,      // Flags
-        1.0f,   // Total gain scale
+        gainScale,   // Total gain scale
         dtermScale    // Additional dTerm scale
     );
     float newAccelY = navPidApply3(
@@ -654,7 +661,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
         accelLimitYMin,
         accelLimitYMax,
         0,      // Flags
-        1.0f,   // Total gain scale
+        gainScale,   // Total gain scale
         dtermScale    // Additional dTerm scale
     );