From 288ae80a59dd5ac1281fb51449350a94ad10d097 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Fri, 5 Apr 2019 15:02:43 +0200 Subject: [PATCH] AP_Landing: move get_bearing_cd to Location and rename to get_bearing_to --- libraries/AP_Landing/AP_Landing_Deepstall.cpp | 2 +- libraries/AP_Landing/AP_Landing_Slope.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Landing/AP_Landing_Deepstall.cpp b/libraries/AP_Landing/AP_Landing_Deepstall.cpp index fb0ae5d850..42d56c4e8a 100644 --- a/libraries/AP_Landing/AP_Landing_Deepstall.cpp +++ b/libraries/AP_Landing/AP_Landing_Deepstall.cpp @@ -174,7 +174,7 @@ void AP_Landing_Deepstall::verify_abort_landing(const Location &prev_WP_loc, Loc // when aborting a landing, mimic the verify_takeoff with steering hold. Once // the altitude has been reached, restart the landing sequence throttle_suppressed = false; - landing.nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); + landing.nav_controller->update_heading_hold(prev_WP_loc.get_bearing_to(next_WP_loc)); } diff --git a/libraries/AP_Landing/AP_Landing_Slope.cpp b/libraries/AP_Landing/AP_Landing_Slope.cpp index f93500791c..6fe1f7647b 100644 --- a/libraries/AP_Landing/AP_Landing_Slope.cpp +++ b/libraries/AP_Landing/AP_Landing_Slope.cpp @@ -39,7 +39,7 @@ void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Lo // when aborting a landing, mimic the verify_takeoff with steering hold. Once // the altitude has been reached, restart the landing sequence throttle_suppressed = false; - nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); + nav_controller->update_heading_hold(prev_WP_loc.get_bearing_to(next_WP_loc)); } /* @@ -136,7 +136,8 @@ bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &n prevents sudden turns if we overshoot the landing point */ struct Location land_WP_loc = next_WP_loc; - int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); + + int32_t land_bearing_cd = prev_WP_loc.get_bearing_to(next_WP_loc); location_update(land_WP_loc, land_bearing_cd*0.01f, prev_WP_loc.get_distance(current_loc) + 200); @@ -288,7 +289,7 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo // project a point 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); + int32_t land_bearing_cd = prev_WP_loc.get_bearing_to(next_WP_loc); // now calculate our aim point, which is before the landing // point and above it