From f91ffffca79b8e93840c1e088fae3a326c69d563 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Fri, 12 Apr 2019 10:23:03 +0200 Subject: [PATCH] APMrover2: use past_interval_finish_line and line_path_proportion from Location --- APMrover2/mode_auto.cpp | 2 +- APMrover2/mode_guided.cpp | 2 +- APMrover2/mode_rtl.cpp | 2 +- APMrover2/mode_smart_rtl.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index 4332505f2d..e8fe59df7c 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -44,7 +44,7 @@ void ModeAuto::update() _distance_to_destination = rover.current_loc.get_distance(_destination); const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius; // check if we've reached the destination - if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) { + if (!_reached_destination && (near_wp || rover.current_loc.past_interval_finish_line(_origin, _destination))) { // trigger reached _reached_destination = true; } diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index e57b164ee2..7c26626f15 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -21,7 +21,7 @@ void ModeGuided::update() _distance_to_destination = rover.current_loc.get_distance(_destination); const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius; // check if we've reached the destination - if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) { + if (!_reached_destination && (near_wp || rover.current_loc.past_interval_finish_line(_origin, _destination))) { _reached_destination = true; rover.gcs().send_mission_item_reached_message(0); } diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp index 2bae9a8634..37ed585ac8 100644 --- a/APMrover2/mode_rtl.cpp +++ b/APMrover2/mode_rtl.cpp @@ -28,7 +28,7 @@ void ModeRTL::update() _distance_to_destination = rover.current_loc.get_distance(_destination); const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius; // check if we've reached the destination - if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) { + if (!_reached_destination && (near_wp || rover.current_loc.past_interval_finish_line(_origin, _destination))) { // trigger reached _reached_destination = true; gcs().send_text(MAV_SEVERITY_INFO, "Reached destination"); diff --git a/APMrover2/mode_smart_rtl.cpp b/APMrover2/mode_smart_rtl.cpp index 5acf1faf1a..2cfccda93a 100644 --- a/APMrover2/mode_smart_rtl.cpp +++ b/APMrover2/mode_smart_rtl.cpp @@ -60,7 +60,7 @@ void ModeSmartRTL::update() } // check if we've reached the next point _distance_to_destination = rover.current_loc.get_distance(_destination); - if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) { + if (_distance_to_destination <= rover.g.waypoint_radius || rover.current_loc.past_interval_finish_line(_origin, _destination)) { _load_point = true; } // continue driving towards destination