diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp
index 514a84c6ae..58b8bb0a07 100644
--- a/src/modules/mc_att_control/mc_att_control.hpp
+++ b/src/modules/mc_att_control/mc_att_control.hpp
@@ -89,8 +89,6 @@ private:
 	 */
 	void		parameters_updated();
 
-	void		publish_rates_setpoint();
-
 	float		throttle_curve(float throttle_stick_input);
 
 	/**
@@ -98,11 +96,6 @@ private:
 	 */
 	void		generate_attitude_setpoint(float dt, bool reset_yaw_sp);
 
-	/**
-	 * Attitude controller.
-	 */
-	void		control_attitude();
-
 	AttitudeControl _attitude_control; ///< class for attitude control calculations
 
 	uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
@@ -119,7 +112,6 @@ private:
 	uORB::Publication<vehicle_attitude_setpoint_s>	_vehicle_attitude_setpoint_pub;
 
 	struct vehicle_attitude_s		_v_att {};		/**< vehicle attitude */
-	struct vehicle_rates_setpoint_s		_v_rates_sp {};		/**< vehicle rates setpoint */
 	struct manual_control_setpoint_s	_manual_control_setpoint {};	/**< manual control setpoint */
 	struct vehicle_control_mode_s		_v_control_mode {};	/**< vehicle control mode */
 	struct vehicle_status_s			_vehicle_status {};	/**< vehicle status */
@@ -128,7 +120,6 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
 	matrix::Vector3f _thrust_setpoint_body; ///< body frame 3D thrust vector
-	matrix::Vector3f _rates_sp; ///< angular rates setpoint
 
 	float _man_yaw_sp{0.f};				/**< current yaw setpoint in manual mode */
 	float _man_tilt_max;			/**< maximum tilt allowed for manual flight [rad] */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 3e45815957..824691c2c2 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -220,31 +220,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
 	_vehicle_attitude_setpoint_pub.publish(attitude_setpoint);
 }
 
-/**
- * Attitude controller.
- * Input: 'vehicle_attitude_setpoint' topics (depending on mode)
- * Output: '_rates_sp' vector
- */
-void
-MulticopterAttitudeControl::control_attitude()
-{
-	_rates_sp = _attitude_control.update(Quatf(_v_att.q));
-}
-
-void
-MulticopterAttitudeControl::publish_rates_setpoint()
-{
-	vehicle_rates_setpoint_s v_rates_sp{};
-
-	v_rates_sp.roll = _rates_sp(0);
-	v_rates_sp.pitch = _rates_sp(1);
-	v_rates_sp.yaw = _rates_sp(2);
-	_thrust_setpoint_body.copyTo(v_rates_sp.thrust_body);
-	v_rates_sp.timestamp = hrt_absolute_time();
-
-	_v_rates_sp_pub.publish(v_rates_sp);
-}
-
 void
 MulticopterAttitudeControl::Run()
 {
@@ -333,16 +308,27 @@ MulticopterAttitudeControl::Run()
 				_man_y_input_filter.reset(0.f);
 			}
 
-			control_attitude();
+			Vector3f rates_sp = _attitude_control.update(Quatf(_v_att.q));
 
 			if (_v_control_mode.flag_control_yawrate_override_enabled) {
 				/* Yaw rate override enabled, overwrite the yaw setpoint */
-				_v_rates_sp_sub.update(&_v_rates_sp);
-				const auto yawrate_reference = _v_rates_sp.yaw;
-				_rates_sp(2) = yawrate_reference;
+				vehicle_rates_setpoint_s v_rates_sp{};
+
+				if (_v_rates_sp_sub.copy(&v_rates_sp)) {
+					const auto yawrate_reference = v_rates_sp.yaw;
+					rates_sp(2) = yawrate_reference;
+				}
 			}
 
-			publish_rates_setpoint();
+			// publish rate setpoint
+			vehicle_rates_setpoint_s v_rates_sp{};
+			v_rates_sp.roll = rates_sp(0);
+			v_rates_sp.pitch = rates_sp(1);
+			v_rates_sp.yaw = rates_sp(2);
+			_thrust_setpoint_body.copyTo(v_rates_sp.thrust_body);
+			v_rates_sp.timestamp = hrt_absolute_time();
+
+			_v_rates_sp_pub.publish(v_rates_sp);
 		}
 
 		// reset yaw setpoint during transitions, tailsitter.cpp generates