From 5abf29d93c1677fe6f14184669b13654708552df Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 16 Feb 2021 13:02:14 -0500 Subject: [PATCH] commander: auto TAKEOFF to LOITER/MISSION minor fix to avoid failsafe confusion - base on nav_state rather than main_state --- src/modules/commander/Commander.cpp | 41 +++++++++++++---------------- 1 file changed, 18 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 69cf6e66f1..c85c62b7c8 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1944,11 +1944,12 @@ Commander::run() } /* start mission result check */ - const auto prev_mission_instance_count = _mission_result_sub.get().instance_count; - - if (_mission_result_sub.update()) { + if (_mission_result_sub.updated()) { const mission_result_s &mission_result = _mission_result_sub.get(); + const auto prev_mission_instance_count = mission_result.instance_count; + _mission_result_sub.update(); + // if mission_result is valid for the current mission const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp) && (mission_result.instance_count > 0); @@ -1956,7 +1957,6 @@ Commander::run() _status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid; if (mission_result_ok) { - if (_status.mission_failure != mission_result.failure) { _status.mission_failure = mission_result.failure; _status_changed = true; @@ -1984,6 +1984,20 @@ Commander::run() } } } + + // Transition main state to loiter or auto-mission after takeoff is completed. + if (_armed.armed && !_land_detector.landed + && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) + && (mission_result.timestamp >= _status.nav_state_timestamp) + && mission_result.finished) { + + if ((_param_takeoff_finished_action.get() == 1) && _status_flags.condition_auto_mission_available) { + main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state); + + } else { + main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state); + } + } } // update manual_control_setpoint before geofence (which might check sticks or switches) @@ -2381,25 +2395,6 @@ Commander::run() } } - /* Reset main state to loiter or auto-mission after takeoff is completed. - * Sometimes, the mission result topic is outdated and the mission is still signaled - * as finished even though we only just started with the takeoff. Therefore, we also - * check the timestamp of the mission_result topic. */ - if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF - && (_mission_result_sub.get().timestamp >= _internal_state.timestamp) - && _mission_result_sub.get().finished) { - - const bool mission_available = (_mission_result_sub.get().timestamp > _boot_timestamp) - && (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid; - - if ((_param_takeoff_finished_action.get() == 1) && mission_available) { - main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state); - - } else { - main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state); - } - } - /* check if we are disarmed and there is a better mode to wait in */ if (!_armed.armed) { /* if there is no radio control but GPS lock the user might want to fly using