Merge pull request iNavFlight#10903 from breadoven/abo_fw_alt_cont_use_pos

Option to use position for fixed wing nav altitude control

Author: breadoven

docs/Settings.md

@@ -3152,6 +3152,16 @@ Adjusts the deceleration response of fixed wing altitude control as the target a
 
 ---
 
+### nav_fw_alt_use_position
+
+Use position for fixed wing altitude control rather than velocity (default method).
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### nav_fw_auto_climb_rate
 
 Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]
@@ -3634,11 +3644,11 @@ D gain of altitude PID controller (Fixedwing)
 
 ### nav_fw_pos_z_ff
 
-FF gain of altitude PID controller (Fixedwing)
+FF gain of altitude PID controller. Not used if nav_fw_alt_use_position is set ON (Fixedwing)
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 10 | 0 | 255 |
+| 30 | 0 | 255 |
 
 ---
 

src/main/fc/settings.yaml

@@ -2160,8 +2160,8 @@ groups:
         min: 0
         max: 255
       - name: nav_fw_pos_z_ff
-        description: "FF gain of altitude PID controller (Fixedwing)"
-        default_value: 10
+        description: "FF gain of altitude PID controller. Not used if nav_fw_alt_use_position is set ON (Fixedwing)"
+        default_value: 30
         field: bank_fw.pid[PID_POS_Z].FF
         min: 0
         max: 255
@@ -2171,6 +2171,11 @@ groups:
         field: fwAltControlResponseFactor
         min: 5
         max: 100
+      - name: nav_fw_alt_use_position
+        description: "Use position for fixed wing altitude control rather than velocity (default method)."
+        default_value: OFF
+        field: fwAltControlUsePos
+        type: bool
       - name: nav_fw_pos_xy_p
         description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH"
         default_value: 75

src/main/flight/pid.c

@@ -179,7 +179,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 10);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -308,6 +308,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,
 
         .fwAltControlResponseFactor = SETTING_NAV_FW_ALT_CONTROL_RESPONSE_DEFAULT,
+        .fwAltControlUsePos = SETTING_NAV_FW_ALT_USE_POSITION_DEFAULT,
 #ifdef USE_SMITH_PREDICTOR
         .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
         .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,

src/main/flight/pid.h

@@ -150,6 +150,7 @@ typedef struct pidProfile_s {
     float fixedWingLevelTrimGain;
 
     uint8_t fwAltControlResponseFactor;
+    bool fwAltControlUsePos;
 #ifdef USE_SMITH_PREDICTOR
     float smithPredictorStrength;
     float smithPredictorDelay;

src/main/navigation/navigation.c

@@ -4956,13 +4956,23 @@ void navigationUsePIDs(void)
                                         0.0f
     );
 
-    navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 300.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f,
-                                        NAV_DTERM_CUT_HZ,
-                                        0.0f
-    );
+    if (pidProfile()->fwAltControlUsePos) {
+        navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
+                                            (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
+                                            (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f,
+                                            0.0f,
+                                            NAV_DTERM_CUT_HZ,
+                                            0.0f
+        );
+    } else {
+        navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
+                                            (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
+                                            (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 300.0f,
+                                            (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f,
+                                            NAV_DTERM_CUT_HZ,
+                                            0.0f
+        );
+    }
 
     navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
                                         (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,

src/main/navigation/navigation_fixedwing.c

@@ -131,25 +131,74 @@ bool adjustFixedWingAltitudeFromRCInput(void)
 // Position to velocity controller for Z axis
 static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 {
-    static pt1Filter_t velzFilterState;
+    static pt1Filter_t pitchFilterState;
 
     float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
 
     // Reduce max allowed climb rate by 2/3 if performing loiter (stall prevention)
-    if (needToCalculateCircularLoiter && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) {
+    if (navGetCurrentStateFlags() & NAV_CTL_HOLD && desiredClimbRate > 0.67f * navConfig()->fw.max_auto_climb_rate) {
         desiredClimbRate = 0.67f * navConfig()->fw.max_auto_climb_rate;
     }
 
     // Here we use negative values for dive for better clarity
     const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
     const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
 
-    // PID controller to translate desired climb rate error into pitch angle [decideg]
-    float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z;
-    float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR);
+    // Default control based on climb rate (velocity)
+    float targetValue = desiredClimbRate;
+    float measuredValue = navGetCurrentActualPositionAndVelocity()->vel.z;
+
+    // Optional control based on altitude (position)
+    if (pidProfile()->fwAltControlUsePos) {
+        static float currentDesiredPosZ = 0.0f;
+        static float trackingAltitude = 0.0f;
+        static bool altitudeRateControlActive = false;
+
+        float desiredAltitude = posControl.desiredState.pos.z;
+        float currentAltitude = navGetCurrentActualPositionAndVelocity()->pos.z;
+
+        /* Determine if altitude rate control required based on magnitude of change in target altitude.
+         * No rate control used during trackback to allow max climb rates based on pitch limits */
+        if (fabsf(currentDesiredPosZ - desiredAltitude) > 100.0f || !isPitchAdjustmentValid) {
+            altitudeRateControlActive = desiredClimbRate && fabsf(desiredAltitude - currentAltitude) > navConfig()->fw.max_auto_climb_rate &&
+                                        !posControl.flags.rthTrackbackActive;
+            trackingAltitude = currentAltitude;
+        }
+        currentDesiredPosZ = desiredAltitude;
+
+        if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT || altitudeRateControlActive) {
+            /* Adjustment factor based on vertical acceleration used to better control altitude position change
+             * driving vertical velocity control. Helps avoid lag induced overcontrol by PID loop. */
+            static float absLastClimbRate = 0.0f;
+            float absClimbRate = fabsf(measuredValue);
+            float accelerationZ = (absClimbRate - absLastClimbRate) / US2S(deltaMicros);
+            absLastClimbRate = absClimbRate;
+            float adjustmentFactor = constrainf(scaleRangef(accelerationZ, 0.0f, 200.0f, 1.0f, 0.0f), 0.0f, 1.0f);
+
+            if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) {
+                posControl.desiredState.pos.z += adjustmentFactor * desiredClimbRate * US2S(deltaMicros);
+                desiredAltitude = posControl.desiredState.pos.z;
+            } else {
+                /* Disable rate control if no longer required, i.e. remaining altitude change is small */
+                altitudeRateControlActive = fabsf(desiredAltitude - currentAltitude) > MAX(fabsf(desiredClimbRate), 50.0f);
+
+                /* Tracking altitude used to control altitude rate where changing posControl.desiredState.pos.z not possible */
+                trackingAltitude += adjustmentFactor * desiredClimbRate * US2S(deltaMicros);
+                desiredAltitude = trackingAltitude;
+            }
+        } else {
+            desiredClimbRate = 0;
+        }
+
+        targetValue = desiredAltitude;
+        measuredValue = currentAltitude;
+    }
+
+    // PID controller to translate desired target error (velocity or position) into pitch angle [decideg]
+    float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, targetValue, measuredValue, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0);
 
     // Apply low-pass filter to prevent rapid correction
-    targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
+    targetPitchAngle = pt1FilterApply4(&pitchFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
 
     // Reconstrain pitch angle (> 0 - climb, < 0 - dive)
     targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
@@ -171,6 +220,7 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
             // Check if last correction was not too long ago
             if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
                 updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate);
+                isPitchAdjustmentValid = true;
             }
             else {
                 // Position update has not occurred in time (first iteration or glitch), reset altitude controller
@@ -180,8 +230,6 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
             // Indicate that information is no longer usable
             posControl.flags.verticalPositionDataConsumed = true;
         }
-
-        isPitchAdjustmentValid = true;
     }
     else {
         // No valid altitude sensor data, don't adjust pitch automatically, rcCommand[PITCH] is passed through to PID controller