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

AP_Landing: Fix slope calculation.

This commit is contained in:
Samuel Tabor 2018-11-25 17:55:54 +00:00 committed by Andrew Tridgell
parent 773c48b30c
commit 266fbabb6f
1 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Landing/AP_Landing_Slope.cpp
View File

@ -282,13 +282,6 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
aim_height = flare_alt*2; aim_height = flare_alt*2;
} }
// calculate slope to landing point
bool is_first_calc = is_zero(slope);
slope = (sink_height - aim_height) / total_distance;
if (is_first_calc) {
gcs().send_text(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(slope)));
}
// time before landing that we will flare // time before landing that we will flare
float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate(); float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate();
@ -312,6 +305,13 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
loc.offset_bearing(land_bearing_cd * 0.01f, -flare_distance); loc.offset_bearing(land_bearing_cd * 0.01f, -flare_distance);
loc.alt += aim_height*100; loc.alt += aim_height*100;
// calculate slope to landing point
bool is_first_calc = is_zero(slope);
slope = (sink_height - aim_height) / (total_distance - flare_distance);
if (is_first_calc) {
gcs().send_text(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(slope)));
}
// calculate point along that slope 500m ahead // calculate point along that slope 500m ahead
loc.offset_bearing(land_bearing_cd * 0.01f, land_projection); loc.offset_bearing(land_bearing_cd * 0.01f, land_projection);
loc.alt -= slope * land_projection * 100; loc.alt -= slope * land_projection * 100;