diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 7505260c88..ded835919d 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -3086,6 +3086,8 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
         posControl.desiredState.pos.z = navGetCurrentActualPositionAndVelocity()->pos.z;
     } else if (mode == ROC_TO_ALT_TARGET) {
         posControl.desiredState.pos.z = targetAltitude;
+    } else {    // ROC_TO_ALT_CONSTANT
+        posControl.desiredState.climbRateDemand = desiredClimbRate;
     }
 
     posControl.flags.rocToAltMode = mode;
@@ -3097,8 +3099,6 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, float targetAlt
         posControl.desiredState.pos.z = MIN(posControl.desiredState.pos.z, navConfig()->general.max_altitude);
         posControl.flags.rocToAltMode = ROC_TO_ALT_TARGET;
     }
-
-    posControl.desiredState.vel.z = desiredClimbRate;   // only used for ROC_TO_ALT_CONSTANT
 }
 
 static void resetAltitudeController(bool useTerrainFollowing)
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 0272df114c..f5c3fb159a 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -134,7 +134,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 {
     static pt1Filter_t velzFilterState;
 
-    float desiredClimbRate = posControl.desiredState.vel.z;
+    float desiredClimbRate = posControl.desiredState.climbRateDemand;
 
     if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
         desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 559898eadf..b35d88c609 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -76,7 +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.vel.z;
+    float targetVel = posControl.desiredState.climbRateDemand;
 
     if (posControl.flags.rocToAltMode != ROC_TO_ALT_CONSTANT) {
         targetVel = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
@@ -249,6 +249,7 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs)
             if (prepareForTakeoffOnReset) {
                 const navEstimatedPosVel_t *posToUse = navGetCurrentActualPositionAndVelocity();
 
+                posControl.desiredState.vel.z = -navConfig()->general.max_manual_climb_rate;
                 posControl.desiredState.pos.z = posToUse->pos.z - (navConfig()->general.max_manual_climb_rate / posControl.pids.pos[Z].param.kP);
                 posControl.pids.vel[Z].integrator = -500.0f;
                 pt1FilterReset(&altholdThrottleFilterState, -500.0f);
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index 428b54cd4f..cf4698a24c 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -140,6 +140,7 @@ typedef struct {
     fpVector3_t pos;
     fpVector3_t vel;
     int32_t     yaw;
+    int16_t     climbRateDemand;
 } navigationDesiredState_t;
 
 typedef enum {