mirror of https://github.com/ArduPilot/ardupilot
Copter: remove baro and pilot thr from crash checks
Add acceleration < 3m/s/s check Run crash and parachute checks at 400hz
This commit is contained in:
parent
e9829e0d82
commit
8cdfac8fcd
|
@ -116,7 +116,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] PROGMEM = {
|
|||
{ SCHED_TASK(update_notify), 8, 90 },
|
||||
{ SCHED_TASK(one_hz_loop), 400, 100 },
|
||||
{ SCHED_TASK(ekf_check), 40, 75 },
|
||||
{ SCHED_TASK(crash_check), 40, 75 },
|
||||
{ SCHED_TASK(landinggear_update), 40, 75 },
|
||||
{ SCHED_TASK(lost_vehicle_check), 40, 50 },
|
||||
{ SCHED_TASK(gcs_check_input), 1, 180 },
|
||||
|
@ -268,8 +267,8 @@ void Copter::fast_loop()
|
|||
// update home from EKF if necessary
|
||||
update_home_from_EKF();
|
||||
|
||||
// check if we've landed
|
||||
update_land_detector();
|
||||
// check if we've landed or crashed
|
||||
update_land_and_crash_detectors();
|
||||
}
|
||||
|
||||
// rc_loops - reads user input from transmitter/receiver
|
||||
|
|
|
@ -367,7 +367,7 @@ private:
|
|||
float target_sonar_alt; // desired altitude in cm above the ground
|
||||
int32_t baro_alt; // barometer altitude in cm above home
|
||||
float baro_climbrate; // barometer climbrate in cm/s
|
||||
LowPassFilterVector3f land_accel_ef_filter; // accelerations for land detector test
|
||||
LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
|
||||
|
||||
// 3D Location vectors
|
||||
// Current location of the copter (altitude is relative to home)
|
||||
|
@ -795,6 +795,7 @@ private:
|
|||
void read_inertia();
|
||||
void read_inertial_altitude();
|
||||
bool land_complete_maybe();
|
||||
void update_land_and_crash_detectors();
|
||||
void update_land_detector();
|
||||
void update_throttle_thr_mix();
|
||||
void landinggear_update();
|
||||
|
|
|
@ -420,6 +420,9 @@
|
|||
#ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF
|
||||
# define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter
|
||||
#endif
|
||||
#ifndef LAND_DETECTOR_ACCEL_MAX
|
||||
# define LAND_DETECTOR_ACCEL_MAX 1.0f // vehicle acceleration must be under 1m/s/s
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// CAMERA TRIGGER AND CONTROL
|
||||
|
|
|
@ -3,57 +3,42 @@
|
|||
#include "Copter.h"
|
||||
|
||||
// Code to detect a crash main ArduCopter code
|
||||
#ifndef CRASH_CHECK_ITERATIONS_MAX
|
||||
# define CRASH_CHECK_ITERATIONS_MAX 20 // 2 second (ie. 10 iterations at 10hz) inverted indicates a crash
|
||||
#endif
|
||||
#ifndef CRASH_CHECK_ANGLE_DEVIATION_CD
|
||||
# define CRASH_CHECK_ANGLE_DEVIATION_CD 2000 // 20 degrees beyond angle max is signal we are inverted
|
||||
#endif
|
||||
#ifndef CRASH_CHECK_ALT_CHANGE_LIMIT_CM
|
||||
# define CRASH_CHECK_ALT_CHANGE_LIMIT_CM 50 // baro altitude must not change by more than 50cm
|
||||
#endif
|
||||
#define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash
|
||||
#define CRASH_CHECK_ANGLE_DEVIATION_CD 2000 // 20 degrees beyond angle max is signal we are inverted
|
||||
#define CRASH_CHECK_ACCEL_MAX 3.0f // vehicle must be accelerating less than 3m/s/s to be considered crashed
|
||||
|
||||
// crash_check - disarms motors if a crash has been detected
|
||||
// crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
|
||||
// should be called at 10hz
|
||||
// called at MAIN_LOOP_RATE
|
||||
void Copter::crash_check()
|
||||
{
|
||||
static uint8_t inverted_count; // number of iterations we have been inverted
|
||||
static int32_t baro_alt_prev;
|
||||
static uint16_t crash_counter; // number of iterations vehicle may have been crashed
|
||||
|
||||
#if PARACHUTE == ENABLED
|
||||
// check parachute
|
||||
parachute_check();
|
||||
#endif
|
||||
|
||||
// return immediately if motors are not armed or pilot's throttle is above zero
|
||||
if (!motors.armed() || (!ap.throttle_zero && !failsafe.radio)) {
|
||||
inverted_count = 0;
|
||||
// return immediately if disarmed
|
||||
if (!motors.armed()) {
|
||||
crash_counter = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// return immediately if we are not in an angle stabilize flight mode or we are flipping
|
||||
if (control_mode == ACRO || control_mode == FLIP) {
|
||||
inverted_count = 0;
|
||||
crash_counter = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
|
||||
if (land_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
|
||||
crash_counter = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// check angles
|
||||
int32_t lean_max = aparm.angle_max + CRASH_CHECK_ANGLE_DEVIATION_CD;
|
||||
if (labs(ahrs.roll_sensor) > lean_max || labs(ahrs.pitch_sensor) > lean_max) {
|
||||
inverted_count++;
|
||||
|
||||
// if we have just become inverted record the baro altitude
|
||||
if (inverted_count == 1) {
|
||||
baro_alt_prev = baro_alt;
|
||||
|
||||
// exit if baro altitude change indicates we are moving (probably falling)
|
||||
}else if (labs(baro_alt - baro_alt_prev) > CRASH_CHECK_ALT_CHANGE_LIMIT_CM) {
|
||||
inverted_count = 0;
|
||||
return;
|
||||
float lean_max = aparm.angle_max + CRASH_CHECK_ANGLE_DEVIATION_CD;
|
||||
if (ToDeg(pythagorous2(ahrs.roll, ahrs.pitch)) > lean_max) {
|
||||
crash_counter++;
|
||||
|
||||
// check if inverted for 2 seconds
|
||||
}else if (inverted_count >= CRASH_CHECK_ITERATIONS_MAX) {
|
||||
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * MAIN_LOOP_RATE)) {
|
||||
// log an error in the dataflash
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
|
||||
// send message to gcs
|
||||
|
@ -63,26 +48,22 @@ void Copter::crash_check()
|
|||
}
|
||||
}else{
|
||||
// we are not inverted so reset counter
|
||||
inverted_count = 0;
|
||||
crash_counter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
#if PARACHUTE == ENABLED
|
||||
|
||||
// Code to detect a crash main ArduCopter code
|
||||
#ifndef PARACHUTE_CHECK_ITERATIONS_MAX
|
||||
# define PARACHUTE_CHECK_ITERATIONS_MAX 10 // 1 second (ie. 10 iterations at 10hz) of loss of control triggers the parachute
|
||||
#endif
|
||||
#ifndef PARACHUTE_CHECK_ANGLE_DEVIATION_CD
|
||||
# define PARACHUTE_CHECK_ANGLE_DEVIATION_CD 3000 // 30 degrees off from target indicates a loss of control
|
||||
#endif
|
||||
#define PARACHUTE_CHECK_TRIGGER_SEC 1 // 1 second of loss of control triggers the parachute
|
||||
#define PARACHUTE_CHECK_ANGLE_DEVIATION_CD 3000 // 30 degrees off from target indicates a loss of control
|
||||
|
||||
// parachute_check - disarms motors and triggers the parachute if serious loss of control has been detected
|
||||
// vehicle is considered to have a "serious loss of control" by the vehicle being more than 30 degrees off from the target roll and pitch angles continuously for 1 second
|
||||
// should be called at 10hz
|
||||
// called at MAIN_LOOP_RATE
|
||||
void Copter::parachute_check()
|
||||
{
|
||||
static uint8_t control_loss_count; // number of iterations we have been out of control
|
||||
static uint16_t control_loss_count; // number of iterations we have been out of control
|
||||
static int32_t baro_alt_start;
|
||||
|
||||
// exit immediately if parachute is not enabled
|
||||
|
@ -125,8 +106,8 @@ void Copter::parachute_check()
|
|||
control_loss_count++;
|
||||
|
||||
// don't let control_loss_count get too high
|
||||
if (control_loss_count > PARACHUTE_CHECK_ITERATIONS_MAX) {
|
||||
control_loss_count = PARACHUTE_CHECK_ITERATIONS_MAX;
|
||||
if (control_loss_count > (PARACHUTE_CHECK_TRIGGER_SEC*MAIN_LOOP_RATE)) {
|
||||
control_loss_count = (PARACHUTE_CHECK_TRIGGER_SEC*MAIN_LOOP_RATE);
|
||||
}
|
||||
|
||||
// record baro alt if we have just started losing control
|
||||
|
@ -141,7 +122,7 @@ void Copter::parachute_check()
|
|||
// To-Do: add check that the vehicle is actually falling
|
||||
|
||||
// check if loss of control for at least 1 second
|
||||
}else if (control_loss_count >= PARACHUTE_CHECK_ITERATIONS_MAX) {
|
||||
}else if (control_loss_count >= (PARACHUTE_CHECK_TRIGGER_SEC*MAIN_LOOP_RATE)) {
|
||||
// reset control loss counter
|
||||
control_loss_count = 0;
|
||||
// log an error in the dataflash
|
||||
|
|
|
@ -6,6 +6,25 @@
|
|||
// counter to verify landings
|
||||
static uint32_t land_detector_count = 0;
|
||||
|
||||
// run land and crash detectors
|
||||
// called at MAIN_LOOP_RATE
|
||||
void Copter::update_land_and_crash_detectors()
|
||||
{
|
||||
// update 1hz filtered acceleration
|
||||
Vector3f accel_ef = ahrs.get_accel_ef_blended();
|
||||
accel_ef.z += GRAVITY_MSS;
|
||||
land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS);
|
||||
|
||||
update_land_detector();
|
||||
|
||||
#if PARACHUTE == ENABLED
|
||||
// check parachute
|
||||
parachute_check();
|
||||
#endif
|
||||
|
||||
crash_check();
|
||||
}
|
||||
|
||||
// update_land_detector - checks if we have landed and updates the ap.land_complete flag
|
||||
// called at MAIN_LOOP_RATE
|
||||
void Copter::update_land_detector()
|
||||
|
@ -18,12 +37,6 @@ void Copter::update_land_detector()
|
|||
// range finder : tend to be problematic at very short distances
|
||||
// input throttle : in slow land the input throttle may be only slightly less than hover
|
||||
|
||||
Vector3f accel_ef = ahrs.get_accel_ef_blended();
|
||||
accel_ef.z += GRAVITY_MSS;
|
||||
|
||||
// lowpass filter on accel
|
||||
accel_ef = land_accel_ef_filter.apply(accel_ef, MAIN_LOOP_SECONDS);
|
||||
|
||||
if (!motors.armed()) {
|
||||
// if disarmed, always landed.
|
||||
set_land_complete(true);
|
||||
|
@ -48,7 +61,7 @@ void Copter::update_land_detector()
|
|||
#endif
|
||||
|
||||
// check that the airframe is not accelerating (not falling or breaking after fast forward flight)
|
||||
bool accel_stationary = (accel_ef.length() < 1.0f);
|
||||
bool accel_stationary = (land_accel_ef_filter.get().length() <= LAND_DETECTOR_ACCEL_MAX);
|
||||
|
||||
if (motor_at_lower_limit && accel_stationary) {
|
||||
// landed criteria met - increment the counter and check if we've triggered
|
||||
|
|
Loading…
Reference in New Issue