From ac982656fd57571a30e8d59322a903d9439751c5 Mon Sep 17 00:00:00 2001 From: Randy Mackay <rmackay9@yahoo.com> Date: Thu, 3 Apr 2014 00:20:20 +0900 Subject: [PATCH] Copter: extend crash check to trigger parachute --- ArduCopter/crash_check.pde | 88 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 88 insertions(+) diff --git a/ArduCopter/crash_check.pde b/ArduCopter/crash_check.pde index 1f4a4e5d7..a58d4a7bd 100644 --- a/ArduCopter/crash_check.pde +++ b/ArduCopter/crash_check.pde @@ -19,6 +19,11 @@ void crash_check() static uint8_t inverted_count; // number of iterations we have been inverted static int32_t baro_alt_prev; +#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() || (g.rc_3.control_in != 0 && !failsafe.radio)) { inverted_count = 0; @@ -59,3 +64,86 @@ void crash_check() inverted_count = 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 + +// 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 +void parachute_check() +{ + static uint8_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 + if (!parachute.enabled()) { + return; + } + + // return immediately if motors are not armed or pilot's throttle is above zero + if (!motors.armed()) { + control_loss_count = 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) { + control_loss_count = 0; + return; + } + + // get desired lean angles + const Vector3f& target_angle = attitude_control.angle_ef_targets(); + + // check roll and pitch angles + if (labs(ahrs.roll_sensor - target_angle.x) > CRASH_CHECK_ANGLE_DEVIATION_CD || + labs(ahrs.pitch_sensor - target_angle.y) > CRASH_CHECK_ANGLE_DEVIATION_CD) { + control_loss_count++; + + // record baro if we have just started losing control + if (control_loss_count == 1) { + baro_alt_start = baro_alt; + + // exit if baro altitude change indicates we are not falling + }else if (baro_alt >= baro_alt_start) { + control_loss_count = 0; + return; + + // 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) { + parachute_release(); + } + }else{ + // we are not inverted so reset counter + control_loss_count = 0; + } +} + +// parachute_release - trigger the release of the parachute, disarm the motors and notify the user +static void parachute_release() +{ + // To-Do: add warning tone and short delay before triggering release + + // 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("Parachute: Released!")); + + // disarm motors + init_disarm_motors(); + + // release parachute + parachute.release(); +} +#endif // PARACHUTE == ENABLED \ No newline at end of file -- GitLab