diff --git a/ArduCopter/takeoff_check.cpp b/ArduCopter/takeoff_check.cpp index 48f172a02cc36..d0c2632fa3717 100644 --- a/ArduCopter/takeoff_check.cpp +++ b/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 } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c0c69937fa9e4..63140f7fbe3dd 100644 --- a/ArduPlane/quadplane.cpp +++ b/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) diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 2ced92498b423..aeb3d039b1928 100644 --- a/ArduPlane/quadplane.h +++ b/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; diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 1fab4c319ea46..fc7804dfbc31f 100644 --- a/Tools/autotest/arducopter.py +++ b/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, diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index 1395e245c22d8..a611ba6fe8cb7 100644 --- a/Tools/autotest/quadplane.py +++ b/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 diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py index a596dc2a37516..d6bfded9b25d6 100644 --- a/Tools/autotest/vehicle_test_suite.py +++ b/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) diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h index b5a7ffee2d437..677a951158940 100644 --- a/libraries/AP_Motors/AP_Motors.h +++ b/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" \ No newline at end of file diff --git a/libraries/AP_Motors/AP_Motors_config.h b/libraries/AP_Motors/AP_Motors_config.h new file mode 100644 index 0000000000000..1e816b3cdfd2a --- /dev/null +++ b/libraries/AP_Motors/AP_Motors_config.h @@ -0,0 +1,5 @@ +#include + +#ifndef AP_MOTORS_ENABLED +#define AP_MOTORS_ENABLED 1 +#endif \ No newline at end of file diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 97246dda8fdba..8340c1c7d2b79 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -74,6 +74,7 @@ #if AP_SCRIPTING_ENABLED #include #endif +#include #include #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 diff --git a/libraries/AP_Vehicle/AP_Vehicle_Motors.cpp b/libraries/AP_Vehicle/AP_Vehicle_Motors.cpp new file mode 100644 index 0000000000000..dfb494e0cd93c --- /dev/null +++ b/libraries/AP_Vehicle/AP_Vehicle_Motors.cpp @@ -0,0 +1,57 @@ +#include "AP_Vehicle.h" + +#include +#include + +#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