diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
index bdc2c040e9..7612923c55 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
@@ -252,9 +252,9 @@ void FlightTaskOrbit::_updateTrajectoryBoundaries()
 	_position_smoothing.setMaxJerk({max_jerk, max_jerk, max_jerk}); // TODO : Should be computed using heading
 	_altitude_velocity_smoothing.setMaxJerk(max_jerk);
 
-	if (_unsmoothed_velocity_setpoint(2) < 0.f) { // up
 		const float z_accel_constraint = _param_mpc_acc_up_max.get();
 
+	if (_velocity_setpoint(2) < 0.f) { // up
 		_position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_up.get());
 		_position_smoothing.setMaxAccelerationZ(z_accel_constraint);
 		_altitude_velocity_smoothing.setMaxVel(_param_mpc_z_vel_max_up.get());
diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
index 54dcc640c4..5da387c5be 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
@@ -120,7 +120,6 @@ private:
 	bool _in_circle_approach = false;
 	PositionSmoothing _position_smoothing;
 	VelocitySmoothing _altitude_velocity_smoothing;
-	Vector3f _unsmoothed_velocity_setpoint;
 
 	/** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */
 	int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER;