diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index def762663e..d383729ae1 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2094,21 +2094,6 @@ Commander::run() _geofence_violated_prev = false; } - _manual_control.setRCAllowed(!_status_flags.rc_input_blocked); - _manual_control.update(); - - // 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)) { - if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, - &_internal_state) == TRANSITION_CHANGED) { - tune_positive(true); - mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks"); - _status_changed = true; - } - } - /* Check for mission flight termination */ if (_armed.armed && _mission_result_sub.get().flight_termination && !_status_flags.circuit_breaker_flight_termination_disabled) { @@ -2126,9 +2111,10 @@ Commander::run() } } + // Manual control input handling + _manual_control.setRCAllowed(!_status_flags.rc_input_blocked); - /* RC input check */ - if (_manual_control.isRCAvailable()) { + if (_manual_control.update()) { /* handle the case where RC signal was regained */ if (!_status_flags.rc_signal_found_once) { @@ -2168,6 +2154,18 @@ Commander::run() } } + // 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)) { + if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, + &_internal_state) == TRANSITION_CHANGED) { + tune_positive(true); + mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks"); + _status_changed = true; + } + } + if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) { // handle landing gear switch if configured and in a manual mode @@ -2233,8 +2231,9 @@ Commander::run() } /* no else case: do not change lockdown flag in unconfigured case */ + } - } else { + if (!_manual_control.isRCAvailable()) { // set RC lost if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) { // ignore RC lost during calibration diff --git a/src/modules/commander/ManualControl.cpp b/src/modules/commander/ManualControl.cpp index 193cfd8361..aaedcc97e1 100644 --- a/src/modules/commander/ManualControl.cpp +++ b/src/modules/commander/ManualControl.cpp @@ -42,19 +42,27 @@ enum OverrideBits { OVERRIDE_IGNORE_THROTTLE_BIT = (1 << 2) }; -void ManualControl::update() +bool 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)); + bool ret = false; - if (_manual_control_setpoint_sub.updated()) { + if (_rc_allowed && _manual_control_setpoint_sub.updated()) { manual_control_setpoint_s manual_control_setpoint; if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { + _rc_available = true; process(manual_control_setpoint); } + + ret = true; } + + if (_last_manual_control_setpoint.timestamp == 0 + || (hrt_elapsed_time(&_last_manual_control_setpoint.timestamp) > (_param_com_rc_loss_t.get() * 1_s))) { + _rc_available = false; + } + + return ret && _rc_available; } void ManualControl::process(manual_control_setpoint_s &manual_control_setpoint) diff --git a/src/modules/commander/ManualControl.hpp b/src/modules/commander/ManualControl.hpp index b174fba7c5..80f49c09d1 100644 --- a/src/modules/commander/ManualControl.hpp +++ b/src/modules/commander/ManualControl.hpp @@ -57,7 +57,12 @@ public: ~ManualControl() override = default; void setRCAllowed(const bool rc_allowed) { _rc_allowed = rc_allowed; } - void update(); + + /** + * Check for manual control input changes and process them + * @return true if there was new data + */ + bool update(); bool isRCAvailable() { return _rc_available; } bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode); bool wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,