autotest: add test for Plane LOITER

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),