From aa7d9603a67849a5d9bb7e2c0fdbcf983a67d415 Mon Sep 17 00:00:00 2001 From: Timothy Scott <timothy@auterion.com> Date: Tue, 16 Jul 2019 17:03:43 +0200 Subject: [PATCH 1/7] Added logic to transition to manual after auto landing or RTL --- src/modules/commander/Commander.cpp | 9 +++++++++ src/modules/navigator/rtl.cpp | 11 ++++++++--- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 3d8aa5df6c5d..1382ef079b25 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1841,6 +1841,15 @@ Commander::run() tune_mission_ok(true); } } + + // TODO Transition into the last state, not just directly to Manual + const bool in_right_state = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || + internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION; + + if (in_right_state && mission_result.finished && land_detector.landed && !armed.armed) { + main_state_transition(status, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); + } } } diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 2a05d217849f..131100adff75 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -95,9 +95,14 @@ RTL::on_activation() void RTL::on_active() { - if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { - advance_rtl(); - set_rtl_item(); + if (_rtl_state != RTL_STATE_LANDED) { + if (is_mission_item_reached()) { + advance_rtl(); + set_rtl_item(); + } + + } else { + _navigator->get_mission_result()->finished = true; } } From b2e4eab9b8a947c8f1328916057c12435c220dd4 Mon Sep 17 00:00:00 2001 From: Timothy Scott <timothy@auterion.com> Date: Wed, 17 Jul 2019 11:21:20 +0200 Subject: [PATCH 2/7] Keep track of last non-auto state and revert to it --- src/modules/commander/Commander.cpp | 9 +++------ src/modules/commander/state_machine_helper.cpp | 18 ++++++++++++++++++ src/modules/commander/state_machine_helper.h | 3 +++ 3 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1382ef079b25..61eb8341c265 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1843,12 +1843,9 @@ Commander::run() } // TODO Transition into the last state, not just directly to Manual - const bool in_right_state = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL || - internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND || - internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION; - - if (in_right_state && mission_result.finished && land_detector.landed && !armed.armed) { - main_state_transition(status, commander_state_s::MAIN_STATE_MANUAL, status_flags, &internal_state); + if (is_auto_state(internal_state.main_state) && mission_result.finished && land_detector.landed && !armed.armed + && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) { + main_state_transition(status, last_non_auto_state, status_flags, &internal_state); } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 06396cb82159..b5d85b7ebf68 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -87,6 +87,8 @@ const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = { static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately +main_state_t last_non_auto_state = commander_state_s::MAIN_STATE_MAX; + void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed, const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act, uint8_t auto_recovery_nav_state); @@ -375,6 +377,11 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai if (ret == TRANSITION_CHANGED) { if (internal_state->main_state != new_main_state) { + // If transitioning from non-auto to auto state, save the last non-auto state + if (!is_auto_state(internal_state->main_state)) { + last_non_auto_state = internal_state->main_state; + } + internal_state->main_state = new_main_state; internal_state->timestamp = hrt_absolute_time(); @@ -1212,3 +1219,14 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta break; } } + +bool is_auto_state(main_state_t state) +{ + return state == commander_state_s::MAIN_STATE_AUTO_RTL + || state == commander_state_s::MAIN_STATE_AUTO_LAND + || state == commander_state_s::MAIN_STATE_AUTO_MISSION + || state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF + || state == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET + || state == commander_state_s::MAIN_STATE_AUTO_LOITER + || state == commander_state_s::MAIN_STATE_AUTO_PRECLAND; +} diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 3ea6711b91a4..31d4d136dfac 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -147,4 +147,7 @@ void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &sta const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning, const low_battery_action_t low_bat_action); +extern main_state_t last_non_auto_state; +bool is_auto_state(main_state_t state); + #endif /* STATE_MACHINE_HELPER_H_ */ From 514c8d9f7b45f19d8068a2778bcb0aebdad33dbd Mon Sep 17 00:00:00 2001 From: Timothy Scott <timothy@auterion.com> Date: Thu, 18 Jul 2019 13:30:00 +0200 Subject: [PATCH 3/7] Removed leftover TODO --- src/modules/commander/Commander.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 61eb8341c265..475cb0b2a2fa 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1842,7 +1842,6 @@ Commander::run() } } - // TODO Transition into the last state, not just directly to Manual if (is_auto_state(internal_state.main_state) && mission_result.finished && land_detector.landed && !armed.armed && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) { main_state_transition(status, last_non_auto_state, status_flags, &internal_state); From 6ff6cf932e71c0f5237d905364ed11a54eadcda2 Mon Sep 17 00:00:00 2001 From: Timothy Scott <timothy@auterion.com> Date: Thu, 18 Jul 2019 17:14:12 +0200 Subject: [PATCH 4/7] Removed landing check --- src/modules/commander/Commander.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 475cb0b2a2fa..1b83d58a65d7 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1842,7 +1842,11 @@ Commander::run() } } - if (is_auto_state(internal_state.main_state) && mission_result.finished && land_detector.landed && !armed.armed + // TODO: Determine what to do with `was_armed`. It updates one loop iteration before this code + // is reached, so it's not correct here. + // One possible solution: Keep track of time of last arm-disarm transition, and see if it's + // less than 1 second ago (or less than 0.1 seconds, or whatever time works) + if (is_auto_state(internal_state.main_state) && mission_result.finished && !armed.armed // && was_armed && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) { main_state_transition(status, last_non_auto_state, status_flags, &internal_state); } From 285dcd77890d0c73d70430fba300b0f2a96cf59f Mon Sep 17 00:00:00 2001 From: Timothy Scott <timothy@auterion.com> Date: Fri, 19 Jul 2019 11:42:37 +0200 Subject: [PATCH 5/7] Added check for recent disarming. --- src/modules/commander/Commander.cpp | 24 +++++++++++++++++------- src/modules/commander/Commander.hpp | 3 +++ src/modules/navigator/rtl.cpp | 1 + 3 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 1b83d58a65d7..aaa7a087aa90 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -118,6 +118,7 @@ static constexpr uint64_t OFFBOARD_TIMEOUT = 500_ms; static constexpr uint64_t HOTPLUG_SENS_TIMEOUT = 8_s; /**< wait for hotplug sensors to come online for upto 8 seconds */ static constexpr uint64_t PRINT_MODE_REJECT_INTERVAL = 500_ms; static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL = 500_ms; +static constexpr uint64_t MISSION_FINISH_DISARM_TIMEOUT = 500_ms; /* Mavlink log uORB handle */ static orb_advert_t mavlink_log_pub = nullptr; @@ -1842,17 +1843,26 @@ Commander::run() } } - // TODO: Determine what to do with `was_armed`. It updates one loop iteration before this code - // is reached, so it's not correct here. - // One possible solution: Keep track of time of last arm-disarm transition, and see if it's - // less than 1 second ago (or less than 0.1 seconds, or whatever time works) - if (is_auto_state(internal_state.main_state) && mission_result.finished && !armed.armed // && was_armed - && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) { - main_state_transition(status, last_non_auto_state, status_flags, &internal_state); + if (mission_result.finished) { + _last_mission_result_finished = mission_result.timestamp; } } } + // When auto mission/rtl/land is finished, transition back to the most recently-used manual mode. + // This check is done using time because we cannot guarantee that the disarm will happen first or + // the mission_result message will come in first. + const bool disarmed_and_mission_finished = !armed.armed + && hrt_elapsed_time(&last_disarmed_timestamp) < MISSION_FINISH_DISARM_TIMEOUT + && hrt_elapsed_time(&_last_mission_result_finished) < MISSION_FINISH_DISARM_TIMEOUT; + + if (disarmed_and_mission_finished && is_auto_state(internal_state.main_state) + && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) { + // This branch will only happen once per mission finish because, after transitioning to the non-auto + // state, is_auto_state(internal_state.main_state) will be false. + main_state_transition(status, last_non_auto_state, status_flags, &internal_state); + } + /* start geofence result check */ geofence_result_sub.update(&geofence_result); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index a32ac7df6e10..f0b7b0fa61c9 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -251,6 +251,9 @@ class Commander : public ModuleBase<Commander>, public ModuleParams uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)}; + // Time of the last received `mission_result` message where finished == True + hrt_abstime _last_mission_result_finished{0}; + hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 131100adff75..334d39a62c05 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -103,6 +103,7 @@ RTL::on_active() } else { _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); } } From 20ce88c5c332486ec62e2849743fbd03e384a9c4 Mon Sep 17 00:00:00 2001 From: Timothy Scott <timothy@auterion.com> Date: Fri, 18 Oct 2019 10:29:49 +0200 Subject: [PATCH 6/7] Removed timing issue --- src/modules/commander/Commander.cpp | 21 ++++++--------------- src/modules/commander/Commander.hpp | 3 --- 2 files changed, 6 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index aaa7a087aa90..8ddf70b4457f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1842,24 +1842,15 @@ Commander::run() tune_mission_ok(true); } } - - if (mission_result.finished) { - _last_mission_result_finished = mission_result.timestamp; - } } } - // When auto mission/rtl/land is finished, transition back to the most recently-used manual mode. - // This check is done using time because we cannot guarantee that the disarm will happen first or - // the mission_result message will come in first. - const bool disarmed_and_mission_finished = !armed.armed - && hrt_elapsed_time(&last_disarmed_timestamp) < MISSION_FINISH_DISARM_TIMEOUT - && hrt_elapsed_time(&_last_mission_result_finished) < MISSION_FINISH_DISARM_TIMEOUT; - - if (disarmed_and_mission_finished && is_auto_state(internal_state.main_state) - && last_non_auto_state != commander_state_s::MAIN_STATE_MAX) { - // This branch will only happen once per mission finish because, after transitioning to the non-auto - // state, is_auto_state(internal_state.main_state) will be false. + const bool just_disarmed = !armed.armed && was_armed; + const bool just_finished_auto_mission = is_auto_state(internal_state.main_state) && _mission_result_sub.get().finished; + const bool last_state_valid = last_non_auto_state != commander_state_s::MAIN_STATE_MAX; + + if(just_disarmed && land_detector.landed && just_finished_auto_mission && last_state_valid){ + PX4_INFO("Just finished auto mission, transitioning back to last manual mode."); main_state_transition(status, last_non_auto_state, status_flags, &internal_state); } diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index f0b7b0fa61c9..a32ac7df6e10 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -251,9 +251,6 @@ class Commander : public ModuleBase<Commander>, public ModuleParams uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)}; - // Time of the last received `mission_result` message where finished == True - hrt_abstime _last_mission_result_finished{0}; - hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; From c945a025ca06f667f110de7a0ef775ea0f2ddca3 Mon Sep 17 00:00:00 2001 From: Timothy Scott <timothy@auterion.com> Date: Fri, 25 Oct 2019 09:21:37 +0200 Subject: [PATCH 7/7] Changed to only switch back to manual mode after RTL, instead of after any auto mode --- src/modules/commander/Commander.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 8ddf70b4457f..55def64b1704 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1846,10 +1846,11 @@ Commander::run() } const bool just_disarmed = !armed.armed && was_armed; - const bool just_finished_auto_mission = is_auto_state(internal_state.main_state) && _mission_result_sub.get().finished; + const bool just_finished_rtl = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL + && _mission_result_sub.get().finished; const bool last_state_valid = last_non_auto_state != commander_state_s::MAIN_STATE_MAX; - if(just_disarmed && land_detector.landed && just_finished_auto_mission && last_state_valid){ + if(just_disarmed && land_detector.landed && just_finished_rtl && last_state_valid){ PX4_INFO("Just finished auto mission, transitioning back to last manual mode."); main_state_transition(status, last_non_auto_state, status_flags, &internal_state); }