From 71a129d6858d6d5fe0766015fd2ff483f0921a57 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Fri, 7 Feb 2020 09:12:45 +1100
Subject: [PATCH] autotest: add test for Plane LOITER

---
 Tools/autotest/arduplane.py | 60 +++++++++++++++++++++++++++++++++++++
 1 file changed, 60 insertions(+)

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
index b298d5868a..faed3893c7 100644
--- a/Tools/autotest/arduplane.py
+++ b/Tools/autotest/arduplane.py
@@ -1572,6 +1572,62 @@ class AutoTestPlane(AutoTest):
 
         self.fly_home_land_and_disarm()
 
+    def LOITER(self):
+        self.takeoff(alt=200)
+        self.set_rc(3, 1500)
+        self.change_mode("LOITER")
+        self.progress("Doing a bit of loitering to start with")
+        tstart = self.get_sim_time()
+        while True:
+            now = self.get_sim_time_cached()
+            if now - tstart > 60:
+                break
+            m = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=5)
+            if m is None:
+                raise NotAchievedException("Did not get VFR_HUD")
+            new_throttle = m.throttle
+            alt = m.alt
+            m = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=5)
+            if m is None:
+                raise NotAchievedException("Did not get ATTITUDE")
+            pitch = math.degrees(m.pitch)
+            self.progress("Pitch:%f throttle:%u alt:%f" % (pitch, new_throttle, alt))
+        m = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=5)
+        if m is None:
+            raise NotAchievedException("Did not get VFR_HUD")
+        initial_throttle = m.throttle
+        initial_alt = m.alt
+        self.progress("Initial throttle: %u" % initial_throttle)
+        # pitch down, ensure throttle decreases:
+        rc2_max = self.get_parameter("RC2_MAX")
+        self.set_rc(2, rc2_max)
+        tstart = self.get_sim_time()
+        while True:
+            now = self.get_sim_time_cached()
+            '''stick-mixing is pushing the aircraft down.  It doesn't want to go
+            down (the target loiter altitude hasn't changed), so it
+            tries to add energy by increasing the throttle.
+            '''
+            if now - tstart > 60:
+                raise NotAchievedException("Did not see increase in throttle")
+            m = self.mav.recv_match(type='VFR_HUD', blocking=True, timeout=5)
+            if m is None:
+                raise NotAchievedException("Did not get VFR_HUD")
+            new_throttle = m.throttle
+            alt = m.alt
+            m = self.mav.recv_match(type='ATTITUDE', blocking=True, timeout=5)
+            if m is None:
+                raise NotAchievedException("Did not get ATTITUDE")
+            pitch = math.degrees(m.pitch)
+            self.progress("Pitch:%f throttle:%u alt:%f" % (pitch, new_throttle, alt))
+            if new_throttle - initial_throttle > 20:
+                self.progress("Throttle delta achieved")
+                break
+        self.progress("Centering elevator and ensuring we get back to loiter altitude")
+        self.set_rc(2, 1500)
+        self.wait_altitude(initial_alt-1, initial_alt+1)
+        self.fly_home_land_and_disarm()
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestPlane, self).tests()
@@ -1661,6 +1717,10 @@ class AutoTestPlane(AutoTest):
              "Test Advanced Failsafe",
              self.test_advanced_failsafe),
 
+            ("LOITER",
+             "Test Loiter mode",
+             self.LOITER),
+
             ("DeepStall",
              "Test DeepStall Landing",
              self.fly_deepstall),