mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Copter: add takeoff RPM check
This commit is contained in:
parent
ff91bbd762
commit
3f96423795
@ -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
|
||||
|
@ -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();
|
||||
|
@ -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
|
||||
|
||||
|
@ -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[];
|
||||
|
@ -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);
|
||||
}
|
||||
|
@ -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);
|
||||
}
|
||||
|
56
ArduCopter/takeoff_check.cpp
Normal file
56
ArduCopter/takeoff_check.cpp
Normal 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
|
||||
}
|
Loading…
Reference in New Issue
Block a user