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

Copter: crash checker 

Crash is determined to have happened when the copter is 20deg more than
the ANGLE_MAX parameter continuously for more than 2 seconds
Not activated when in ACRO mode or while flipping
This commit is contained in:
Randy Mackay 2013-10-29 22:11:48 +09:00
parent c64aa0e4de
commit 2326b2e5f5
3 changed files with 49 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduCopter/ArduCopter.pde
View File

@ -871,6 +871,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ barometer_accumulate, 2, 250 },
{ update_notify, 2, 100 },
{ one_hz_loop, 100, 420 },
{ crash_check, 10, 20 },
{ gcs_check_input, 2, 550 },
{ gcs_send_heartbeat, 100, 150 },
{ gcs_send_deferred, 2, 720 },
@ -976,7 +977,7 @@ void loop()
{
// wait for an INS sample
if (!ins.wait_for_sample(1000)) {
Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_INS_DELAY);
Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_MAIN_INS_DELAY);
return;
}
uint32_t timer = micros();

43
ArduCopter/crash_check.pde Normal file
View File

@ -0,0 +1,43 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// Code to detect a crash main ArduCopter code
#ifndef CRASH_CHECK_ITERATIONS_MAX
# define CRASH_CHECK_ITERATIONS_MAX 20 // 1 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
// 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
void crash_check()
{
static uint8_t inverted_count; // number of iterations we have been inverted
// return immediately if motors are not armed
if (!motors.armed()) {
inverted_count = 0;
return;
}
// return immediately if we are not in an angle stabilize flight mode or we are flipping
if (control_mode == ACRO || ap.do_flip) {
inverted_count = 0;
return;
}
// check angles
int32_t lean_max = g.angle_max + CRASH_CHECK_ANGLE_DEVIATION_CD;
if (labs(ahrs.roll_sensor) > lean_max || labs(ahrs.pitch_sensor) > lean_max) {
inverted_count++;
if (inverted_count >= CRASH_CHECK_ITERATIONS_MAX) {
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
// send message to gcs
gcs_send_text_P(SEVERITY_HIGH,PSTR("Crash: disarming"));
// disarm motors
init_disarm_motors();
}
}
}

6
ArduCopter/defines.h
View File

@ -445,6 +445,7 @@ enum ap_message {
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9
#define ERROR_SUBSYSTEM_FLIGHT_MODE 10
#define ERROR_SUBSYSTEM_GPS 11
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1
@ -458,8 +459,9 @@ enum ap_message {
// subsystem specific error codes -- gps
#define ERROR_CODE_GPS_GLITCH 2
// subsystem specific error codes -- main
#define ERROR_CODE_INS_DELAY 1
#define ERROR_CODE_MAIN_INS_DELAY 1
// subsystem specific error codes -- crash checker
#define ERROR_CODE_CRASH_CHECK_CRASH 1
#endif // _DEFINES_H