From d774bf3ea51700977e99e565c6a3b64533e6c4f7 Mon Sep 17 00:00:00 2001
From: Jaaaky <43599380+Jaaaky@users.noreply.github.com>
Date: Sun, 25 Aug 2019 11:54:16 +0300
Subject: [PATCH] AP_Mission: Better AUTO watchdog restore

This allows to run any commands following the last nav_waypoint
that may be required or change flight behavior for following wps
---
 libraries/AP_Mission/AP_Mission.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
index d32b145230..8409d96e36 100644
--- a/libraries/AP_Mission/AP_Mission.cpp
+++ b/libraries/AP_Mission/AP_Mission.cpp
@@ -225,7 +225,7 @@ void AP_Mission::update()
     }
 
     // save persistent waypoint_num for watchdog restore
-    hal.util->persistent_data.waypoint_num = _nav_cmd.index;
+    hal.util->persistent_data.waypoint_num = _prev_nav_cmd_wp_index != AP_MISSION_CMD_INDEX_NONE ? MIN(_nav_cmd.index,_prev_nav_cmd_wp_index+1) : _nav_cmd.index;
 
     // check if we have an active nav command
     if (!_flags.nav_cmd_loaded || _nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {