From de95e3d274edb8a845c1bd04c89a821c8dfed8d4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 6 Dec 2019 15:23:21 +0100 Subject: [PATCH] commander: remove manual mode before geofence handling --- src/modules/commander/Commander.cpp | 27 ++++++--------------------- 1 file changed, 6 insertions(+), 21 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index cac0d44579..20e151bb10 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -141,8 +141,6 @@ static struct safety_s safety = {}; static int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM]; static struct commander_state_s internal_state = {}; -static uint8_t main_state_before_rtl = commander_state_s::MAIN_STATE_MAX; - static manual_control_setpoint_s sp_man = {}; ///< the current manual control setpoint static manual_control_setpoint_s _last_sp_man = {}; ///< the manual control setpoint valid at the last mode switch static uint8_t _last_sp_man_arm_switch = 0; @@ -1773,15 +1771,10 @@ Commander::run() _auto_disarm_killed.set_state_and_update(false, hrt_absolute_time()); } - - if (!_geofence_warning_action_on) { - // store the last good main_state when not in an navigation - // hold state - main_state_before_rtl = internal_state.main_state; - - } else if (internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL - && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER - && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { + if (_geofence_warning_action_on + && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL + && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LOITER + && internal_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) { // reset flag again when we switched out of it _geofence_warning_action_on = false; } @@ -1938,23 +1931,15 @@ Commander::run() // abort auto mode or geofence reaction if sticks are moved significantly // but only if not in a low battery handling action - const bool not_in_low_battery_reaction = _battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL; + const bool low_battery_reaction = _battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL; const bool is_rotary_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - 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_RTL || 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 && is_rotary_wing && not_in_low_battery_reaction - && (in_auto_mode || (_geofence_warning_action_on && manual_mode_before_geofence))) { + if (rc_override != 0 && is_rotary_wing && !low_battery_reaction && !_geofence_warning_action_on && in_auto_mode) { // 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) ||