Implement ESC Minimum RPM Check Before Takeoff

ArduCopter/takeoff_check.cpp

@@ -8,49 +8,6 @@
 void Copter::takeoff_check()
 {
 #if HAL_WITH_ESC_TELEM && FRAME_CONFIG != HELI_FRAME
-    // If takeoff check is disabled or vehicle is armed and flying then clear block and return
-    if ((g2.takeoff_rpm_min <= 0) || (motors->armed() && !ap.land_complete)) {
-        motors->set_spoolup_block(false);
-        return;
-    }
-
-    // block takeoff when disarmed but do not display warnings
-    if (!motors->armed()) {
-        motors->set_spoolup_block(true);
-        takeoff_check_warning_ms = 0;
-        return;
-    }
-
-    // if motors have become unblocked return immediately
-    // this ensures the motors can only be blocked immediate after arming
-    if (!motors->get_spoolup_block()) {
-        return;
-    }
-
-    // check ESCs are sending RPM at expected level
-    uint32_t motor_mask = motors->get_motor_mask();
-    const bool telem_active = AP::esc_telem().is_telemetry_active(motor_mask);
-    const bool rpm_adequate = AP::esc_telem().are_motors_running(motor_mask, g2.takeoff_rpm_min, g2.takeoff_rpm_max);
-
-    // if RPM is at the expected level clear block
-    if (telem_active && rpm_adequate) {
-        motors->set_spoolup_block(false);
-        return;
-    }
-
-    // warn user telem inactive or rpm is inadequate every 5 seconds
-    uint32_t now_ms = AP_HAL::millis();
-    if (takeoff_check_warning_ms == 0) {
-        takeoff_check_warning_ms = now_ms;
-    }
-    if (now_ms - takeoff_check_warning_ms > 5000) {
-        takeoff_check_warning_ms = now_ms;
-        const char* prefix_str = "Takeoff blocked:";
-        if (!telem_active) {
-            gcs().send_text(MAV_SEVERITY_CRITICAL, "%s waiting for ESC RPM", prefix_str);
-        } else if (!rpm_adequate) {
-            gcs().send_text(MAV_SEVERITY_CRITICAL, "%s ESC RPM out of range", prefix_str);
-        }
-    }
+    motors_takeoff_check(g2.takeoff_rpm_min, g2.takeoff_rpm_max, ap.land_complete);
 #endif
 }

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,11 @@ void QuadPlane::update_throttle_hover()
  */
 void QuadPlane::motors_output(bool run_rate_controller)
 {
+#if HAL_WITH_ESC_TELEM
+    const bool allow_spoolup_block = in_vtol_takeoff() && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE || motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN);
+    plane.motors_takeoff_check(takeoff_rpm_min, takeoff_rpm_max, allow_spoolup_block);
+#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

@@ -1774,6 +1774,56 @@ def RTL_AUTOLAND_1_FROM_GUIDED(self):
 
         self.fly_home_land_and_disarm()
 
+    # Tests the motor failsafe
+    def TakeoffCheck(self):
+        '''Test takeoff check - auto mode'''
+        self.set_parameters({
+            "AHRS_EKF_TYPE": 10,
+            'SIM_ESC_TELEM': 1,
+        })
+
+        self.start_subtest("Test blocking doesn't occur with in-range RPM")
+        self.context_push()
+        self.set_parameters({
+            'SIM_ESC_ARM_RPM': 1000,
+            'Q_TKOFF_RPM_MIN': 1000,
+            'Q_TKOFF_RPM_MAX': 2000,
+        })
+        self.upload_simple_relhome_mission([
+            (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 1),
+            (mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND, 0, 0, 0),
+        ])
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.wait_current_waypoint(2)
+        self.wait_disarmed()
+        self.set_current_waypoint(0, check_afterwards=False)
+        self.context_pop()
+        self.end_subtest("Test blocking doesn't occur with in-range RPM")
+
+        self.start_subtest("Ensure blocked if motors don't spool up")
+        self.context_push()
+        self.set_parameters({
+            'SIM_ESC_ARM_RPM': 500,
+            'Q_TKOFF_RPM_MIN': 1000,
+        })
+        self.upload_simple_relhome_mission([
+            (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30),
+            (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
+        ])
+        self.test_takeoff_check_mode("AUTO", force_disarm=True)
+        self.context_pop()
+
+        self.start_subtest("Ensure blocked if virtual motors are missing virtual props")
+        self.context_push()
+        self.set_parameters({
+            'Q_TKOFF_RPM_MIN': 1,
+            'Q_TKOFF_RPM_MAX': 3,
+        })
+        self.test_takeoff_check_mode("AUTO", force_disarm=True)
+        self.context_pop()
+
     def tests(self):
         '''return list of all tests'''
 
@@ -1820,5 +1870,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

@@ -5791,6 +5791,29 @@ def arming_test_mission(self):
         else:
             return None
 
+    def test_takeoff_check_mode(self, mode, user_takeoff=False, force_disarm=False):
+        # stabilize check
+        self.start_subtest("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(force=force_disarm)
+        self.wait_disarmed()
+
     def set_safetyswitch_on(self, **kwargs):
         self.set_safetyswitch(1, **kwargs)
 

libraries/AP_Motors/AP_Motors.h

@@ -13,3 +13,4 @@
 #include "AP_Motors6DOF.h"
 #include "AP_MotorsMatrix_6DoF_Scripting.h"
 #include "AP_MotorsMatrix_Scripting_Dynamic.h"
+#include "AP_Motors_config.h"

libraries/AP_Motors/AP_Motors_config.h

@@ -0,0 +1,5 @@
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#ifndef AP_MOTORS_ENABLED
+#define AP_MOTORS_ENABLED 1
+#endif

libraries/AP_Vehicle/AP_Vehicle.h

@@ -74,6 +74,7 @@
 #if AP_SCRIPTING_ENABLED
 #include <AP_Scripting/AP_Scripting.h>
 #endif
+#include <AP_Motors/AP_Motors_config.h>
 
 #include <AP_Gripper/AP_Gripper_config.h>
 #if AP_GRIPPER_ENABLED
@@ -484,6 +485,18 @@ class AP_Vehicle : public AP_HAL::HAL::Callbacks {
     // check for motor noise at a particular frequency
     void check_motor_noise();
 
+#if AP_MOTORS_ENABLED && HAL_WITH_ESC_TELEM
+    // code common to multiple vehicles which ensures ESC telemetry is
+    // reporting that all motors are performing.  It blocks transition
+    // to throttle-unlimited during takeoff if RPM requirements not met
+    void motors_takeoff_check(float rpm_min, float rpm_max, bool land_complete);
+    // state for takeoff_check:
+    struct {
+        uint32_t warning_ms;
+    } takeoff_check_state;
+
+#endif
+
     ModeReason control_mode_reason = ModeReason::UNKNOWN;
 
 #if AP_SIM_ENABLED

libraries/AP_Vehicle/AP_Vehicle_Motors.cpp

@@ -0,0 +1,57 @@
+#include "AP_Vehicle.h"
+
+#include <AP_Motors/AP_Motors.h>
+#include <GCS_MAVLink/GCS.h>
+
+#if AP_MOTORS_ENABLED && HAL_WITH_ESC_TELEM
+void AP_Vehicle::motors_takeoff_check(float rpm_min, float rpm_max, bool land_complete)
+{
+    auto motors = AP::motors();
+
+    // If takeoff check is disabled or vehicle is armed and flying then clear block and return
+    if ((rpm_min <= 0) || (motors->armed() && !land_complete)) {
+        motors->set_spoolup_block(false);
+        return;
+    }
+
+    // block takeoff when disarmed but do not display warnings
+    if (!motors->armed()) {
+        motors->set_spoolup_block(true);
+        takeoff_check_state.warning_ms = 0;
+        return;
+    }
+
+    // if motors have become unblocked return immediately
+    // this ensures the motors can only be blocked immediate after arming
+    if (!motors->get_spoolup_block()) {
+        return;
+    }
+
+    // check ESCs are sending RPM at expected level
+    uint32_t motor_mask = motors->get_motor_mask();
+    const bool telem_active = AP::esc_telem().is_telemetry_active(motor_mask);
+    const bool rpm_adequate = AP::esc_telem().are_motors_running(motor_mask, rpm_min, rpm_max);
+
+    // if RPM is at the expected level clear block
+    if (telem_active && rpm_adequate) {
+        motors->set_spoolup_block(false);
+        return;
+    }
+
+    // warn user telem inactive or rpm is inadequate every 5 seconds
+    uint32_t now_ms = AP_HAL::millis();
+    if (takeoff_check_state.warning_ms == 0) {
+        takeoff_check_state.warning_ms = now_ms;
+    }
+    if (now_ms - takeoff_check_state.warning_ms > 5000) {
+        takeoff_check_state.warning_ms = now_ms;
+        const char* prefix_str = "Takeoff blocked:";
+        if (!telem_active) {
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "%s waiting for ESC RPM", prefix_str);
+        } else if (!rpm_adequate) {
+            gcs().send_text(MAV_SEVERITY_CRITICAL, "%s ESC RPM out of range", prefix_str);
+        }
+    }
+}
+
+#endif  // AP_MOTORS_ENABLED && HAL_WITH_ESC_TELEM