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

Plane: improved landing glide slope

Browse Source 
we project a point 500m past the landing point to prevent
discontinuites close to the landing point
This commit is contained in:
Andrew Tridgell 2014-08-27 14:33:57 +10:00
parent 68dd61c7c7
commit 24622030b4
2 changed files with 40 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduPlane/altitude.pde
View File

@ -33,20 +33,7 @@ static void adjust_altitude_target()
// rate, and ignores the target altitude
set_target_altitude_location(next_WP_loc);
} else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
// in land approach use a linear glide slope to the flare point
Location loc = next_WP_loc;
float flare_distance = gps.ground_speed() * g.land_flare_sec;
loc.alt += g.land_flare_alt*100;
if (flare_distance >= wp_distance ||
flare_distance >= wp_totalDistance ||
location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
set_target_altitude_location(loc);
} else {
set_target_altitude_proportion(loc,
(wp_distance-flare_distance) / (wp_totalDistance-flare_distance));
}
// stay within the range of the start and end locations in altitude
constrain_target_altitude_location(next_WP_loc, prev_WP_loc);
setup_landing_glide_slope();
} else if (nav_controller->reached_loiter_target() || (wp_distance <= 30) || (wp_totalDistance<=30)) {
// once we reach a loiter target then lock to the final
// altitude target
@ -330,6 +317,7 @@ static void reset_offset_altitude(void)
target_altitude.offset_cm = 0;
}
/*
reset the altitude offset used for glide slopes, based on difference
between altitude at a destination and current altitude. If

41
ArduPlane/landing.pde
View File

@ -37,12 +37,13 @@ static bool verify_land()
1) we are within LAND_FLARE_ALT meters of the landing altitude
2) we are within LAND_FLARE_SEC of the landing point vertically
by the calculated sink rate
3) we have gone past the landing point (to prevent us keeping
throttle on after landing if we've had positive baro drift)
3) we have gone past the landing point and don't have
rangefinder data (to prevent us keeping throttle on
after landing if we've had positive baro drift)
*/
if (height <= g.land_flare_alt ||
height <= -auto_state.land_sink_rate * g.land_flare_sec ||
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))) {
if (!auto_state.land_complete) {
gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"),
@ -80,3 +81,37 @@ static bool verify_land()
return false;
}
/*
a special glide slope calculation for the landing approach
During the land approach use a linear glide slope to a point
projected through the landing point. We don't use the landing point
itself as that leads to discontinuities close to the landing point,
which can lead to erratic pitch control
*/
static void setup_landing_glide_slope(void)
{
Location loc = next_WP_loc;
// project a poiunt 500 meters past the landing point, passing
// through the landing point
const float land_projection = 500;
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;
location_update(loc, land_bearing_cd*0.01f, land_projection);
loc.alt += land_slope * land_projection * 100;
// setup the offset_cm for set_target_altitude_proportion()
target_altitude.offset_cm = loc.alt - prev_WP_loc.alt;
// calculate the proportion we are to the target
float land_distance = get_distance(current_loc, loc);
float land_total_distance = get_distance(prev_WP_loc, loc);
// now setup the glide slope for landing
set_target_altitude_proportion(loc, land_distance / land_total_distance);
// stay within the range of the start and end locations in altitude
constrain_target_altitude_location(loc, prev_WP_loc);
}