use position for altitude control

docs/Settings.md

@@ -3132,6 +3132,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]
@@ -3614,7 +3624,7 @@ 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 |
 | --- | --- | --- |

src/main/fc/settings.yaml

@@ -2148,7 +2148,7 @@ groups:
         min: 0
         max: 255
       - name: nav_fw_pos_z_ff
-        description: "FF gain of altitude PID controller (Fixedwing)"
+        description: "FF gain of altitude PID controller. Not used if nav_fw_alt_use_position is set ON (Fixedwing)"
         default_value: 10
         field: bank_fw.pid[PID_POS_Z].FF
         min: 0
@@ -2159,6 +2159,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

@@ -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

@@ -4959,13 +4959,23 @@ void navigationUsePIDs(void)
                                         0.0f
     );
 
-    navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
+    if (pidProfile()->fwAltControlUsePos) {
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.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].D / 300.0f,
+                                            (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
-                                        (float)pidProfile()->bank_fw.pid[PID_POS_Z].FF / 100.0f,
+                                            (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 30.0f,
-                                        NAV_DTERM_CUT_HZ,
+                                            0.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,7 +131,7 @@ 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);
 
@@ -144,12 +144,57 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     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]
+    // Default control based on climb rate (velocity)
-    float currentClimbRate = navGetCurrentActualPositionAndVelocity()->vel.z;
+    float targetValue = desiredClimbRate;
-    float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, desiredClimbRate, currentClimbRate, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, PID_DTERM_FROM_ERROR);
+    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) {
+            posControl.desiredState.pos.z += desiredClimbRate * US2S(deltaMicros);
+            desiredAltitude = posControl.desiredState.pos.z;
+        } else if (altitudeRateControlActive) {
+            /* Disable rate control if no longer required (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.
+             * Adjustment factor based on vertical acceleration used to control rate of change of tracking altitude
+             * to better control desired vertical velocity. Helps avoid overcontrol by PID loop. */
+            static float absLastClimbRate = 0.0f;
+            float absClimbRate = fabsf(measuredValue);
+            float accelerationZ = (absClimbRate - absLastClimbRate) / US2S(deltaMicros);
+            absLastClimbRate = absClimbRate;
+            float accDerivedAdjustmentFactor = constrainf(scaleRangef(accelerationZ, 0.0f, 200.0f, 1.0f, 0.0f), 0.0f, 1.0f);
+
+            trackingAltitude += accDerivedAdjustmentFactor * desiredClimbRate * US2S(deltaMicros);
+            desiredAltitude = trackingAltitude;
+        }
+
+        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, PID_DTERM_FROM_ERROR);
 
     // 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 +216,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 +226,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