Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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:
Randy Mackay 2015-06-17 20:37:15 +09:00
parent e9829e0d82
commit 8cdfac8fcd
5 changed files with 54 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduCopter/ArduCopter.cpp
View File

@ -116,7 +116,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] PROGMEM = {
{ SCHED_TASK(update_notify), 8, 90 }, { SCHED_TASK(update_notify), 8, 90 },
{ SCHED_TASK(one_hz_loop), 400, 100 }, { SCHED_TASK(one_hz_loop), 400, 100 },
{ SCHED_TASK(ekf_check), 40, 75 }, { SCHED_TASK(ekf_check), 40, 75 },
{ SCHED_TASK(crash_check), 40, 75 },
{ SCHED_TASK(landinggear_update), 40, 75 }, { SCHED_TASK(landinggear_update), 40, 75 },
{ SCHED_TASK(lost_vehicle_check), 40, 50 }, { SCHED_TASK(lost_vehicle_check), 40, 50 },
{ SCHED_TASK(gcs_check_input), 1, 180 }, { 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 if necessary
update_home_from_EKF(); update_home_from_EKF();
// check if we've landed // check if we've landed or crashed
update_land_detector(); update_land_and_crash_detectors();
} }
// rc_loops - reads user input from transmitter/receiver // rc_loops - reads user input from transmitter/receiver

3
ArduCopter/Copter.h
View File

@ -367,7 +367,7 @@ private:
float target_sonar_alt; // desired altitude in cm above the ground float target_sonar_alt; // desired altitude in cm above the ground
int32_t baro_alt; // barometer altitude in cm above home int32_t baro_alt; // barometer altitude in cm above home
float baro_climbrate; // barometer climbrate in cm/s 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 // 3D Location vectors
// Current location of the copter (altitude is relative to home) // Current location of the copter (altitude is relative to home)
@ -795,6 +795,7 @@ private:
void read_inertia(); void read_inertia();
void read_inertial_altitude(); void read_inertial_altitude();
bool land_complete_maybe(); bool land_complete_maybe();
void update_land_and_crash_detectors();
void update_land_detector(); void update_land_detector();
void update_throttle_thr_mix(); void update_throttle_thr_mix();
void landinggear_update(); void landinggear_update();

3
ArduCopter/config.h
View File

@ -420,6 +420,9 @@
#ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF #ifndef LAND_DETECTOR_ACCEL_LPF_CUTOFF
# define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter # define LAND_DETECTOR_ACCEL_LPF_CUTOFF 1.0f // frequency cutoff of land detector accelerometer filter
#endif #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 // CAMERA TRIGGER AND CONTROL

73
ArduCopter/crash_check.cpp
View File

@ -3,57 +3,42 @@
#include "Copter.h" #include "Copter.h"
// Code to detect a crash main ArduCopter code // Code to detect a crash main ArduCopter code
#ifndef CRASH_CHECK_ITERATIONS_MAX #define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash
# define CRASH_CHECK_ITERATIONS_MAX 20 // 2 second (ie. 10 iterations at 10hz) inverted indicates a crash #define CRASH_CHECK_ANGLE_DEVIATION_CD 2000 // 20 degrees beyond angle max is signal we are inverted
#endif #define CRASH_CHECK_ACCEL_MAX 3.0f // vehicle must be accelerating less than 3m/s/s to be considered crashed
#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
// crash_check - disarms motors if a crash has been detected // 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 // 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() void Copter::crash_check()
{ {
static uint8_t inverted_count; // number of iterations we have been inverted static uint16_t crash_counter; // number of iterations vehicle may have been crashed
static int32_t baro_alt_prev;
#if PARACHUTE == ENABLED // return immediately if disarmed
// check parachute if (!motors.armed()) {
parachute_check(); crash_counter = 0;
#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; return;
} }
// return immediately if we are not in an angle stabilize flight mode or we are flipping // return immediately if we are not in an angle stabilize flight mode or we are flipping
if (control_mode == ACRO || control_mode == FLIP) { 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; return;
} }
// check angles // check angles
int32_t lean_max = aparm.angle_max + CRASH_CHECK_ANGLE_DEVIATION_CD; float lean_max = aparm.angle_max + CRASH_CHECK_ANGLE_DEVIATION_CD;
if (labs(ahrs.roll_sensor) > lean_max || labs(ahrs.pitch_sensor) > lean_max) { if (ToDeg(pythagorous2(ahrs.roll, ahrs.pitch)) > lean_max) {
inverted_count++; crash_counter++;
// 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;
// check if inverted for 2 seconds // 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 an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH); Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
// send message to gcs // send message to gcs
@ -63,26 +48,22 @@ void Copter::crash_check()
} }
}else{ }else{
// we are not inverted so reset counter // we are not inverted so reset counter
inverted_count = 0; crash_counter = 0;
} }
} }
#if PARACHUTE == ENABLED #if PARACHUTE == ENABLED
// Code to detect a crash main ArduCopter code // Code to detect a crash main ArduCopter code
#ifndef PARACHUTE_CHECK_ITERATIONS_MAX #define PARACHUTE_CHECK_TRIGGER_SEC 1 // 1 second of loss of control triggers the parachute
# define PARACHUTE_CHECK_ITERATIONS_MAX 10 // 1 second (ie. 10 iterations at 10hz) of loss of control triggers the parachute #define PARACHUTE_CHECK_ANGLE_DEVIATION_CD 3000 // 30 degrees off from target indicates a loss of control
#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
// parachute_check - disarms motors and triggers the parachute if serious loss of control has been detected // 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 // 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() 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; static int32_t baro_alt_start;
// exit immediately if parachute is not enabled // exit immediately if parachute is not enabled
@ -125,8 +106,8 @@ void Copter::parachute_check()
control_loss_count++; control_loss_count++;
// don't let control_loss_count get too high // don't let control_loss_count get too high
if (control_loss_count > PARACHUTE_CHECK_ITERATIONS_MAX) { if (control_loss_count > (PARACHUTE_CHECK_TRIGGER_SEC*MAIN_LOOP_RATE)) {
control_loss_count = PARACHUTE_CHECK_ITERATIONS_MAX; control_loss_count = (PARACHUTE_CHECK_TRIGGER_SEC*MAIN_LOOP_RATE);
} }
// record baro alt if we have just started losing control // 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 // To-Do: add check that the vehicle is actually falling
// check if loss of control for at least 1 second // 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 // reset control loss counter
control_loss_count = 0; control_loss_count = 0;
// log an error in the dataflash // log an error in the dataflash

27
ArduCopter/land_detector.cpp
View File

@ -6,6 +6,25 @@
// counter to verify landings // counter to verify landings
static uint32_t land_detector_count = 0; 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 // update_land_detector - checks if we have landed and updates the ap.land_complete flag
// called at MAIN_LOOP_RATE // called at MAIN_LOOP_RATE
void Copter::update_land_detector() void Copter::update_land_detector()
@ -18,12 +37,6 @@ void Copter::update_land_detector()
// range finder : tend to be problematic at very short distances // 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 // 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 (!motors.armed()) {
// if disarmed, always landed. // if disarmed, always landed.
set_land_complete(true); set_land_complete(true);
@ -48,7 +61,7 @@ void Copter::update_land_detector()
#endif #endif
// check that the airframe is not accelerating (not falling or breaking after fast forward flight) // 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) { if (motor_at_lower_limit && accel_stationary) {
// landed criteria met - increment the counter and check if we've triggered // landed criteria met - increment the counter and check if we've triggered