Copter: add takeoff RPM check

This commit is contained in:
Randy Mackay 2022-06-28 20:19:10 +09:00 committed by Peter Hall
parent ff91bbd762
commit 3f96423795
7 changed files with 81 additions and 2 deletions

View File

@ -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

View File

@ -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();

View File

@ -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

View File

@ -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[];

View File

@ -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);
}

View File

@ -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);
}

View File

@ -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
}