zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: remove baro and pilot thr from crash checks 

Add acceleration < 3m/s/s check
Run crash and parachute checks at 400hz
master
Randy Mackay 10 years ago
parent
e9829e0d82
commit
8cdfac8fcd
5 changed files with 54 additions and 57 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      ArduCopter/ArduCopter.cpp
  2. 3
      ArduCopter/Copter.h
  3. 3
      ArduCopter/config.h
  4. 73
      ArduCopter/crash_check.cpp
  5. 27
      ArduCopter/land_detector.cpp

5
ArduCopter/ArduCopter.cpp
Unescape View File

@ -116,7 +116,6 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] PROGMEM = { @@ -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() @@ -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

3
ArduCopter/Copter.h
Unescape View File

@ -367,7 +367,7 @@ private: @@ -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: @@ -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();

3
ArduCopter/config.h
Unescape View File

@ -420,6 +420,9 @@ @@ -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

73
ArduCopter/crash_check.cpp
Unescape View File

@ -3,57 +3,42 @@ @@ -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;
}
// 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;
// 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;
}
// 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 angles
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() @@ -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() @@ -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() @@ -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

27
ArduCopter/land_detector.cpp
Unescape View File

@ -6,6 +6,25 @@ @@ -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() @@ -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() @@ -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

Write Preview
Loading…
Cancel
Save