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

Copter: fixed parachute checks for sink rate 

ensure is_flying is set, setup sink rate at the right position, force
disarm if chute releases
This commit is contained in:
Andrew Tridgell 2020-08-05 08:28:52 +10:00
parent b04844ca13
commit dbf6f6f4b1
2 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Copter.cpp
View File

@ -517,6 +517,10 @@ void Copter::one_hz_loop()
adsb.set_is_flying(!ap.land_complete); adsb.set_is_flying(!ap.land_complete);
#endif #endif
#if PARACHUTE == ENABLED
parachute.set_is_flying(!ap.land_complete);
#endif
AP_Notify::flags.flying = !ap.land_complete; AP_Notify::flags.flying = !ap.land_complete;
} }

11
ArduCopter/crash_check.cpp
View File

@ -192,6 +192,11 @@ void Copter::parachute_check()
return; return;
} }
if (parachute.release_initiated()) {
copter.arming.disarm(AP_Arming::Method::PARACHUTE_RELEASE);
return;
}
// return immediately if we are not in an angle stabilize flight mode or we are flipping // return immediately if we are not in an angle stabilize flight mode or we are flipping
if (control_mode == Mode::Number::ACRO || control_mode == Mode::Number::FLIP) { if (control_mode == Mode::Number::ACRO || control_mode == Mode::Number::FLIP) {
control_loss_count = 0; control_loss_count = 0;
@ -209,6 +214,9 @@ void Copter::parachute_check()
return; return;
} }
// pass sink rate to parachute library
parachute.set_sink_rate(-inertial_nav.get_velocity_z() * 0.01);
// check for angle error over 30 degrees // check for angle error over 30 degrees
const float angle_error = attitude_control->get_att_error_angle_deg(); const float angle_error = attitude_control->get_att_error_angle_deg();
if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) { if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
@ -242,9 +250,6 @@ void Copter::parachute_check()
// release parachute // release parachute
parachute_release(); parachute_release();
} }
// pass sink rate to parachute library
parachute.set_sink_rate(-inertial_nav.get_velocity_z() * 0.01);
} }
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user // parachute_release - trigger the release of the parachute, disarm the motors and notify the user