From f290a72d9c8f27b1e76cb9c48977cbef136672f6 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 23 Oct 2018 21:52:05 +1100
Subject: [PATCH] Tools: autotest: add test for camera trigger distance

---
 Tools/autotest/apmrover2.py             | 51 +++++++++++++++++++++++++
 Tools/autotest/rover-camera-mission.txt |  7 ++++
 2 files changed, 58 insertions(+)
 create mode 100644 Tools/autotest/rover-camera-mission.txt

diff --git a/Tools/autotest/apmrover2.py b/Tools/autotest/apmrover2.py
index a665f8098c..bd1bd0d8ab 100644
--- a/Tools/autotest/apmrover2.py
+++ b/Tools/autotest/apmrover2.py
@@ -736,6 +736,54 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
         if ex is not None:
             raise ex
 
+    def test_camera_mission_items(self):
+        self.context_push()
+        ex = None
+        try:
+            self.mavproxy.send('wp load %s\n' %
+                               os.path.join(testdir, "rover-camera-mission.txt"))
+            self.wait_ready_to_arm()
+            self.mavproxy.send('mode auto\n')
+            self.wait_mode('AUTO')
+            self.wait_ready_to_arm()
+            self.arm_vehicle()
+            prev_cf = None
+            while True:
+                cf = self.mav.recv_match(type='CAMERA_FEEDBACK', blocking=True)
+                if prev_cf is None:
+                    prev_cf = cf
+                    continue
+                dist_travelled = self.get_distance_int(prev_cf, cf)
+                prev_cf = cf
+                mc = self.mav.messages.get("MISSION_CURRENT", None)
+                if mc is None:
+                    continue
+                elif mc.seq == 2:
+                    expected_distance = 2
+                elif mc.seq == 4:
+                    expected_distance = 5
+                elif mc.seq ==5:
+                    break
+                else:
+                    continue
+                self.progress("Expected distance %f got %f" %
+                              (expected_distance, dist_travelled))
+                error = abs(expected_distance - dist_travelled)
+                # Rover moves at ~5m/s; we appear to do something at
+                # 5Hz, so we do see over a meter of error!
+                max_error = 1.5
+                if error > max_error:
+                    raise NotAchievedException("Camera distance error: %f (%f)" %
+                                               (error, max_error))
+
+            self.disarm_vehicle()
+        except Exception as e:
+            self.progress("Exception caught")
+            ex = e
+        self.context_pop()
+        if ex is not None:
+            raise ex
+
     def autotest(self):
         """Autotest APMrover2 in SITL."""
         self.check_test_syntax(test_file=os.path.realpath(__file__))
@@ -811,6 +859,9 @@ Brakes have negligible effect (with=%0.2fm without=%0.2fm delta=%0.2fm)
             self.run_test("Test AC Avoidance switch",
                           self.drive_fence_ac_avoidance)
 
+            self.run_test("Test Camera Mission Items",
+                          self.test_camera_mission_items)
+
             self.run_test("Download logs", lambda:
                           self.log_download(
                               self.buildlogs_path("APMrover2-log.bin")))
diff --git a/Tools/autotest/rover-camera-mission.txt b/Tools/autotest/rover-camera-mission.txt
new file mode 100644
index 0000000000..672f034264
--- /dev/null
+++ b/Tools/autotest/rover-camera-mission.txt
@@ -0,0 +1,7 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	40.071377	-105.229790	-0.110000	1
+1	0	0	206	2.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	40.071968	-105.229782	0.000000	1
+3	0	0	206	5.000000	0.000000	0.000000	0.000000	0.000000	0.000000	0.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	40.072365	-105.229774	0.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	40.072872	-105.229752	0.000000	1