add pre-takeoff check that ESCs are reporting good RPMs

Co-authored-by: Loki077 <[email protected]>

ArduPlane/quadplane.cpp

@@ -538,6 +538,21 @@ const AP_Param::GroupInfo QuadPlane::var_info2[] = {
     // @User: Standard
     AP_GROUPINFO("BCK_PIT_LIM", 38, QuadPlane, q_bck_pitch_lim, 10.0f),
 
+#if HAL_WITH_ESC_TELEM
+    // @Param: TKOFF_RPM_MIN
+    // @DisplayName: Takeoff Check RPM minimum
+    // @Description: Takeoff is not permitted until motors report at least this RPM.  Set to zero to disable check
+    // @Range: 0 10000
+    // @User: Standard
+    AP_GROUPINFO("TKOFF_RPM_MIN", 39, QuadPlane, takeoff_rpm_min, 0),
+    // @Param: TKOFF_RPM_MAX
+    // @DisplayName: Takeoff Check RPM maximum
+    // @Description: Takeoff is not permitted until motors report no more than this RPM.  Set to zero to disable check
+    // @Range: 0 10000
+    // @User: Standard
+    AP_GROUPINFO("TKOFF_RPM_MAX", 40, QuadPlane, takeoff_rpm_max, 0),
+#endif
+
     AP_GROUPEND
 };
 
@@ -1926,6 +1941,10 @@ void QuadPlane::update_throttle_hover()
  */
 void QuadPlane::motors_output(bool run_rate_controller)
 {
+#if HAL_WITH_ESC_TELEM
+    motors->takeoff_check(takeoff_rpm_min, takeoff_rpm_max, false);
+#endif
+
     /* Delay for ARMING_DELAY_MS after arming before allowing props to spin:
        1) for safety (OPTION_DELAY_ARMING)
        2) to allow motors to return to vertical (OPTION_DISARMED_TILT)

ArduPlane/quadplane.h

@@ -612,6 +612,11 @@ class QuadPlane
     // AHRS alt for land abort and package place, meters
     float land_descend_start_alt;
 
+    // support for blocking takeoff until RPMs reach parameter-defined
+    // minimum levels:
+    AP_Float takeoff_rpm_min;
+    AP_Float takeoff_rpm_max;
+
     // min alt for navigation in takeoff
     AP_Float takeoff_navalt_min;
     uint32_t takeoff_last_run_ms;

Tools/autotest/arducopter.py

@@ -1165,29 +1165,6 @@ def VibrationFailsafe(self):
         self.context_pop()
         self.reboot_sitl()
 
-    def test_takeoff_check_mode(self, mode, user_takeoff=False):
-        # stabilize check
-        self.progress("Motor takeoff check in %s" % mode)
-        self.change_mode(mode)
-        self.zero_throttle()
-        self.wait_ready_to_arm()
-        self.context_push()
-        self.context_collect('STATUSTEXT')
-        self.arm_vehicle()
-        if user_takeoff:
-            self.run_cmd(
-                mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
-                p7=10,
-            )
-        else:
-            self.set_rc(3, 1700)
-        # we may never see ourselves as armed in a heartbeat
-        self.wait_statustext("Takeoff blocked: ESC RPM out of range", check_context=True)
-        self.context_pop()
-        self.zero_throttle()
-        self.disarm_vehicle()
-        self.wait_disarmed()
-
     # Tests the motor failsafe
     def TakeoffCheck(self):
         '''Test takeoff check'''
@@ -1207,6 +1184,7 @@ def TakeoffCheck(self):
         self.test_takeoff_check_mode("POSHOLD")
         # self.test_takeoff_check_mode("SPORT")
 
+        self.start_subtest("Check TKOFF_RPM_MAX works")
         self.set_parameters({
             "AHRS_EKF_TYPE": 10,
             'SIM_ESC_TELEM': 1,

Tools/autotest/quadplane.py

@@ -1793,6 +1793,39 @@ def RTL_AUTOLAND_1_FROM_GUIDED(self):
 
         self.fly_home_land_and_disarm()
 
+    # Tests the motor failsafe
+    def TakeoffCheck(self):
+        '''Test takeoff check'''
+        self.set_parameters({
+            "AHRS_EKF_TYPE": 10,
+            'SIM_ESC_TELEM': 1,
+            'SIM_ESC_ARM_RPM': 500,
+            'TKOFF_RPM_MIN': 1000,
+        })
+
+        self.test_takeoff_check_mode("QSTABILIZE")
+        self.test_takeoff_check_mode("QACRO")
+        self.test_takeoff_check_mode("QLOITER")
+        self.test_takeoff_check_mode("QHOVER")
+#        self.test_takeoff_check_mode("GUIDED", True)  ?! can vtol-takeoff in this?
+# auto?!
+
+        self.start_subtest("Check TKOFF_RPM_MAX works")
+        self.set_parameters({
+            "AHRS_EKF_TYPE": 10,
+            'SIM_ESC_TELEM': 1,
+            'TKOFF_RPM_MIN': 1,
+            'TKOFF_RPM_MAX': 3,
+        })
+        self.test_takeoff_check_mode("STABILIZE")
+        self.test_takeoff_check_mode("ACRO")
+        self.test_takeoff_check_mode("LOITER")
+        self.test_takeoff_check_mode("ALT_HOLD")
+        # self.test_takeoff_check_mode("FLOWHOLD")
+        self.test_takeoff_check_mode("GUIDED", True)
+        self.test_takeoff_check_mode("POSHOLD")
+        # self.test_takeoff_check_mode("SPORT")
+
     def tests(self):
         '''return list of all tests'''
 
@@ -1839,5 +1872,6 @@ def tests(self):
             self.DCMClimbRate,
             self.RTL_AUTOLAND_1,  # as in fly-home then go to landing sequence
             self.RTL_AUTOLAND_1_FROM_GUIDED,  # as in fly-home then go to landing sequence
+            self.TakeoffCheck,
         ])
         return ret

Tools/autotest/vehicle_test_suite.py

@@ -5669,6 +5669,29 @@ def arming_test_mission(self):
         else:
             return None
 
+    def test_takeoff_check_mode(self, mode, user_takeoff=False):
+        # stabilize check
+        self.progress("Motor takeoff check in %s" % mode)
+        self.change_mode(mode)
+        self.zero_throttle()
+        self.wait_ready_to_arm()
+        self.context_push()
+        self.context_collect('STATUSTEXT')
+        self.arm_vehicle()
+        if user_takeoff:
+            self.run_cmd(
+                mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+                p7=10,
+            )
+        else:
+            self.set_rc(3, 1700)
+        # we may never see ourselves as armed in a heartbeat
+        self.wait_statustext("Takeoff blocked: ESC RPM out of range", check_context=True)
+        self.context_pop()
+        self.zero_throttle()
+        self.disarm_vehicle()
+        self.wait_disarmed()
+
     def set_safetyswitch_on(self, **kwargs):
         self.set_safetyswitch(1, **kwargs)