Refactor Dterm atenuation

src/main/fc/settings.yaml

@@ -1184,18 +1184,18 @@ groups:
         field: navVelXyDTermLpfHz
         min: 0
         max: 100
-      - name: nav_mc_vel_xy_dterm_dyn_scale
-        field: navVelXyDtermDynScale
+      - name: nav_mc_vel_xy_dterm_attenuation
+        field: navVelXyDtermAttenuation
         min: 0
-        max: 1
-      - name: nav_mc_vel_xy_dterm_dyn_scale_min_at
-        field: navVelXyDtermDynScaleMinAt
+        max: 100
+      - name: nav_mc_vel_xy_dterm_attenuation_start
+        field: navVelXyDtermAttenuationStart
         min: 0
-        max: 1
-      - name: nav_mc_vel_xy_dterm_dyn_scale_max_at
-        field: navVelXyDtermDynScaleMaxAt
+        max: 100
+      - name: nav_mc_vel_xy_dterm_attenuation_end
+        field: navVelXyDtermAttenuationEnd
         min: 0
-        max: 1
+        max: 100
       - name: nav_fw_pos_z_p
         field: bank_fw.pid[PID_POS_Z].P
         condition: USE_NAV

src/main/flight/pid.c

@@ -257,9 +257,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,
+        .navVelXyDtermAttenuation = 80,
+        .navVelXyDtermAttenuationStart = 20,
+        .navVelXyDtermAttenuationEnd = 60,
         .iterm_relax_type = ITERM_RELAX_SETPOINT,
         .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT,
         .iterm_relax = ITERM_RELAX_RP,

src/main/flight/pid.h

@@ -138,9 +138,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 navVelXyDtermAttenuation;       // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity
+    uint8_t navVelXyDtermAttenuationStart;  // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity
+    uint8_t navVelXyDtermAttenuationEnd;    // VEL_XY dynamic Dterm scale: Dterm will be fully attenuated at this percent of max velocity
     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

@@ -171,8 +171,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
     }
 );
 
-navigationPosControl_t  posControl;
-navSystemStatus_t       NAV_Status;
+EXTENDED_FASTRAM navigationPosControl_t  posControl;
+EXTENDED_FASTRAM navSystemStatus_t       NAV_Status;
+EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
 
 #if defined(NAV_BLACKBOX)
 int16_t navCurrentState;
@@ -3231,6 +3232,14 @@ 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.breakingBoostFactor = (float) navConfig()->mc.braking_boost_factor / 100.0f;
+
     // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
     navPidInit(
         &posControl.pids.pos[Z],

src/main/navigation/navigation_multicopter.c

@@ -50,7 +50,6 @@
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
-
 /*-----------------------------------------------------------
  * Altitude controller for multicopter aircraft
  *-----------------------------------------------------------*/
@@ -471,10 +470,8 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
 
 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;
@@ -522,26 +519,25 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
     
     //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);
+    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);
 
     /* 
-     * 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);
+    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);
 
-    //Use the higher scaling factor from the two above
-    const float dtermScale = MAX(setpointScale, measurementScale);
+    //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
@@ -574,8 +570,7 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
 
 #ifdef USE_MR_BRAKING_MODE
     //Boost required accelerations
-    if (STATE(NAV_CRUISE_BRAKING_BOOST) && navConfig()->mc.braking_boost_factor > 0) {
-        const float rawBoostFactor = (float)navConfig()->mc.braking_boost_factor / 100.0f;
+    if (STATE(NAV_CRUISE_BRAKING_BOOST) && multicopterPosXyCoefficients.breakingBoostFactor > 0.0f) {
 
         //Scale boost factor according to speed
         const float boostFactor = constrainf(
@@ -584,10 +579,10 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
                 navConfig()->mc.braking_boost_speed_threshold,
                 navConfig()->general.max_manual_speed,
                 0.0f,
-                rawBoostFactor
+                multicopterPosXyCoefficients.breakingBoostFactor
             ),
             0.0f,
-            rawBoostFactor
+            multicopterPosXyCoefficients.breakingBoostFactor
         );
 
         //Boost required acceleration for harder braking

src/main/navigation/navigation_private.h

@@ -363,11 +363,19 @@ typedef struct {
     float                       totalTripDistance;
 } navigationPosControl_t;
 
+typedef struct {
+    float dTermAttenuation;
+    float dTermAttenuationStart;
+    float dTermAttenuationEnd;
+    float breakingBoostFactor;
+} multicopterPosXyCoefficients_t;
+
 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
 PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList);
 #endif
 
 extern navigationPosControl_t posControl;
+extern multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
 
 /* Internally used functions */
 const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);