diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
index 3136c98051..70b5835a6c 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
@@ -179,8 +179,8 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint)
 	_orbit_velocity = 1.f;
 	_center = _position;
 	_initial_heading = _yaw;
-	_slew_rate_yaw.setForcedValue(_yaw);
-	_slew_rate_yaw.setSlewRate(math::radians(_param_mpc_yawrauto_max.get()));
+	_heading_smoothing.reset(PX4_ISFINITE(last_setpoint.yaw) ? last_setpoint.yaw : _yaw,
+				 PX4_ISFINITE(last_setpoint.yawspeed) ? last_setpoint.yawspeed : 0.f);
 	_slew_rate_velocity.setSlewRate(_param_mpc_acc_hor.get());
 
 	// need a valid position and velocity
@@ -238,7 +238,14 @@ bool FlightTaskOrbit::update()
 	}
 
 	// Apply yaw smoothing
-	_yaw_setpoint = _slew_rate_yaw.update(_yaw_setpoint, _deltatime);
+	_heading_smoothing.setMaxHeadingRate(math::radians(_param_mpc_yawrauto_max.get()));
+	_heading_smoothing.setMaxHeadingAccel(math::radians(_param_mpc_yawrauto_acc.get()));
+	_heading_smoothing.update(_yaw_setpoint, _deltatime);
+	_yaw_setpoint = _heading_smoothing.getSmoothedHeading();
+
+	if (_in_circle_approach) { // don't override feed-forward which is already calculated for circling
+		_yawspeed_setpoint = _heading_smoothing.getSmoothedHeadingRate();
+	}
 
 	// publish information to UI
 	sendTelemetry();
@@ -362,7 +369,7 @@ void FlightTaskOrbit::_generate_circle_yaw_setpoints()
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_UNCONTROLLED:
 		// no yaw setpoint
 		_yaw_setpoint = NAN;
-		_yawspeed_setpoint = NAN;
+		_yawspeed_setpoint = 0.f; // No yaw setpoint is invalid -> just brake to 0°/s
 		break;
 
 	case orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE:
diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
index 13e53bf83f..af4ddc67be 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
@@ -45,6 +45,7 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/orbit_status.h>
 #include <lib/slew_rate/SlewRateYaw.hpp>
+#include <lib/motion_planning/HeadingSmoothing.hpp>
 #include <lib/motion_planning/PositionSmoothing.hpp>
 #include <lib/motion_planning/VelocitySmoothing.hpp>
 #include <lib/slew_rate/SlewRate.hpp>
@@ -127,7 +128,7 @@ private:
 	bool _started_clockwise{true};
 	bool _currently_orbiting{false};
 	float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */
-	SlewRateYaw<float> _slew_rate_yaw;
+	HeadingSmoothing _heading_smoothing;
 	SlewRate<float> _slew_rate_velocity;
 
 	orb_advert_t _mavlink_log_pub{nullptr};
@@ -138,6 +139,7 @@ private:
 		(ParamInt<px4::params::MC_ORBIT_YAW_MOD>) _param_mc_orbit_yaw_mod,
 		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise, /**< cruise speed for circle approach */
 		(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
+		(ParamFloat<px4::params::MPC_YAWRAUTO_ACC>) _param_mpc_yawrauto_acc,
 		(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
 		(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
 		_param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated