diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7d3296d3b3..9ea2231aea 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1822,17 +1822,23 @@ Commander::run() _geofence_violated_prev = false; } - // revert geofence failsafe transition if sticks are moved and we were previously in a manual mode + // abort auto mode or geofence reaction if sticks are moved significantly // but only if not in a low battery handling action - if (rc_override != 0 && (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL) - && (_geofence_warning_action_on - && (main_state_before_rtl == commander_state_s::MAIN_STATE_MANUAL || - main_state_before_rtl == commander_state_s::MAIN_STATE_ALTCTL || - main_state_before_rtl == commander_state_s::MAIN_STATE_POSCTL || - main_state_before_rtl == commander_state_s::MAIN_STATE_ACRO || - main_state_before_rtl == commander_state_s::MAIN_STATE_RATTITUDE || - main_state_before_rtl == commander_state_s::MAIN_STATE_STAB))) { + const bool not_in_low_battery_reaction = _battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL; + const bool manual_mode_before_geofence = + main_state_before_rtl == commander_state_s::MAIN_STATE_MANUAL || + main_state_before_rtl == commander_state_s::MAIN_STATE_ALTCTL || + main_state_before_rtl == commander_state_s::MAIN_STATE_POSCTL || + main_state_before_rtl == commander_state_s::MAIN_STATE_ACRO || + main_state_before_rtl == commander_state_s::MAIN_STATE_RATTITUDE || + main_state_before_rtl == commander_state_s::MAIN_STATE_STAB; + const bool in_auto_mode = + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER; + if (rc_override != 0 && not_in_low_battery_reaction + && (in_auto_mode || (_geofence_warning_action_on && manual_mode_before_geofence))) { // transition to previous state if sticks are touched if ((_last_sp_man.timestamp != sp_man.timestamp) && ((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) || @@ -1846,27 +1852,6 @@ Commander::run() } } - // abort landing or auto or loiter if sticks are moved significantly - // but only if not in a low battery handling action - if (rc_override != 0 && (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL) && - (internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || - internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION || - internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER)) { - // transition to previous state if sticks are touched - - if ((_last_sp_man.timestamp != sp_man.timestamp) && - ((fabsf(sp_man.x - _last_sp_man.x) > min_stick_change) || - (fabsf(sp_man.y - _last_sp_man.y) > min_stick_change) || - (fabsf(sp_man.z - _last_sp_man.z) > min_stick_change) || - (fabsf(sp_man.r - _last_sp_man.r) > min_stick_change))) { - - // revert to position control in any case - main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &internal_state); - mavlink_log_critical(&mavlink_log_pub, "Autopilot off! Returning control to pilot"); - } - } - - /* Check for mission flight termination */ if (armed.armed && _mission_result_sub.get().flight_termination && !status_flags.circuit_breaker_flight_termination_disabled) {