From 27ee1c82a2087ecec157d3f296f54a4c36e84129 Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Sun, 23 Feb 2020 09:52:57 +0000 Subject: [PATCH 1/2] AP_Mission: Added force resume for when MIS_RESTART=1 --- libraries/AP_Mission/AP_Mission.cpp | 3 ++- libraries/AP_Mission/AP_Mission.h | 5 +++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index b006e08b50d06..f5097decc66aa 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -163,10 +163,11 @@ bool AP_Mission::starts_with_takeoff_cmd() /// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start() void AP_Mission::start_or_resume() { - if (_restart) { + if (_restart == 1 && !_force_resume) { start(); } else { resume(); + _force_resume = false; } } diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index a48575307f74e..5e6b22bfe9ac2 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -305,6 +305,7 @@ class AP_Mission { _flags.nav_cmd_loaded = false; _flags.do_cmd_loaded = false; _flags.in_landing_sequence = false; + _force_resume = false; } // get singleton instance @@ -479,6 +480,9 @@ class AP_Mission { // set in_landing_sequence flag void set_in_landing_sequence_flag(bool flag) { _flags.in_landing_sequence = flag; } + // force mission to resume when start_or_resume() is called + void set_force_resume(bool force_resume) { _force_resume = force_resume; } + // get a reference to the AP_Mission semaphore, allowing an external caller to lock the // storage while working with multiple waypoints HAL_Semaphore &get_semaphore(void) { @@ -582,6 +586,7 @@ class AP_Mission { uint16_t _prev_nav_cmd_id; // id of the previous "navigation" command. (WAYPOINT, LOITER_TO_ALT, ect etc) uint16_t _prev_nav_cmd_index; // index of the previous "navigation" command. Rarely used which is why we don't store the whole command uint16_t _prev_nav_cmd_wp_index; // index of the previous "navigation" command that contains a waypoint. Rarely used which is why we don't store the whole command + bool _force_resume; // when set true it forces mission to resume irrespective of MIS_RESTART param. // jump related variables struct jump_tracking_struct { From 53c93b19b3504b86e0c4a1f80b9c9893a546d28e Mon Sep 17 00:00:00 2001 From: Gone4Dirt Date: Sun, 23 Feb 2020 09:54:30 +0000 Subject: [PATCH 2/2] Plane: force mission resume on RTL when DO_LAND_START in mission --- ArduPlane/ArduPlane.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index f14c03dcee066..49d8a791a7c11 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -467,6 +467,7 @@ void Plane::update_navigation() // we've reached the RTL point, see if we have a landing sequence if (mission.jump_to_landing_sequence()) { // switch from RTL -> AUTO + mission.set_force_resume(true); set_mode(mode_auto, ModeReason::UNKNOWN); } @@ -479,6 +480,7 @@ void Plane::update_navigation() // Go directly to the landing sequence if (mission.jump_to_landing_sequence()) { // switch from RTL -> AUTO + mission.set_force_resume(true); set_mode(mode_auto, ModeReason::UNKNOWN); }