Browse Source

Plane: added crash detection in autoland

if we are no longer flying then flare, which turns off the motor

This is based on work by Tom Pittenger
mission-4.1.18
Andrew Tridgell 10 years ago
parent
commit
e55350a5d6
  1. 7
      ArduPlane/landing.pde

7
ArduPlane/landing.pde

@ -52,12 +52,17 @@ static bool verify_land() @@ -52,12 +52,17 @@ static bool verify_land()
*/
if (height <= g.land_flare_alt ||
height <= auto_state.land_sink_rate * aparm.land_flare_sec ||
(!rangefinder_state.in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc))) {
(!rangefinder_state.in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) ||
(fabsf(auto_state.land_sink_rate) < 0.2f && !is_flying())) {
if (!auto_state.land_complete) {
if (!is_flying()) {
gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), gps.ground_speed());
} else {
gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"),
height, auto_state.land_sink_rate, gps.ground_speed());
}
}
auto_state.land_complete = true;
if (gps.ground_speed() < 3) {

Loading…
Cancel
Save