Autotest: Update Autorotation tests for new mode change and bailout m…

…ethods

Tools/autotest/helicopter.py

@@ -302,11 +302,15 @@ def SplineWaypoint(self, timeout=600):
         self.progress("Lowering rotor speed")
         self.set_rc(8, 1000)
 
-    def AutoRotation(self, timeout=600):
+    def Autorotation(self, timeout=600):
         """Check engine-out behaviour"""
-        self.set_parameter("AROT_ENABLE", 1)
+        self.context_push()
         start_alt = 100 # metres
-        self.set_parameter("PILOT_TKOFF_ALT", start_alt * 100)
+        self.set_parameters({
+            "AROT_ENABLE": 1,
+            "H_RSC_AROT_ENBL": 1,
+        })
+        bail_out_time = self.get_parameter('H_RSC_AROT_RUNUP')
         self.change_mode('POSHOLD')
         self.set_rc(3, 1000)
         self.set_rc(8, 1000)
@@ -322,85 +326,169 @@ def AutoRotation(self, timeout=600):
                            relative=True,
                            timeout=timeout)
         self.context_collect('STATUSTEXT')
-        self.progress("Triggering autorotate by raising interlock")
-        self.set_rc(3, 1000)
+
+        # Reset collective to enter hover
+        self.set_rc(3, 1500)
+
+        # Change to the autorotation flight mode
+        self.progress("Triggering autorotate mode")
+        self.change_mode('AUTOROTATE')
+        self.delay_sim_time(2)
+
+        # Disengage the interlock to remove power
         self.set_rc(8, 1000)
 
+        # Ensure we have progressed through the mode's state machine
         self.wait_statustext("SS Glide Phase", check_context=True)
 
-        self.change_mode('STABILIZE')
+        self.progress("Testing bailout from autorotation")
+        self.set_rc(8, 2000)
+        # See if the output ramps to a value close to expected with the prescribed time
+        self.wait_servo_channel_value(8, 1659, timeout=bail_out_time+1, comparator=operator.ge)
+
+        # Successfully bailed out, disengage the interlock and allow autorotation to progress
+        self.set_rc(8, 1000)
         self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s",
                              check_context=True,
                              regex=True)
         speed = float(self.re_match.group(1))
         if speed > 30:
             raise NotAchievedException("Hit too hard")
+
+        # Set throttle low to trip auto disarm
+        self.set_rc(3, 1000)
+
         self.wait_disarmed()
+        self.context_pop()
 
-    def ManAutoRotation(self, timeout=600):
+    def ManAutorotation(self, timeout=600):
         """Check autorotation power recovery behaviour"""
-        RAMP_TIME = 4
-        AROT_RAMP_TIME = 2
-        start_alt = 100 # metres
-        self.set_parameters({
-            "H_RSC_AROT_MN_EN": 1,
-            "H_RSC_AROT_ENG_T": AROT_RAMP_TIME,
-            "H_RSC_AROT_IDLE": 20,
-            "H_RSC_RAMP_TIME": RAMP_TIME,
-            "H_RSC_IDLE": 0,
-            "PILOT_TKOFF_ALT": start_alt * 100,
-        })
-        self.change_mode('POSHOLD')
-        self.set_rc(3, 1000)
-        self.set_rc(8, 1000)
-        self.wait_ready_to_arm()
-        self.arm_vehicle()
-        self.set_rc(8, 2000)
-        self.progress("wait for rotor runup to complete")
-        self.wait_servo_channel_value(8, 1659, timeout=10)
-        self.delay_sim_time(20)
-        self.set_rc(3, 2000)
-        self.wait_altitude(start_alt - 1,
-                           (start_alt + 5),
-                           relative=True,
-                           timeout=timeout)
-        self.context_collect('STATUSTEXT')
-        self.change_mode('STABILIZE')
-        self.progress("Triggering manual autorotation by disabling interlock")
-        self.set_rc(3, 1000)
-        self.set_rc(8, 1000)
-        self.wait_servo_channel_value(8, 1199, timeout=3)
-        self.progress("channel 8 set to autorotation window")
+        RSC_CHAN = 8
+
+        def check_rsc_output(self, throttle, timeout):
+            # Check we get a sensible throttle output
+            expected_pwm = int(throttle * 0.01 * 1000 + 1000)
+
+            # Help out the detection by accepting some margin
+            margin = 2
+
+            # See if the output ramps to a value close to expected with the prescribed time
+            self.wait_servo_channel_in_range(RSC_CHAN, expected_pwm-margin, expected_pwm+margin, timeout=timeout)
+
+        def TestAutorotationConfig(self, rsc_idle, arot_ramp_time, arot_idle, cool_down):
+            RAMP_TIME = 10
+            RUNUP_TIME = 15
+            AROT_RUNUP_TIME = arot_ramp_time + 4
+            RSC_SETPOINT = 66
+            self.set_parameters({
+                "H_RSC_AROT_ENBL": 1,
+                "H_RSC_AROT_RAMP": arot_ramp_time,
+                "H_RSC_AROT_RUNUP": AROT_RUNUP_TIME,
+                "H_RSC_AROT_IDLE": arot_idle,
+                "H_RSC_RAMP_TIME": RAMP_TIME,
+                "H_RSC_RUNUP_TIME": RUNUP_TIME,
+                "H_RSC_IDLE": rsc_idle,
+                "H_RSC_SETPOINT": RSC_SETPOINT,
+                "H_RSC_CLDWN_TIME": cool_down
+            })
 
-        # wait to establish autorotation
-        self.delay_sim_time(2)
+            # Check the RSC config so we know what to expect on the throttle output
+            if self.get_parameter("H_RSC_MODE") != 2:
+                self.set_parameter("H_RSC_MODE", 2)
+                self.reboot_sitl()
 
-        self.set_rc(8, 2000)
-        self.wait_servo_channel_value(8, 1659, timeout=AROT_RAMP_TIME * 1.1)
+            self.change_mode('POSHOLD')
+            self.set_rc(3, 1000)
+            self.set_rc(8, 1000)
+            self.wait_ready_to_arm()
+            self.arm_vehicle()
+            self.set_rc(8, 2000)
+            self.progress("wait for rotor runup to complete")
+            check_rsc_output(self, RSC_SETPOINT, RUNUP_TIME+1)
+
+            self.delay_sim_time(20)
+            self.set_rc(3, 2000)
+            self.wait_altitude(100,
+                               105,
+                               relative=True,
+                               timeout=timeout)
+            self.context_collect('STATUSTEXT')
+            self.change_mode('STABILIZE')
 
-        # give time for engine to power up
-        self.set_rc(3, 1400)
-        self.delay_sim_time(2)
+            self.progress("Triggering manual autorotation by disabling interlock")
+            self.set_rc(3, 1000)
+            self.set_rc(8, 1000)
 
-        self.progress("in-flight power recovery")
-        self.set_rc(3, 1500)
-        self.delay_sim_time(5)
+            self.wait_statustext(r"RSC: In Autorotation", check_context=True)
 
-        # initiate autorotation again
-        self.set_rc(3, 1000)
-        self.set_rc(8, 1000)
+            # Check we are using the correct throttle output. This should happen instantly on ramp down.
+            idle_thr = rsc_idle
+            if (arot_idle > 0):
+                idle_thr = arot_idle
 
-        self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s",
-                             check_context=True,
-                             regex=True)
-        speed = float(self.re_match.group(1))
-        if speed > 30:
-            raise NotAchievedException("Hit too hard")
+            check_rsc_output(self, idle_thr, 1)
 
-        self.set_rc(3, 1000)
-        # verify servo 8 resets to RSC_IDLE after land complete
-        self.wait_servo_channel_value(8, 1000, timeout=3)
-        self.wait_disarmed()
+            self.progress("RSC is outputting correct idle throttle")
+
+            # Wait to establish autorotation.
+            self.delay_sim_time(2)
+
+            # Re-engage interlock to start bailout sequence
+            self.set_rc(8, 2000)
+
+            # Ensure we see the bailout state
+            self.wait_statustext("RSC: Bailing Out", check_context=True)
+
+            # Check we are back up to flight throttle. Autorotation ramp up time should be used
+            check_rsc_output(self, RSC_SETPOINT, arot_ramp_time+1)
+
+            # Give time for engine to power up
+            self.set_rc(3, 1400)
+            self.delay_sim_time(2)
+
+            self.progress("in-flight power recovery")
+            self.set_rc(3, 1500)
+            self.delay_sim_time(5)
+
+            # Initiate autorotation again
+            self.set_rc(3, 1000)
+            self.set_rc(8, 1000)
+
+            self.wait_statustext(r"SIM Hit ground at ([0-9.]+) m/s",
+                                 check_context=True,
+                                 regex=True)
+            speed = float(self.re_match.group(1))
+            if speed > 30:
+                raise NotAchievedException("Hit too hard")
+
+            # Check that cool down is still used correctly if set
+            # First wait until we are out of the autorotation state
+            self.wait_statustext("RSC: Autorotation Stopped")
+            if (cool_down > 0):
+                check_rsc_output(self, rsc_idle*1.5, cool_down)
+
+            # Verify RSC output resets to RSC_IDLE after land complete
+            check_rsc_output(self, rsc_idle, 20)
+            self.wait_disarmed()
+
+        # We test the bailout behavior of two different configs
+        # First we test config with a regular throttle curve
+        self.progress("testing autorotation with throttle curve config")
+        self.context_push()
+        TestAutorotationConfig(self, rsc_idle=5.0, arot_ramp_time=2.0, arot_idle=0, cool_down=0)
+
+        # Now we test a config that would be used with an ESC with internal governor and an autorotation window
+        self.progress("testing autorotation with ESC autorotation window config")
+        TestAutorotationConfig(self, rsc_idle=0.0, arot_ramp_time=0.0, arot_idle=20.0, cool_down=0)
+
+        # Check rsc output behavior when using the cool down feature
+        self.progress("testing autorotation with cool down enabled and zero autorotation idle")
+        TestAutorotationConfig(self, rsc_idle=5.0, arot_ramp_time=2.0, arot_idle=0, cool_down=5.0)
+
+        self.progress("testing that H_RSC_AROT_IDLE is used over RSC_IDLE when cool down is enabled")
+        TestAutorotationConfig(self, rsc_idle=5.0, arot_ramp_time=2.0, arot_idle=10, cool_down=5.0)
+
+        self.context_pop()
 
     def mission_item_home(self, target_system, target_component):
         '''returns a mission_item_int which can be used as home in a mission'''
@@ -1024,8 +1112,8 @@ def tests(self):
             self.PosHoldTakeOff,
             self.StabilizeTakeOff,
             self.SplineWaypoint,
-            self.AutoRotation,
-            self.ManAutoRotation,
+            self.Autorotation,
+            self.ManAutorotation,
             self.governortest,
             self.FlyEachFrame,
             self.AirspeedDrivers,