From 6b8840a01b87dce6958cd7907b913eb88f5ddb61 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Jul 2021 17:13:11 +0200 Subject: [PATCH] commander: fix switch to ALTCTL when RC regained This fixes the case where we sometimes switch to altitude control instead of position control when RC is regained. What happens is that we detect that the pilot wants to take over control right when RC comes back. This means that we try to go in position control in main_state_transition, however, we are already in position control because we come back from the failsafe state. The result of main_state_transition is then TRANSITION_NOT_CHANGED, and therefore we "fall back" to altitude control even though being already in position control would have been fine. This fix checks the return result of main_state_transition correctly and only reacts to TRANSITION_CHANGED and TRANSITION_DENIED but ignores TRANSITION_NOT_CHANGED. --- src/modules/commander/Commander.cpp | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7638384739..12c1ed628d 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2392,21 +2392,28 @@ Commander::run() 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)) { - if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, - _internal_state) == TRANSITION_CHANGED) { + const transition_result_t posctl_result = + main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state); + + if (posctl_result == TRANSITION_CHANGED) { tune_positive(true); mavlink_log_info(&_mavlink_log_pub, "Pilot took over position control using sticks\t"); events::send(events::ID("commander_rc_override_pos"), events::Log::Info, "Pilot took over position control using sticks"); _status_changed = true; - } else if (main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, - _internal_state) == TRANSITION_CHANGED) { - tune_positive(true); - mavlink_log_info(&_mavlink_log_pub, "Pilot took over altitude control using sticks\t"); - events::send(events::ID("commander_rc_override_alt"), events::Log::Info, - "Pilot took over altitude control using sticks"); - _status_changed = true; + } else if (posctl_result == TRANSITION_DENIED) { + // If transition to POSCTL was denied, then we can try again with ALTCTL. + const transition_result_t altctl_result = + main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, _internal_state); + + if (altctl_result == TRANSITION_CHANGED) { + tune_positive(true); + mavlink_log_info(&_mavlink_log_pub, "Pilot took over altitude control using sticks\t"); + events::send(events::ID("commander_rc_override_alt"), events::Log::Info, + "Pilot took over altitude control using sticks"); + _status_changed = true; + } } }