From 659ba8324bee99cc6fa55b8b64865f3f4aaec91a Mon Sep 17 00:00:00 2001 From: CharlieBurge Date: Wed, 1 Mar 2023 23:23:06 +0000 Subject: [PATCH] Copter: report success if already paused or resumed --- ArduCopter/mode_auto.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 4402016683..8453f5d3ec 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -2235,12 +2235,16 @@ bool ModeAuto::verify_nav_attitude_time(const AP_Mission::Mission_Command& cmd) // pause - Prevent aircraft from progressing along the track bool ModeAuto::pause() { - // do not pause if already paused or not in the WP sub mode or already reached to the destination - if(wp_nav->paused() || _mode != SubMode::WP || wp_nav->reached_wp_destination()) { + // do not pause if not in the WP sub mode or already reached to the destination + if(_mode != SubMode::WP || wp_nav->reached_wp_destination()) { return false; } - wp_nav->set_pause(); + // do not pause if already paused + if (!wp_nav->paused()) { + wp_nav->set_pause(); + } + return true; } @@ -2248,11 +2252,10 @@ bool ModeAuto::pause() bool ModeAuto::resume() { // do not resume if not paused before - if(!wp_nav->paused()) { - return false; + if(wp_nav->paused()) { + wp_nav->set_resume(); } - wp_nav->set_resume(); return true; }