From 3f9642379534a99fe7fa8cef673bc17dbebaf04e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 28 Jun 2022 20:19:10 +0900 Subject: [PATCH] Copter: add takeoff RPM check --- ArduCopter/Copter.cpp | 1 + ArduCopter/Copter.h | 6 ++++ ArduCopter/Parameters.cpp | 7 +++++ ArduCopter/Parameters.h | 1 + ArduCopter/mode_acro.cpp | 6 +++- ArduCopter/mode_stabilize.cpp | 6 +++- ArduCopter/takeoff_check.cpp | 56 +++++++++++++++++++++++++++++++++++ 7 files changed, 81 insertions(+), 2 deletions(-) create mode 100644 ArduCopter/takeoff_check.cpp diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 3060306202..a9a1202d71 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -192,6 +192,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(ekf_check, 10, 75, 84), SCHED_TASK(check_vibration, 10, 50, 87), SCHED_TASK(gpsglitch_check, 10, 50, 90), + SCHED_TASK(takeoff_check, 50, 50, 91), #if LANDING_GEAR_ENABLED == ENABLED SCHED_TASK(landinggear_update, 10, 75, 93), #endif diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 0cf8109de3..47e04ac454 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -328,6 +328,9 @@ private: uint32_t clear_ms; // system time high vibrations stopped } vibration_check; + // takeoff check + uint32_t takeoff_check_warning_ms; // system time user was last warned of takeoff check failure + // GCS selection GCS_Copter _gcs; // avoid using this; use gcs() GCS_Copter &gcs() { return _gcs; } @@ -897,6 +900,9 @@ private: bool rangefinder_up_ok() const; void update_optical_flow(void); + // takeoff_check.cpp + void takeoff_check(); + // RC_Channel.cpp void save_trim(); void auto_trim(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 96cc672366..c13eb4af7c 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1151,6 +1151,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("TKOFF_SLEW_TIME", 57, ParametersG2, takeoff_throttle_slew_time, 2.0), + // @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", 58, ParametersG2, takeoff_rpm_min, 0), + // ID 62 is reserved for the SHOW_... parameters from the Skybrush fork at // https://github.com/skybrush-io/ardupilot diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0796faa118..07c4822a0b 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -670,6 +670,7 @@ public: // ramp time of throttle during take-off AP_Float takeoff_throttle_slew_time; + AP_Int16 takeoff_rpm_min; }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/mode_acro.cpp b/ArduCopter/mode_acro.cpp index bc27a4003a..238c79b039 100644 --- a/ArduCopter/mode_acro.cpp +++ b/ArduCopter/mode_acro.cpp @@ -24,17 +24,21 @@ void ModeAcro::run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } + bool limit_throttle_out = false; + switch (motors->get_spool_state()) { case AP_Motors::SpoolState::SHUT_DOWN: // Motors Stopped attitude_control->reset_target_and_rate(true); attitude_control->reset_rate_controller_I_terms(); + limit_throttle_out = true; break; case AP_Motors::SpoolState::GROUND_IDLE: // Landed attitude_control->reset_target_and_rate(); attitude_control->reset_rate_controller_I_terms_smoothly(); + limit_throttle_out = true; break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: @@ -58,7 +62,7 @@ void ModeAcro::run() } // output pilot's throttle without angle boost - attitude_control->set_throttle_out(get_pilot_desired_throttle(), + attitude_control->set_throttle_out(limit_throttle_out ? 0 : get_pilot_desired_throttle(), false, copter.g.throttle_filt); } diff --git a/ArduCopter/mode_stabilize.cpp b/ArduCopter/mode_stabilize.cpp index 135cdea51c..6e05a7b969 100644 --- a/ArduCopter/mode_stabilize.cpp +++ b/ArduCopter/mode_stabilize.cpp @@ -28,17 +28,21 @@ void ModeStabilize::run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); } + bool limit_throttle_out = false; + switch (motors->get_spool_state()) { case AP_Motors::SpoolState::SHUT_DOWN: // Motors Stopped attitude_control->reset_yaw_target_and_rate(); attitude_control->reset_rate_controller_I_terms(); + limit_throttle_out = true; break; case AP_Motors::SpoolState::GROUND_IDLE: // Landed attitude_control->reset_yaw_target_and_rate(); attitude_control->reset_rate_controller_I_terms_smoothly(); + limit_throttle_out = true; break; case AP_Motors::SpoolState::THROTTLE_UNLIMITED: @@ -58,7 +62,7 @@ void ModeStabilize::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // output pilot's throttle - attitude_control->set_throttle_out(get_pilot_desired_throttle(), + attitude_control->set_throttle_out(limit_throttle_out ? 0 : get_pilot_desired_throttle(), true, g.throttle_filt); } diff --git a/ArduCopter/takeoff_check.cpp b/ArduCopter/takeoff_check.cpp new file mode 100644 index 0000000000..8ee0df6c62 --- /dev/null +++ b/ArduCopter/takeoff_check.cpp @@ -0,0 +1,56 @@ +#include "Copter.h" + +// +// pre-takeoff checks +// + +// detects if the vehicle should be allowed to takeoff or not and sets the motors.blocked flag +void Copter::takeoff_check() +{ +#if HAL_WITH_ESC_TELEM + // 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); + + // 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 too low", prefix_str); + } + } +#endif +}