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

Copter: use angle error instead of absolute angle

This commit is contained in:
Leonard Hall 2015-06-17 22:08:48 +09:00 committed by Randy Mackay
parent 911bee3518
commit 4154ecdf18
1 changed files with 18 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
ArduCopter/crash_check.cpp
View File

@ -15,7 +15,7 @@ void Copter::crash_check()
static uint16_t crash_counter; // number of iterations vehicle may have been crashed static uint16_t crash_counter; // number of iterations vehicle may have been crashed
// return immediately if disarmed // return immediately if disarmed
if (!motors.armed()) { if (!motors.armed() || ap.land_complete) {
crash_counter = 0; crash_counter = 0;
return; return;
} }
@ -32,23 +32,24 @@ void Copter::crash_check()
return; return;
} }
// check angles // check for angle error over 30 degrees
float lean_max = aparm.angle_max + CRASH_CHECK_ANGLE_DEVIATION_CD; const Vector3f angle_error = attitude_control.angle_bf_error();
if (ToDeg(pythagorous2(ahrs.roll, ahrs.pitch)) > lean_max) { if (pythagorous2(angle_error.x, angle_error.y) <= 3000.0f) {
crash_counter++;
// check if inverted for 2 seconds
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
gcs_send_text_P(SEVERITY_HIGH,PSTR("Crash: Disarming"));
// disarm motors
init_disarm_motors();
}
}else{
// we are not inverted so reset counter
crash_counter = 0; crash_counter = 0;
return;
}
// we may be crashing
crash_counter++;
// check if crashing for 2 seconds
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
gcs_send_text_P(SEVERITY_HIGH,PSTR("Crash: Disarming"));
// disarm motors
init_disarm_motors();
} }
} }