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_pub{ORB_ID(landing_gear)}; uORB::Publication _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{};