From cc3c6f63e5acb997d5025632be21095aaa264432 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 3 Mar 2021 19:40:43 -0500
Subject: [PATCH] mc_rate_control: simplify manual_control_setpoint update

---
 .../MulticopterRateControl.cpp                | 29 +++++--------------
 .../MulticopterRateControl.hpp                |  1 -
 2 files changed, 8 insertions(+), 22 deletions(-)

diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp
index 94e0c56209..aa3c46b551 100644
--- a/src/modules/mc_rate_control/MulticopterRateControl.cpp
+++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp
@@ -159,32 +159,19 @@ MulticopterRateControl::Run()
 			}
 		}
 
-		const bool manual_control_updated = _manual_control_setpoint_sub.update(&_manual_control_setpoint);
-
-		// generate the rate setpoint from sticks?
-		bool manual_rate_sp = false;
-
-		if (_v_control_mode.flag_control_manual_enabled &&
-		    !_v_control_mode.flag_control_altitude_enabled &&
-		    !_v_control_mode.flag_control_velocity_enabled &&
-		    !_v_control_mode.flag_control_position_enabled) {
-
-			if (!_v_control_mode.flag_control_attitude_enabled) {
-				manual_rate_sp = true;
-			}
-		}
-
-		if (manual_rate_sp) {
-			if (manual_control_updated) {
+		if (_v_control_mode.flag_control_manual_enabled && !_v_control_mode.flag_control_attitude_enabled) {
+			// generate the rate setpoint from sticks
+			manual_control_setpoint_s manual_control_setpoint;
 
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
 				// manual rates control - ACRO mode
 				const Vector3f man_rate_sp{
-					math::superexpo(_manual_control_setpoint.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
-					math::superexpo(-_manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
-					math::superexpo(_manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
+					math::superexpo(manual_control_setpoint.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
+					math::superexpo(-manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
+					math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
 
 				_rates_sp = man_rate_sp.emult(_acro_rate_max);
-				_thrust_sp = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
+				_thrust_sp = math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
 
 				// publish rate setpoint
 				vehicle_rates_setpoint_s v_rates_sp{};
diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp
index 1e64e9c9a3..57ea08b973 100644
--- a/src/modules/mc_rate_control/MulticopterRateControl.hpp
+++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp
@@ -108,7 +108,6 @@ private:
 	uORB::Publication<landing_gear_s>		_landing_gear_pub{ORB_ID(landing_gear)};
 	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};			/**< rate setpoint publication */
 
-	manual_control_setpoint_s	_manual_control_setpoint{};
 	vehicle_control_mode_s		_v_control_mode{};
 	vehicle_status_s		_vehicle_status{};