Fix heli ice/turbine cooldown and add autotest

Tools/autotest/helicopter.py

@@ -15,6 +15,7 @@
 from pysim import vehicleinfo
 
 import copy
+import operator
 
 
 class AutoTestHelicopter(AutoTestCopter):
@@ -697,6 +698,52 @@ def check_airspeeds(mav, m):
             self.disarm_vehicle()
             self.context_pop()
 
+    def TurbineCoolDown(self, timeout=200):
+        """Check Turbine Cool Down Feature"""
+        # set option for Turbine
+        RAMP_TIME = 4
+        SETPOINT = 66
+        IDLE = 15
+        COOLDOWN_TIME = 5
+        self.set_parameter("RC6_OPTION", 161)
+        self.set_parameter("H_RSC_RAMP_TIME", RAMP_TIME)
+        self.set_parameter("H_RSC_SETPOINT", SETPOINT)
+        self.set_parameter("H_RSC_IDLE", IDLE)
+        self.set_parameter("H_RSC_CLDWN_TIME", COOLDOWN_TIME)
+        self.set_rc(3, 1000)
+        self.set_rc(8, 1000)
+
+        self.progress("Starting turbine")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+
+        self.set_rc(6, 2000)
+        self.wait_statustext('Turbine startup')
+
+        # Engage interlock to run up to head speed
+        self.set_rc(8, 2000)
+
+        # Check throttle gets to setpoint
+        expected_thr = SETPOINT * 0.01 * 1000 + 1000 - 1 # servo end points are 1000 to 2000
+        self.wait_servo_channel_value(8, expected_thr, timeout=RAMP_TIME+1, comparator=operator.ge)
+
+        self.progress("Checking cool down behaviour, idle x 1.5")
+        self.set_rc(8, 1000)
+        tstart = self.get_sim_time()
+        expected_thr = IDLE * 1.5 * 0.01 * 1000 + 1000 + 1
+        self.wait_servo_channel_value(8, expected_thr, timeout=2, comparator=operator.le)
+
+        # Check that the throttle drops to idle after cool down time
+        expected_thr = IDLE * 0.01 * 1000 + 1000 + 1
+        self.wait_servo_channel_value(8, expected_thr, timeout=COOLDOWN_TIME+1, comparator=operator.le)
+
+        measured_time = self.get_sim_time() - tstart
+        if (abs(measured_time - COOLDOWN_TIME) > 1.0):
+            raise NotAchievedException('Throttle did not reduce to idle within H_RSC_CLDWN_TIME')
+
+        self.set_rc(6, 1000)
+        self.wait_disarmed(timeout=20)
+
     def TurbineStart(self, timeout=200):
         """Check Turbine Start Feature"""
         RAMP_TIME = 4
@@ -980,6 +1027,7 @@ def tests(self):
             self.FlyEachFrame,
             self.AirspeedDrivers,
             self.TurbineStart,
+            self.TurbineCoolDown,
             self.NastyMission,
             self.PIDNotches,
             self.AutoTune,

libraries/AP_Motors/AP_MotorsHeli_RSC.cpp

@@ -295,6 +295,10 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
 
         // ensure _idle_throttle not set to invalid value
         _idle_throttle = get_idle_output();
+
+        // reset fast idle timer
+        _fast_idle_timer = 0.0;
+
         break;
 
     case RotorControlState::IDLE:
@@ -331,14 +335,11 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
                     _starting = false;
                 }
             } else {
-                if (_cooldown_time > 0) {
-                    _idle_throttle = get_idle_output() * 1.5f;
-                    _fast_idle_timer += dt;
-                    if (_fast_idle_timer > (float)_cooldown_time) {
-                        _fast_idle_timer = 0.0f;
-                    }
-                } else {
-                    _idle_throttle = get_idle_output();
+                _idle_throttle = get_idle_output();
+                if (_fast_idle_timer > 0.0) {
+                    // running at fast idle for engine cool down
+                    _idle_throttle *= 1.5;
+                    _fast_idle_timer -= dt;
                 }
             }
             // this resets the bailout feature if the aircraft has landed.
@@ -353,6 +354,11 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
         // set main rotor ramp to increase to full speed
         update_rotor_ramp(1.0f, dt);
 
+        // set fast idle timer so next time RSC goes to idle, the cooldown timer starts
+        if (_cooldown_time.get() > 0) {
+            _fast_idle_timer = _cooldown_time.get();
+        }
+
         // ensure _idle_throttle not set to invalid value due to premature switch out of turbine start
         if (_starting) {
             _idle_throttle = get_idle_output();