diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt
index dd9b3da41d..4b7cac1852 100644
--- a/src/modules/flight_mode_manager/CMakeLists.txt
+++ b/src/modules/flight_mode_manager/CMakeLists.txt
@@ -52,16 +52,16 @@ endif()
 
 # add core flight tasks to list
 list(APPEND flight_tasks_all
+	AutoFollowMe
+	AutoLineSmoothVel
+	Descend
+	Failsafe
+	ManualAcceleration
 	ManualAltitude
 	ManualAltitudeSmoothVel
 	ManualPosition
 	ManualPositionSmoothVel
-	AutoLineSmoothVel
-	AutoFollowMe
-	Failsafe
-	Descend
 	Transition
-	ManualAcceleration
 	${flight_tasks_to_add}
 )
 
diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
index 941d05ea8e..e986db4616 100644
--- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
+++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
@@ -87,11 +87,12 @@ const vehicle_local_position_setpoint_s FlightTask::getPositionSetpoint()
 	vehicle_local_position_setpoint.vy = _velocity_setpoint(1);
 	vehicle_local_position_setpoint.vz = _velocity_setpoint(2);
 
-	_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
-	_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
 	vehicle_local_position_setpoint.yaw = _yaw_setpoint;
 	vehicle_local_position_setpoint.yawspeed = _yawspeed_setpoint;
 
+	_acceleration_setpoint.copyTo(vehicle_local_position_setpoint.acceleration);
+	_jerk_setpoint.copyTo(vehicle_local_position_setpoint.jerk);
+
 	// deprecated, only kept for output logging
 	matrix::Vector3f(NAN, NAN, NAN).copyTo(vehicle_local_position_setpoint.thrust);
 
@@ -104,7 +105,8 @@ void FlightTask::_resetSetpoints()
 	_velocity_setpoint.setNaN();
 	_acceleration_setpoint.setNaN();
 	_jerk_setpoint.setNaN();
-	_yaw_setpoint = _yawspeed_setpoint = NAN;
+	_yaw_setpoint = NAN;
+	_yawspeed_setpoint = NAN;
 }
 
 void FlightTask::_evaluateVehicleLocalPosition()
diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
index c42852c0c4..257ff8ae2d 100644
--- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
+++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
@@ -244,15 +244,14 @@ protected:
 	 */
 	matrix::Vector3f _position_setpoint;
 	matrix::Vector3f _velocity_setpoint;
+	matrix::Vector3f _velocity_setpoint_feedback;
 	matrix::Vector3f _acceleration_setpoint;
+	matrix::Vector3f _acceleration_setpoint_feedback;
 	matrix::Vector3f _jerk_setpoint;
 
 	float _yaw_setpoint{};
 	float _yawspeed_setpoint{};
 
-	matrix::Vector3f _velocity_setpoint_feedback;
-	matrix::Vector3f _acceleration_setpoint_feedback;
-
 	ekf_reset_counters_s _reset_counters{}; ///< Counters for estimator local position resets
 
 	/**