Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add min alt check to parachute release

Browse Source
This commit is contained in:
Randy Mackay 2014-04-03 09:56:30 +09:00
parent b478c3a321
commit 56768a8d61
2 changed files with 34 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/control_modes.pde
View File

@ -378,7 +378,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
case AUX_SWITCH_PARACHUTE_RELEASE:
if (ch_flag == AUX_SWITCH_HIGH) {
parachute_release();
parachute_manual_release();
}
break;
@ -393,7 +393,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
break;
case AUX_SWITCH_HIGH:
parachute.enabled(true);
parachute_release();
parachute_manual_release();
break;
}
#endif

32
ArduCopter/crash_check.pde
View File

@ -88,6 +88,9 @@ void parachute_check()
return;
}
// call update to give parachute a chance to move servo or relay back to off position
parachute.update();
// return immediately if motors are not armed or pilot's throttle is above zero
if (!motors.armed()) {
control_loss_count = 0;
@ -100,6 +103,12 @@ void parachute_check()
return;
}
// ensure we are above the minimum altitude above home
if (baro_alt < parachute.alt_min_cm()) {
control_loss_count = 0;
return;
}
// get desired lean angles
const Vector3f& target_angle = attitude_control.angle_ef_targets();
@ -108,6 +117,11 @@ void parachute_check()
labs(ahrs.pitch_sensor - target_angle.y) > CRASH_CHECK_ANGLE_DEVIATION_CD) {
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;
}
// record baro if we have just started losing control
if (control_loss_count == 1) {
baro_alt_start = baro_alt;
@ -121,6 +135,9 @@ void parachute_check()
// check if loss of control for at least 1 second
}else if (control_loss_count >= PARACHUTE_CHECK_ITERATIONS_MAX) {
// reset control loss counter
control_loss_count = 0;
// release parachute
parachute_release();
}
}else{
@ -146,4 +163,19 @@ static void parachute_release()
// release parachute
parachute.release();
}
// parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error
// checks if the vehicle is landed
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()) {
// warn user of reason for failure
gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Not Released"));
return;
}
// if we get this far release parachute
parachute_release();
}
#endif // PARACHUTE == ENABLED