diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index 939f40d858..78c90de94f 100644
--- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -251,9 +251,7 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 
 void FlightTaskManualAltitude::_updateSetpoints()
 {
-	FlightTaskManualStabilized::_updateSetpoints(); // get yaw and thrust setpoints
-
-	_thrust_setpoint *= NAN; // Don't need thrust setpoint from Stabilized mode.
+	FlightTaskManualStabilized::_updateHeadingSetpoints(); // get yaw setpoint
 
 	// Thrust in xy are extracted directly from stick inputs. A magnitude of
 	// 1 means that maximum thrust along xy is demanded. A magnitude of 0 means no
@@ -269,6 +267,7 @@ void FlightTaskManualAltitude::_updateSetpoints()
 
 	_thrust_setpoint(0) = sp(0);
 	_thrust_setpoint(1) = sp(1);
+	_thrust_setpoint(2) = NAN;
 
 	_updateAltitudeLock();
 }
diff --git a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp
index b846a1ba42..41f28c581b 100644
--- a/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp
+++ b/src/lib/FlightTasks/tasks/ManualStabilized/FlightTaskManualStabilized.hpp
@@ -58,12 +58,12 @@ public:
 	void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
 
 protected:
-	virtual void _updateSetpoints(); /**< updates all setpoints*/
+	virtual void _updateSetpoints(); /**< updates all setpoints */
+	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
 	virtual void _scaleSticks(); /**< scales sticks to yaw and thrust */
 	void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /**< rotates vector into local frame */
 
 private:
-	void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
 	void _updateThrustSetpoints(); /**< sets thrust setpoint */
 	float _throttleCurve(); /**< piecewise linear mapping from stick to throttle */