Copter: log parachute events

This commit is contained in:
Randy Mackay 2014-04-03 17:55:34 +09:00
parent 699b84e8ca
commit 47c1cb8b99
3 changed files with 26 additions and 9 deletions

View File

@ -387,9 +387,11 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
switch (ch_flag) {
case AUX_SWITCH_LOW:
parachute.enabled(false);
Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case AUX_SWITCH_MIDDLE:
parachute.enabled(true);
Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case AUX_SWITCH_HIGH:
parachute.enabled(true);

View File

@ -103,12 +103,17 @@ void parachute_check()
return;
}
// ensure we are above the minimum altitude above home
if (baro_alt < parachute.alt_min_cm()) {
// ensure we are flying
if (ap.land_complete) {
control_loss_count = 0;
return;
}
// ensure the first control_loss event is from above the min altitude
if (control_loss_count == 0 && parachute.alt_min_cm() != 0 && baro_alt < parachute.alt_min_cm()) {
return;
}
// get desired lean angles
const Vector3f& target_angle = attitude_control.angle_ef_targets();
@ -122,7 +127,7 @@ void parachute_check()
control_loss_count = PARACHUTE_CHECK_ITERATIONS_MAX;
}
// record baro if we have just started losing control
// record baro alt if we have just started losing control
if (control_loss_count == 1) {
baro_alt_start = baro_alt;
@ -137,6 +142,8 @@ void parachute_check()
}else if (control_loss_count >= PARACHUTE_CHECK_ITERATIONS_MAX) {
// reset control loss counter
control_loss_count = 0;
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_LOSS_OF_CONTROL);
// release parachute
parachute_release();
}
@ -151,11 +158,9 @@ 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
// send message to gcs and dataflash
gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Released!"));
Log_Write_Event(DATA_PARACHUTE_RELEASED);
// disarm motors
init_disarm_motors();
@ -169,13 +174,16 @@ static void parachute_release()
static void parachute_manual_release()
{
// do not release if we are landed or below the minimum altitude above home
if (ap.land_complete || baro_alt < parachute.alt_min_cm()) {
if (ap.land_complete || (parachute.alt_min_cm() != 0 && baro_alt < parachute.alt_min_cm())) {
// warn user of reason for failure
gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Not Released"));
gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Too Low"));
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_TOO_LOW);
return;
}
// if we get this far release parachute
parachute_release();
}
#endif // PARACHUTE == ENABLED

View File

@ -282,6 +282,9 @@ enum FlipState {
#define DATA_EPM_ON 46
#define DATA_EPM_OFF 47
#define DATA_EPM_NEUTRAL 48
#define DATA_PARACHUTE_DISABLED 49
#define DATA_PARACHUTE_ENABLED 50
#define DATA_PARACHUTE_RELEASED 51
// Centi-degrees to radians
#define DEGX100 5729.57795f
@ -327,6 +330,7 @@ enum FlipState {
#define ERROR_SUBSYSTEM_CRASH_CHECK 12
#define ERROR_SUBSYSTEM_FLIP 13
#define ERROR_SUBSYSTEM_AUTOTUNE 14
#define ERROR_SUBSYSTEM_PARACHUTE 15
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1
@ -343,10 +347,13 @@ enum FlipState {
#define ERROR_CODE_MAIN_INS_DELAY 1
// subsystem specific error codes -- crash checker
#define ERROR_CODE_CRASH_CHECK_CRASH 1
#define ERROR_CODE_CRASH_CHECK_LOSS_OF_CONTROL 2
// subsystem specific error codes -- flip
#define ERROR_CODE_FLIP_ABANDONED 2
// subsystem specific error codes -- autotune
#define ERROR_CODE_AUTOTUNE_BAD_GAINS 2
// parachute failed to deploy because of low altitude
#define ERROR_CODE_PARACHUTE_TOO_LOW 2
// Arming Check Enable/Disable bits
#define ARMING_CHECK_NONE 0x00