improvements

src/main/navigation/navigation.c

@@ -1898,16 +1898,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                     setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
 
                     // Use linear climb between WPs arriving at WP altitude when within 10% of total distance to WP
-                    // Update climbrate until within 25% of total distance to WP, then hold constant
+                    // Update climb rate until within 100cm of total climb xy distance to WP, then hold constant
-                    static float climbRate = 0;
+                    float climbRate = 0.0f;
-                    if (posControl.wpDistance > 0.25f * posControl.wpInitialDistance) {
+                    if (posControl.wpDistance - 0.1f * posControl.wpInitialDistance > 100.0f) {
                         climbRate = posControl.actualState.velXY * (posControl.activeWaypoint.pos.z - posControl.actualState.abs.pos.z) /
                                     (posControl.wpDistance - 0.1f * posControl.wpInitialDistance);
                     }
-
-                    if (fabsf(climbRate) >= (STATE(AIRPLANE) ? navConfig()->fw.max_auto_climb_rate : navConfig()->mc.max_auto_climb_rate)) {
-                        climbRate = 0;
-                    }
                     updateClimbRateToAltitudeController(climbRate, posControl.activeWaypoint.pos.z, ROC_TO_ALT_TARGET);
 
                     if(STATE(MULTIROTOR)) {
@@ -3401,14 +3397,20 @@ bool isProbablyStillFlying(void)
 float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
 {
     const bool emergLandingIsActive = navigationIsExecutingAnEmergencyLanding();
-
     float maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_auto_climb_rate : navConfig()->fw.max_auto_climb_rate;
+
+    if (posControl.flags.rocToAltMode == ROC_TO_ALT_CONSTANT) {
+        if (posControl.flags.isAdjustingAltitude) {
+            maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate;
+        }
+
+        return constrainf(posControl.desiredState.climbRateDemand, -maxClimbRate, maxClimbRate);
+    }
+
     if (posControl.desiredState.climbRateDemand) {
-        maxClimbRate = ABS(posControl.desiredState.climbRateDemand);
+        maxClimbRate = constrainf(ABS(posControl.desiredState.climbRateDemand), 0.0f, maxClimbRate);
     } else if (emergLandingIsActive) {
         maxClimbRate = navConfig()->general.emerg_descent_rate;
-    } else if (posControl.flags.isAdjustingAltitude) {
-        maxClimbRate = STATE(MULTIROTOR) ? navConfig()->mc.max_manual_climb_rate : navConfig()->fw.max_manual_climb_rate;
     }
 
     const float targetAltitudeError = targetAltitude - navGetCurrentActualPositionAndVelocity()->pos.z;
@@ -3420,8 +3422,8 @@ float getDesiredClimbRate(float targetAltitude, timeDelta_t deltaMicros)
         targetVel = pidProfile()->fwAltControlResponseFactor * targetAltitudeError / 100.0f;
     }
 
-    if (emergLandingIsActive && targetAltitudeError > -50) {
+    if (emergLandingIsActive && targetAltitudeError > -50.0f) {
-        return -100;    // maintain 1 m/s descent during emerg landing when approaching T/O altitude
+        return -100.0f;    // maintain 1 m/s descent during emerg landing when within 50cm of min speed landing altitude target
     } else {
         return constrainf(targetVel, -maxClimbRate, maxClimbRate);
     }
@@ -3439,6 +3441,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
 
     if (mode == ROC_TO_ALT_CURRENT) {
         posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
+        desiredClimbRate = 0.0f;
     } else if (mode == ROC_TO_ALT_TARGET) {
         posControl.desiredState.pos.z = targetAltitude;
     }

src/main/navigation/navigation_fixedwing.c

@@ -133,11 +133,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 {
     static pt1Filter_t velzFilterState;
 
-    float desiredClimbRate = posControl.desiredState.climbRateDemand;
+    float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
-
-    if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
-        desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
-    }
 
     // Reduce max allowed climb pitch if performing loiter (stall prevention)
     if (needToCalculateCircularLoiter && desiredClimbRate > 0.0f) {
@@ -770,8 +766,8 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs)
     rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
 
     if (posControl.flags.estAltStatus >= EST_USABLE) {
-        // target min descent rate 10m above takeoff altitude
+        // target min descent rate at distance 2 x emerg descent rate above takeoff altitude
-        updateClimbRateToAltitudeController(0, 1000.0f, ROC_TO_ALT_TARGET);
+        updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_TARGET);
         applyFixedWingAltitudeAndThrottleController(currentTimeUs);
 
         int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));

src/main/navigation/navigation_multicopter.c

@@ -76,11 +76,7 @@ float getSqrtControllerVelocity(float targetAltitude, timeDelta_t deltaMicros)
 // Position to velocity controller for Z axis
 static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
 {
-    float targetVel = posControl.desiredState.climbRateDemand;
+    float targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
-
-    if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
-        targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
-    }
 
     posControl.pids.pos[Z].output_constrained = targetVel;      // only used for Blackbox and OSD info
 
@@ -141,6 +137,7 @@ bool adjustMulticopterAltitudeFromRCInput(void)
     }
     else {
         const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500);
+
         if (rcThrottleAdjustment) {
             // set velocity proportional to stick movement
             float rcClimbRate;
@@ -928,8 +925,8 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
 
         // Check if last correction was not too long ago
         if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
-            // target min descent rate 5m above takeoff altitude
+            // target min descent rate at distance 2 x emerg descent rate above takeoff altitude
-            updateClimbRateToAltitudeController(0, 500.0f, ROC_TO_ALT_TARGET);
+            updateClimbRateToAltitudeController(0, 2.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_TARGET);
             updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
             updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
         }