From 7bf1a3e978f00a63c70b777c71b97e6150f33b76 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 5 Aug 2019 21:09:07 +1000
Subject: [PATCH] Tools: autotest: add test for Plane reposition using terrain
 offset

---
 Tools/autotest/arduplane.py | 58 +++++++++++++++++++++++++++++++++++--
 1 file changed, 56 insertions(+), 2 deletions(-)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index 0acb8d5632..f0de1a488f 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -570,11 +570,65 @@ class AutoTestPlane(AutoTest):
             p5=int(loc.lat * 1e7),
             p6=int(loc.lng * 1e7),
             p7=new_alt,    # alt
-            frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
+            frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
         )
         self.wait_altitude(new_alt-10, new_alt, timeout=30, relative=True)
 
-        self.fly_home_land_and_disarm()
+        self.install_terrain_handlers_context()
+
+        self.location_offset_ne(loc, 500, 500)
+        terrain_height_wanted = 150
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_DO_REPOSITION,
+            0,
+            0,
+            0,
+            0,
+            int(loc.lat*1e7),
+            int(loc.lng*1e7),
+            terrain_height_wanted,    # alt
+            frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
+        )
+
+        # move to specific terrain-relative altitude and hold for <n> seconds
+        tstart = self.get_sim_time_cached()
+        achieve_start = None
+        tr = None
+        while True:
+            if self.get_sim_time_cached() - tstart > 120:
+                raise NotAchievedException("Did not move to correct terrain alt")
+
+            m = self.mav.recv_match(type='TERRAIN_REPORT',
+                                    blocking=True,
+                                    timeout=1)
+            tr = m
+            terrain_height_achieved = m.current_height
+            self.progress("terrain_alt=%f want=%f" %
+                          (terrain_height_achieved, terrain_height_wanted))
+            if m is None:
+                continue
+            if abs(terrain_height_wanted - terrain_height_achieved) > 5:
+                if achieve_start is not None:
+                    self.progress("Achieve stop")
+                    achieve_start = None
+            elif achieve_start is None:
+                self.progress("Achieve start")
+                achieve_start = self.get_sim_time_cached()
+            if achieve_start is not None:
+                if self.get_sim_time_cached() - achieve_start > 10:
+                    break
+        m = self.mav.recv_match(type='GLOBAL_POSITION_INT',
+                                blocking=True,
+                                timeout=1)
+        self.progress("TR: %s" % tr)
+        self.progress("GPI: %s" % m)
+        min_delta = 4
+        delta = abs(m.relative_alt/1000.0 - tr.current_height)
+        if abs(delta < min_delta):
+            raise NotAchievedException("Expected altitude delta (want=%f got=%f)" %
+                                       (min_delta, delta))
+
+        self.fly_home_land_and_disarm(timeout=180)
 
     def ExternalPositionEstimate(self):
         '''Test mavlink EXTERNAL_POSITION_ESTIMATE command'''