mirror of https://github.com/ArduPilot/ardupilot
AP_Landing: use TECS landing sink rate estimate in flare distance calc
This commit is contained in:
parent
0840bf5a21
commit
60ec5f5076
|
@ -283,8 +283,10 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
|
||||||
aim_height = flare_alt*2;
|
aim_height = flare_alt*2;
|
||||||
}
|
}
|
||||||
|
|
||||||
// time before landing that we will flare
|
// calculate time spent in flare assuming the sink rate reduces over time from sink_rate at aim_height
|
||||||
float flare_time = aim_height / tecs_Controller->get_land_sinkrate();
|
// to tecs_controller->get_land_sinkrate() at touchdown with more reduction in sink rate initially
|
||||||
|
const float flare_sink_rate_avg = MAX(0.67f * tecs_Controller->get_land_sinkrate() + 0.33f * sink_rate, 0.1f);
|
||||||
|
const float flare_time = aim_height / flare_sink_rate_avg;
|
||||||
|
|
||||||
// distance to flare is based on ground speed, adjusted as we
|
// distance to flare is based on ground speed, adjusted as we
|
||||||
// get closer. This takes into account the wind
|
// get closer. This takes into account the wind
|
||||||
|
|
Loading…
Reference in New Issue