From ee2d408edc2839585d0f7c26fb1f20bd47e66a37 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 16 Feb 2021 10:40:22 +0100 Subject: [PATCH] Commander: move rc availability to ManualControl --- src/modules/commander/Commander.cpp | 14 +++++++------- src/modules/commander/Commander.hpp | 2 -- src/modules/commander/ManualControl.cpp | 11 +++++++++-- src/modules/commander/ManualControl.hpp | 8 +++++++- 4 files changed, 23 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0bc2411a89..7ba1c2a327 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1991,9 +1991,6 @@ Commander::run() } } - _manual_control.update(); - _manual_control_setpoint = _manual_control._manual_control_setpoint; - /* start geofence result check */ _geofence_result_sub.update(&_geofence_result); @@ -2095,10 +2092,14 @@ Commander::run() _geofence_violated_prev = false; } + _manual_control.setRCAllowed(!_status_flags.rc_input_blocked); + _manual_control.update(); + _manual_control_setpoint = _manual_control._manual_control_setpoint; + // abort autonomous mode and switch to position mode if sticks are moved significantly if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && !in_low_battery_failsafe && !_geofence_warning_action_on - && _manual_control.wantsOverride(_vehicle_control_mode, !_status.rc_signal_lost)) { + && _manual_control.wantsOverride(_vehicle_control_mode)) { if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state) == TRANSITION_CHANGED) { tune_positive(true); @@ -2124,9 +2125,9 @@ Commander::run() } } + /* RC input check */ - if (!_status_flags.rc_input_blocked && _manual_control_setpoint.timestamp != 0 && - (hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) { + if (_manual_control.isRCAvailable()) { /* handle the case where RC signal was regained */ if (!_status_flags.rc_signal_found_once) { @@ -2687,7 +2688,6 @@ Commander::run() _last_condition_global_position_valid = _status_flags.condition_global_position_valid; _was_armed = _armed.armed; - _last_manual_control_setpoint = _manual_control_setpoint; arm_auth_update(now, params_updated || param_init_forced); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 2c042f4312..be17bb6237 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -195,7 +195,6 @@ private: (ParamInt) _param_com_hldl_reg_t, (ParamInt) _param_nav_rcl_act, - (ParamFloat) _param_com_rc_loss_t, (ParamFloat) _param_com_rcl_act_t, (ParamFloat) _param_com_home_h_t, @@ -356,7 +355,6 @@ private: unsigned int _leds_counter{0}; manual_control_setpoint_s _manual_control_setpoint{}; - 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{this}; diff --git a/src/modules/commander/ManualControl.cpp b/src/modules/commander/ManualControl.cpp index 8f4f45be5c..e7a57ff8be 100644 --- a/src/modules/commander/ManualControl.cpp +++ b/src/modules/commander/ManualControl.cpp @@ -32,6 +32,9 @@ ****************************************************************************/ #include "ManualControl.hpp" +#include + +using namespace time_literals; enum OverrideBits { OVERRIDE_AUTO_MODE_BIT = (1 << 0), @@ -41,6 +44,10 @@ enum OverrideBits { void ManualControl::update() { + _rc_available = _rc_allowed + && _last_manual_control_setpoint.timestamp != 0 + && (hrt_elapsed_time(&_last_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s)); + if (_manual_control_setpoint_sub.updated()) { manual_control_setpoint_s manual_control_setpoint; @@ -56,7 +63,7 @@ void ManualControl::process(manual_control_setpoint_s &manual_control_setpoint) _manual_control_setpoint = manual_control_setpoint; } -bool ManualControl::wantsOverride(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 override_auto_mode = (_param_rc_override.get() & OverrideBits::OVERRIDE_AUTO_MODE_BIT) && vehicle_control_mode.flag_control_auto_enabled; @@ -64,7 +71,7 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_ 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)) { + if (_rc_available && (override_auto_mode || override_offboard_mode)) { 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) diff --git a/src/modules/commander/ManualControl.hpp b/src/modules/commander/ManualControl.hpp index c965028016..46181e0f8b 100644 --- a/src/modules/commander/ManualControl.hpp +++ b/src/modules/commander/ManualControl.hpp @@ -53,8 +53,10 @@ public: ManualControl(ModuleParams *parent) : ModuleParams(parent) {}; ~ManualControl() override = default; + void setRCAllowed(const bool rc_allowed) { _rc_allowed = rc_allowed; } void update(); - bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode, const bool rc_available); + bool isRCAvailable() { return _rc_available; } + bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode); //private: void process(manual_control_setpoint_s &manual_control_setpoint); @@ -63,7 +65,11 @@ public: manual_control_setpoint_s _manual_control_setpoint{}; manual_control_setpoint_s _last_manual_control_setpoint{}; + bool _rc_allowed{false}; + bool _rc_available{false}; + DEFINE_PARAMETERS( + (ParamFloat) _param_com_rc_loss_t, (ParamInt) _param_rc_override, (ParamFloat) _param_com_rc_stick_ov )