diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 972078432d..0bc2411a89 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2096,11 +2096,9 @@ Commander::run() } // abort autonomous mode and switch to position mode if sticks are moved significantly - if ((_param_rc_override.get() != 0) - && (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) + if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && !in_low_battery_failsafe && !_geofence_warning_action_on - && _manual_control.wantsOverride(_param_rc_override.get(), _param_com_rc_stick_ov.get(), _vehicle_control_mode, - !_status.rc_signal_lost)) { + && _manual_control.wantsOverride(_vehicle_control_mode, !_status.rc_signal_lost)) { if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state) == TRANSITION_CHANGED) { tune_positive(true); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index df3a43178c..2c042f4312 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -245,10 +245,8 @@ private: (ParamInt) _param_flight_uuid, (ParamInt) _param_takeoff_finished_action, - (ParamInt) _param_rc_override, (ParamInt) _param_rc_in_off, (ParamInt) _param_rc_arm_hyst, - (ParamFloat) _param_com_rc_stick_ov, (ParamInt) _param_fltmode_1, (ParamInt) _param_fltmode_2, @@ -361,7 +359,7 @@ private: manual_control_setpoint_s _last_manual_control_setpoint{}; manual_control_switches_s _manual_control_switches{}; manual_control_switches_s _last_manual_control_switches{}; - ManualControl _manual_control; + ManualControl _manual_control{this}; hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost int32_t _flight_mode_slots[manual_control_switches_s::MODE_SLOT_NUM] {}; uint8_t _last_manual_control_switches_arm_switch{manual_control_switches_s::SWITCH_POS_NONE}; diff --git a/src/modules/commander/ManualControl.cpp b/src/modules/commander/ManualControl.cpp index 8539c2f495..8f4f45be5c 100644 --- a/src/modules/commander/ManualControl.cpp +++ b/src/modules/commander/ManualControl.cpp @@ -56,24 +56,23 @@ void ManualControl::process(manual_control_setpoint_s &manual_control_setpoint) _manual_control_setpoint = manual_control_setpoint; } -bool ManualControl::wantsOverride(const int param_rc_override, const float param_com_rc_stick_ov, - const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available) +bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available) { - const bool override_auto_mode = (param_rc_override & OverrideBits::OVERRIDE_AUTO_MODE_BIT) + const bool override_auto_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_AUTO_MODE_BIT) && vehicle_control_mode.flag_control_auto_enabled; - const bool override_offboard_mode = (param_rc_override & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT) + const bool override_offboard_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_OFFBOARD_MODE_BIT) && vehicle_control_mode.flag_control_offboard_enabled; if (rc_available && (override_auto_mode || override_offboard_mode)) { - const float minimum_stick_change = .01f * param_com_rc_stick_ov; + const float minimum_stick_change = .01f * _param_com_rc_stick_ov.get(); const bool rpy_moved = (fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > minimum_stick_change) || (fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > minimum_stick_change) || (fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > minimum_stick_change); const bool throttle_moved = (fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) * 2.f > minimum_stick_change); - const bool use_throttle = !(param_rc_override & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT); + const bool use_throttle = !(_param_rc_override.get() & OverrideBits::OVERRIDE_IGNORE_THROTTLE_BIT); if (rpy_moved || (use_throttle && throttle_moved)) { return true; diff --git a/src/modules/commander/ManualControl.hpp b/src/modules/commander/ManualControl.hpp index c71540e633..c965028016 100644 --- a/src/modules/commander/ManualControl.hpp +++ b/src/modules/commander/ManualControl.hpp @@ -41,19 +41,20 @@ #pragma once +#include + #include #include #include -class ManualControl +class ManualControl : ModuleParams { public: - ManualControl() = default; - ~ManualControl() = default; + ManualControl(ModuleParams *parent) : ModuleParams(parent) {}; + ~ManualControl() override = default; void update(); - bool wantsOverride(const int param_rc_override, const float param_com_rc_stick_ov, - const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available); + bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available); //private: void process(manual_control_setpoint_s &manual_control_setpoint); @@ -61,4 +62,9 @@ public: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; manual_control_setpoint_s _manual_control_setpoint{}; manual_control_setpoint_s _last_manual_control_setpoint{}; + + DEFINE_PARAMETERS( + (ParamInt) _param_rc_override, + (ParamFloat) _param_com_rc_stick_ov + ) };