From b8c019985359d1557afcb67bcaf926bb2612e764 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 7 Aug 2018 11:25:23 +1000
Subject: [PATCH] Tools: autotest: add test for copter's NAV_DELAY function

---
 Tools/autotest/arducopter.py        | 53 +++++++++++++++++++++++++++++
 Tools/autotest/copter_nav_delay.txt |  6 ++++
 2 files changed, 59 insertions(+)
 create mode 100644 Tools/autotest/copter_nav_delay.txt

diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 9f6df57fc9..0ff1ce222a 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -1244,6 +1244,57 @@ class AutoTestCopter(AutoTest):
         if ex is not None:
             raise ex
 
+    def fly_nav_delay(self):
+        """fly a simple mission that has a delay in it"""
+
+        num_wp = self.load_mission("copter_nav_delay.txt")
+
+        self.mavproxy.send('mode loiter\n')
+        self.mav.wait_heartbeat()
+        self.wait_mode('LOITER')
+        self.progress("Waiting reading for arm")
+        self.wait_ready_to_arm()
+
+        self.context_push();
+
+        ex = None
+        try:
+            self.arm_vehicle()
+            self.mavproxy.send('mode auto\n')
+            self.wait_mode('AUTO')
+            self.set_rc(3, 1600)
+            count_start = -1
+            count_stop = -1
+            tstart = self.get_sim_time()
+            while self.armed(): # we RTL at end of mission
+                now = self.get_sim_time()
+                if now - tstart > 120:
+                    raise AutoTestTimeoutException()
+                m = self.mav.recv_match(type='MISSION_CURRENT', blocking=True)
+                self.progress("MISSION_CURRENT.seq=%u" % (m.seq,))
+                if m.seq == 3:
+                    self.progress("At delay item")
+                    if count_start == -1:
+                        count_start = now
+                if m.seq > 3:
+                    if count_stop == -1:
+                        count_stop = now
+            calculated_delay = count_stop - count_start
+            want_delay = 59 # should reflect what's in the mission file
+            self.progress("Stopped for %u seconds (want >=%u seconds)" %
+                          (calculated_delay, want_delay))
+            if calculated_delay < want_delay:
+                raise NotAchievedException()
+
+        except Exception as e:
+            self.progress("Exception caught")
+            ex = e
+
+        self.set_rc(3, 1000)
+
+        if ex is not None:
+            raise ex
+
     def test_setting_modes_via_modeswitch(self):
         self.context_push();
         ex = None
@@ -1405,6 +1456,8 @@ class AutoTestCopter(AutoTest):
             self.set_rc_default()
             self.set_rc(3, 1000)
 
+            self.run_test("Fly Nav Delay", self.fly_nav_delay)
+
             self.run_test("Test submode change",
                           self.fly_guided_change_submode)
 
diff --git a/Tools/autotest/copter_nav_delay.txt b/Tools/autotest/copter_nav_delay.txt
new file mode 100644
index 0000000000..3acc161a49
--- /dev/null
+++ b/Tools/autotest/copter_nav_delay.txt
@@ -0,0 +1,6 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.000000	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.362968	149.165146	20.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.362881	149.165222	20.000000	1
+3	0	0	93	60.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+4	0	0	20	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1