Skip to content
Snippets Groups Projects
Commit 976ae14f authored by Andrew Tridgell's avatar Andrew Tridgell
Browse files

Plane: improve landing aim point calculation

try to account for wind and current ground speed to dynamically update
glide slope in landing to flare at the right point
parent 70f5ec60
No related branches found
No related tags found
No related merge requests found
...@@ -104,13 +104,51 @@ static void setup_landing_glide_slope(void) ...@@ -104,13 +104,51 @@ static void setup_landing_glide_slope(void)
{ {
Location loc = next_WP_loc; Location loc = next_WP_loc;
// project a poiunt 500 meters past the landing point, passing // project a point 500 meters past the landing point, passing
// through the landing point // through the landing point
const float land_projection = 500; const float land_projection = 500;
int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
float land_slope = ((next_WP_loc.alt - prev_WP_loc.alt)*0.01f) / (float)wp_totalDistance; float total_distance = get_distance(prev_WP_loc, next_WP_loc);
// height we need to sink for this WP
float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f;
// current ground speed
float groundspeed = ahrs.groundspeed();
if (groundspeed < 0.5f) {
groundspeed = 0.5f;
}
// calculate time to lose the needed altitude
float sink_time = total_distance / groundspeed;
if (sink_time < 0.5f) {
sink_time = 0.5f;
}
// find the sink rate needed for the target location
float sink_rate = sink_height / sink_time;
// the height we aim for is the one to give us the right flare point
float aim_height = g.land_flare_sec * sink_rate;
// time before landing that we will flare
float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate();
// distance to flare is based on ground speed, adjusted as we
// get closer. This takes into account the wind
float flare_distance = groundspeed * flare_time;
// now calculate our aim point, which is before the landing
// point and above it
location_update(loc, land_bearing_cd*0.01f, -flare_distance);
loc.alt += aim_height*100;
// calculate slope to landing point
float land_slope = (sink_height - aim_height) / total_distance;
// calculate point along that slope 500m ahead
location_update(loc, land_bearing_cd*0.01f, land_projection); location_update(loc, land_bearing_cd*0.01f, land_projection);
loc.alt += land_slope * land_projection * 100; loc.alt -= land_slope * land_projection * 100;
// setup the offset_cm for set_target_altitude_proportion() // setup the offset_cm for set_target_altitude_proportion()
target_altitude.offset_cm = loc.alt - prev_WP_loc.alt; target_altitude.offset_cm = loc.alt - prev_WP_loc.alt;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment