autotest: Add a test for GLIDE_SLOPE_THR

Tools/autotest/ArduPlane_Tests/GlideSlopeThresh/rapid-descent-then-climb.txt

@@ -0,0 +1,13 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.090027	1
+1	0	3	22	15.000000	0.000000	0.000000	0.000000	-35.361065	149.164916	48.849998	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.359467	149.161697	100.000000	1
+3	0	3	31	0.000000	0.000000	0.000000	0.000000	-35.359908	149.161790	200.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361402	149.161984	200.000000	1
+5	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.361477	149.161986	99.550003	1
+6	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.363121	149.162222	200.000000	1
+7	0	3	31	0.000000	0.000000	0.000000	0.000000	-35.365708	149.162588	100.000000	1
+8	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366333	149.162659	80.000000	1
+9	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366161	149.165617	50.000000	1
+10	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.365403	149.165512	30.000000	1
+11	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.362777	149.165172	0.000000	1

Tools/autotest/arduplane.py

@@ -3348,6 +3348,91 @@ function'''
         if m.intake_manifold_temperature < 20:
             raise NotAchievedException("Bad intake manifold temperature")
 
+    def test_glide_slope_threshold(self):
+
+        # Test that GLIDE_SLOPE_THRESHOLD correctly controls re-planning glide slope
+        # in the scenario that aircraft is above planned slope and slope is positive (climbing).
+        #
+        #
+        #  Behaviour with GLIDE_SLOPE_THRESH = 0 (no slope replanning)
+        #       (2)..      __(4)
+        #         |  \..__/
+        #         |  __/
+        #         (3)
+        #
+        # Behaviour with GLIDE_SLOPE_THRESH = 5 (slope replanning when >5m error)
+        #       (2)........__(4)
+        #         |     __/
+        #         |  __/
+        #         (3)
+        # Solid is plan, dots are actual flightpath.
+
+        self.load_mission('rapid-descent-then-climb.txt', strict=False)
+
+        self.set_current_waypoint(1)
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        #
+        # Initial run with GLIDE_SLOPE_THR = 5 (default).
+        #
+        self.set_parameter("GLIDE_SLOPE_THR", 5)
+
+        # Wait for waypoint commanding rapid descent, followed by climb.
+        self.wait_current_waypoint(5, timeout=1200)
+
+        # Altitude should not descend significantly below the initial altitude
+        init_altitude = self.get_altitude(relative=True, timeout=2)
+        timeout = 600
+        wpnum = 7
+        tstart = self.get_sim_time()
+        while True:
+            if self.get_sim_time() - tstart > timeout:
+                raise AutoTestTimeoutException("Did not get wanted current waypoint")
+
+            if (self.get_altitude(relative=True, timeout=2) - init_altitude) < -10:
+                raise NotAchievedException("Descended >10m before reaching desired waypoint,\
+  indicating slope was not replanned")
+
+            seq = self.mav.waypoint_current()
+            self.progress("Waiting for wp=%u current=%u" % (wpnum, seq))
+            if seq == wpnum:
+                break
+
+        self.set_current_waypoint(2)
+
+        #
+        # Second run with GLIDE_SLOPE_THR = 0 (no re-plan).
+        #
+        self.set_parameter("GLIDE_SLOPE_THR", 0)
+
+        # Wait for waypoint commanding rapid descent, followed by climb.
+        self.wait_current_waypoint(5, timeout=1200)
+
+        # This time altitude should descend significantly below the initial altitude
+        init_altitude = self.get_altitude(relative=True, timeout=2)
+        timeout = 600
+        wpnum = 7
+        tstart = self.get_sim_time()
+        while True:
+            if self.get_sim_time() - tstart > timeout:
+                raise AutoTestTimeoutException("Did not get wanted altitude")
+
+            seq = self.mav.waypoint_current()
+            self.progress("Waiting for wp=%u current=%u" % (wpnum, seq))
+            if seq == wpnum:
+                raise NotAchievedException("Reached desired waypoint without first decending 10m,\
+ indicating slope was replanned unexpectedly")
+
+            if (self.get_altitude(relative=True, timeout=2) - init_altitude) < -10:
+                break
+
+        # Disarm
+        self.wait_disarmed(timeout=600)
+
+        self.progress("Mission OK")
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestPlane, self).tests()
@@ -3601,6 +3686,10 @@ function'''
              "Test soaring speed-to-fly",
              self.fly_soaring_speed_to_fly),
 
+            ("GlideSlopeThresh",
+             "Test rebuild glide slope if above and climbing",
+             self.test_glide_slope_threshold),
+
             ("LogUpload",
              "Log upload",
              self.log_upload),