From ec9f8998a7c369b2d923b3047af657215915120b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 25 Nov 2020 21:14:55 -0500 Subject: [PATCH] commander: limit mode switch re-evaluation logic on estimate validity changes - re-evaluating mode switches on altitude or position validity change can create confusing when also relying on mode changes via mavlink - limiting this logic to only apply when disarmed solves the original problem (respecting initial switch position) without introducing potential problems mid-flight - fixes https://github.com/PX4/PX4-Autopilot/issues/16235 --- src/modules/commander/Commander.cpp | 83 +++++++++++++++++------------ 1 file changed, 49 insertions(+), 34 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 9e5b86f617..f9e91510c4 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1567,7 +1567,9 @@ Commander::run() } /* update safety topic */ - if (_safety_sub.updated()) { + const bool safety_updated = _safety_sub.updated(); + + if (safety_updated) { const bool previous_safety_off = _safety.safety_off; if (_safety_sub.copy(&_safety)) { @@ -1780,7 +1782,7 @@ Commander::run() } // update manual_control_setpoint before geofence (which might check sticks or switches) - _manual_control_setpoint_sub.update(&_manual_control_setpoint); + const bool manual_control_setpoint_updated = _manual_control_setpoint_sub.update(&_manual_control_setpoint); /* start geofence result check */ @@ -2075,23 +2077,13 @@ Commander::run() tune_negative(true); } - /* evaluate the main state machine according to mode switches */ - bool first_rc_eval = (_last_manual_control_setpoint.timestamp == 0) && (_manual_control_setpoint.timestamp > 0); - transition_result_t main_res = set_main_state(status, &_status_changed); - - /* store last position lock state */ - _last_condition_local_altitude_valid = status_flags.condition_local_altitude_valid; - _last_condition_local_position_valid = status_flags.condition_local_position_valid; - _last_condition_global_position_valid = status_flags.condition_global_position_valid; - - /* play tune on mode change only if armed, blink LED always */ - if (main_res == TRANSITION_CHANGED || first_rc_eval) { - tune_positive(armed.armed); - _status_changed = true; - - } else if (main_res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - mavlink_log_critical(&mavlink_log_pub, "Switching to this mode is currently not possible"); + if (manual_control_setpoint_updated || safety_updated) { + // evaluate the main state machine according to mode switches + if (set_main_state(status, &_status_changed) == TRANSITION_CHANGED) { + // play tune on mode change only if armed, blink LED always + tune_positive(armed.armed); + _status_changed = true; + } } /* check throttle kill switch */ @@ -2493,6 +2485,11 @@ Commander::run() _status_changed = false; + /* store last position lock state */ + _last_condition_local_altitude_valid = status_flags.condition_local_altitude_valid; + _last_condition_local_position_valid = status_flags.condition_local_position_valid; + _last_condition_global_position_valid = status_flags.condition_global_position_valid; + arm_auth_update(now, params_updated || param_init_forced); px4_indicate_external_reset_lockout(LockoutComponent::Commander, armed.armed); @@ -2705,19 +2702,19 @@ Commander::set_main_state_override_on(const vehicle_status_s &status_local, bool transition_result_t Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed) { - /* set main state according to RC switches */ - transition_result_t res = TRANSITION_DENIED; + if ((_manual_control_setpoint.timestamp == 0) + || (_manual_control_setpoint.timestamp == _last_manual_control_setpoint.timestamp)) { + + // no manual control or no update -> nothing changed + return TRANSITION_NOT_CHANGED; + } // Note: even if status_flags.offboard_control_set_by_command is set // we want to allow rc mode change to take precidence. This is a safety // feature, just in case offboard control goes crazy. - const bool altitude_got_valid = (!_last_condition_local_altitude_valid && status_flags.condition_local_altitude_valid); - const bool lpos_got_valid = (!_last_condition_local_position_valid && status_flags.condition_local_position_valid); - const bool gpos_got_valid = (!_last_condition_global_position_valid && status_flags.condition_global_position_valid); - const bool first_time_rc = (_last_manual_control_setpoint.timestamp == 0); - const bool rc_values_updated = (_last_manual_control_setpoint.timestamp != _manual_control_setpoint.timestamp); - const bool some_switch_changed = + // only switch mode based on RC switch if necessary to also allow mode switching via MAVLink + bool should_evaluate_rc_mode_switch = (_last_manual_control_setpoint.offboard_switch != _manual_control_setpoint.offboard_switch) || (_last_manual_control_setpoint.return_switch != _manual_control_setpoint.return_switch) || (_last_manual_control_setpoint.mode_switch != _manual_control_setpoint.mode_switch) @@ -2729,12 +2726,27 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed || (_last_manual_control_setpoint.stab_switch != _manual_control_setpoint.stab_switch) || (_last_manual_control_setpoint.man_switch != _manual_control_setpoint.man_switch); - // only switch mode based on RC switch if necessary to also allow mode switching via MAVLink - const bool should_evaluate_rc_mode_switch = first_time_rc - || altitude_got_valid - || lpos_got_valid - || gpos_got_valid - || (rc_values_updated && some_switch_changed); + + if (status_local.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + // if already armed don't evaluate first time RC + if (_last_manual_control_setpoint.timestamp == 0) { + should_evaluate_rc_mode_switch = false; + _last_manual_control_setpoint = _manual_control_setpoint; + } + + } else { + // not armed + if (!should_evaluate_rc_mode_switch) { + // to respect initial switch position (eg POSCTL) force RC switch re-evaluation if estimates become valid + const bool altitude_got_valid = (!_last_condition_local_altitude_valid && status_flags.condition_local_altitude_valid); + const bool lpos_got_valid = (!_last_condition_local_position_valid && status_flags.condition_local_position_valid); + const bool gpos_got_valid = (!_last_condition_global_position_valid && status_flags.condition_global_position_valid); + + if (altitude_got_valid || lpos_got_valid || gpos_got_valid) { + should_evaluate_rc_mode_switch = true; + } + } + } if (!should_evaluate_rc_mode_switch) { @@ -2742,7 +2754,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed // if the system now later enters an autonomous state the pilot can move // the sticks to break out of the autonomous state - if (!_geofence_warning_action_on + if (!status.rc_signal_lost && !_geofence_warning_action_on && (_internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL || _internal_state.main_state == commander_state_s::MAIN_STATE_ALTCTL || _internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL || @@ -2767,6 +2779,9 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed // the desired mode reset_posvel_validity(changed); + /* set main state according to RC switches */ + transition_result_t res = TRANSITION_DENIED; + /* offboard switch overrides main switch */ if (_manual_control_setpoint.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &_internal_state);