Browse Source

AP_Landing: use past_interval_finish_line and line_path_proportion from Location

master
Pierre Kancir 6 years ago committed by Andrew Tridgell
parent
commit
8168b3c8e4
  1. 4
      libraries/AP_Landing/AP_Landing_Deepstall.cpp
  2. 2
      libraries/AP_Landing/AP_Landing_Slope.cpp

4
libraries/AP_Landing/AP_Landing_Deepstall.cpp

@ -281,8 +281,8 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne @@ -281,8 +281,8 @@ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &ne
memcpy(&entry_point, &landing_point, sizeof(Location));
entry_point.offset_bearing(target_heading_deg + 180.0, travel_distance);
if (!location_passed_point(current_loc, arc_exit, entry_point)) {
if (location_passed_point(current_loc, arc_exit, extended_approach)) {
if (!current_loc.past_interval_finish_line(arc_exit, entry_point)) {
if (current_loc.past_interval_finish_line(arc_exit, extended_approach)) {
// this should never happen, but prevent against an indefinite fly away
stage = DEEPSTALL_STAGE_FLY_TO_LANDING;
}

2
libraries/AP_Landing/AP_Landing_Slope.cpp

@ -303,7 +303,7 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo @@ -303,7 +303,7 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
target_altitude_offset_cm = loc.alt - prev_WP_loc.alt;
// calculate the proportion we are to the target
float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc);
float land_proportion = current_loc.line_path_proportion(prev_WP_loc, loc);
// now setup the glide slope for landing
set_target_altitude_proportion_fn(loc, 1.0f - land_proportion);

Loading…
Cancel
Save