Finish location removal from AP_Math !

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;
             }

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);
             }

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");

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

ArduPlane/ArduPlane.cpp

@@ -469,7 +469,7 @@ void Plane::update_navigation()
     case Mode::Number::RTL:
         if (quadplane.available() && quadplane.rtl_mode == 1 &&
             (nav_controller->reached_loiter_target() ||
-             location_passed_point(current_loc, prev_WP_loc, next_WP_loc) ||
+             current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc) ||
              auto_state.wp_distance < MAX(qrtl_radius, quadplane.stopping_distance())) &&
             AP_HAL::millis() - last_mode_change_ms > 1000) {
             /*
@@ -588,7 +588,7 @@ void Plane::update_alt()
     if (auto_throttle_mode && !throttle_suppressed) {        
 
         float distance_beyond_land_wp = 0;
-        if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
+        if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND && current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
             distance_beyond_land_wp = current_loc.get_distance(next_WP_loc);
         }
 

ArduPlane/altitude.cpp

@@ -45,7 +45,7 @@ void Plane::adjust_altitude_target()
         // altitude target
         set_target_altitude_location(next_WP_loc);
     } else if (target_altitude.offset_cm != 0 && 
-               !location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
+               !current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
         // control climb/descent rate
         set_target_altitude_proportion(next_WP_loc, 1.0f-auto_state.wp_proportion);
 
@@ -66,8 +66,7 @@ void Plane::setup_glide_slope(void)
     // establish the distance we are travelling to the next waypoint,
     // for calculating out rate of change of altitude
     auto_state.wp_distance = current_loc.get_distance(next_WP_loc);
-    auto_state.wp_proportion = location_path_proportion(current_loc, 
-                                                        prev_WP_loc, next_WP_loc);
+    auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc);
     SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion);
     update_flight_stage();
 

ArduPlane/commands.cpp

@@ -50,7 +50,7 @@ void Plane::set_next_WP(const struct Location &loc)
     // past the waypoint when we start on a leg, then use the current
     // location as the previous waypoint, to prevent immediately
     // considering the waypoint complete
-    if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
+    if (current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
         gcs().send_text(MAV_SEVERITY_NOTICE, "Resetting previous waypoint");
         prev_WP_loc = current_loc;
     }

ArduPlane/commands_logic.cpp

@@ -617,7 +617,7 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
     // If override with p3 - then this is not used as it will overfly badly
     if (g.waypoint_max_radius > 0 &&
         auto_state.wp_distance > (uint16_t)g.waypoint_max_radius) {
-        if (location_passed_point(current_loc, prev_WP_loc, flex_next_WP_loc)) {
+        if (current_loc.past_interval_finish_line(prev_WP_loc, flex_next_WP_loc)) {
             // this is needed to ensure completion of the waypoint
             if (cmd_passby == 0) {
                 prev_WP_loc = current_loc;
@@ -644,7 +644,7 @@ bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
 	}
 
     // have we flown past the waypoint?
-    if (location_passed_point(current_loc, prev_WP_loc, flex_next_WP_loc)) {
+    if (current_loc.past_interval_finish_line(prev_WP_loc, flex_next_WP_loc)) {
         gcs().send_text(MAV_SEVERITY_INFO, "Passed waypoint #%i dist %um",
                           (unsigned)mission.get_current_nav_cmd().index,
                           (unsigned)current_loc.get_distance(flex_next_WP_loc));
@@ -1038,7 +1038,7 @@ bool Plane::verify_landing_vtol_approach(const AP_Mission::Mission_Command &cmd)
                 // check if we should move on to the next waypoint
                 Location breakout_loc = cmd.content.location;
                 breakout_loc.offset_bearing(vtol_approach_s.approach_direction_deg + 180, quadplane.stopping_distance());
-                if(location_passed_point(current_loc, start, breakout_loc)) {
+                if(current_loc.past_interval_finish_line(start, breakout_loc)) {
                     vtol_approach_s.approach_stage = VTOL_LANDING;
                     quadplane.do_vtol_land(cmd);
                     // fallthrough

ArduPlane/navigation.cpp

@@ -87,8 +87,7 @@ void Plane::navigate()
     // waypoint distance from plane
     // ----------------------------
     auto_state.wp_distance = current_loc.get_distance(next_WP_loc);
-    auto_state.wp_proportion = location_path_proportion(current_loc, 
-                                                        prev_WP_loc, next_WP_loc);
+    auto_state.wp_proportion = current_loc.line_path_proportion(prev_WP_loc, next_WP_loc);
     SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion);
 
     // update total loiter angle

ArduPlane/quadplane.cpp

@@ -2169,8 +2169,7 @@ void QuadPlane::vtol_position_controller(void)
             float target_altitude = plane.next_WP_loc.alt;
             if (poscontrol.slow_descent) {
                 // gradually descend as we approach target
-                plane.auto_state.wp_proportion = location_path_proportion(plane.current_loc, 
-                                                                          plane.prev_WP_loc, plane.next_WP_loc);
+                plane.auto_state.wp_proportion = plane.current_loc.line_path_proportion(plane.prev_WP_loc, plane.next_WP_loc);
                 target_altitude = linear_interpolate(plane.prev_WP_loc.alt,
                                                      plane.next_WP_loc.alt,
                                                      plane.auto_state.wp_proportion,

libraries/AP_Beacon/AP_Beacon.h

@@ -19,6 +19,7 @@
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_SerialManager/AP_SerialManager.h>
+#include <AP_Common/Location.h>
 
 class AP_Beacon_Backend;
 

libraries/AP_Common/Location.cpp

@@ -330,3 +330,31 @@ bool Location::check_latlng() const
 {
     return check_lat(lat) && check_lng(lng);
 }
+
+// see if location is past a line perpendicular to
+// the line between point1 and point2 and passing through point2.
+// If point1 is our previous waypoint and point2 is our target waypoint
+// then this function returns true if we have flown past
+// the target waypoint
+bool Location::past_interval_finish_line(const Location &point1, const Location &point2) const
+{
+    return this->line_path_proportion(point1, point2) >= 1.0f;
+}
+
+/*
+  return the proportion we are along the path from point1 to
+  point2, along a line parallel to point1<->point2.
+
+  This will be more than 1 if we have passed point2
+ */
+float Location::line_path_proportion(const Location &point1, const Location &point2) const
+{
+    const Vector2f vec1 = point1.get_distance_NE(point2);
+    const Vector2f vec2 = point1.get_distance_NE(*this);
+    const float dsquared = sq(vec1.x) + sq(vec1.y);
+    if (dsquared < 0.001f) {
+        // the two points are very close together
+        return 1.0f;
+    }
+    return (vec1 * vec2) / dsquared;
+}

libraries/AP_Common/Location.h

@@ -108,8 +108,28 @@ class Location
     // return true when lat and lng are within range
     bool check_latlng() const;
 
+    // see if location is past a line perpendicular to
+    // the line between point1 and point2 and passing through point2.
+    // If point1 is our previous waypoint and point2 is our target waypoint
+    // then this function returns true if we have flown past
+    // the target waypoint
+    bool past_interval_finish_line(const Location &point1, const Location &point2) const;
+
+    /*
+      return the proportion we are along the path from point1 to
+      point2, along a line parallel to point1<->point2.
+      This will be more than 1 if we have passed point2
+     */
+    float line_path_proportion(const Location &point1, const Location &point2) const;
+
 private:
     static AP_Terrain *_terrain;
+
+    // scaling factor from 1e-7 degrees to meters at equator
+    // == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH
+    static constexpr float LOCATION_SCALING_FACTOR = 0.011131884502145034f;
+    // inverse of LOCATION_SCALING_FACTOR
+    static constexpr float LOCATION_SCALING_FACTOR_INV = 89.83204953368922f;
 };
 
 #endif /* LOCATION_H */

libraries/AP_Landing/AP_Landing_Deepstall.cpp

@@ -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;
             }

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
     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);

libraries/AP_Math/examples/location/location.cpp

@@ -55,7 +55,7 @@ static void test_passed_waypoint(void)
         struct Location loc = location_from_point(test_points[i].location);
         struct Location wp1 = location_from_point(test_points[i].wp1);
         struct Location wp2 = location_from_point(test_points[i].wp2);
-        if (location_passed_point(loc, wp1, wp2) != test_points[i].passed) {
+        if (loc.past_interval_finish_line(wp1, wp2) != test_points[i].passed) {
             hal.console->printf("Failed waypoint test %u\n", (unsigned)i);
             return;
         }

libraries/AP_Math/location.cpp

@@ -40,45 +40,6 @@ float get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
     return bearing;
 }
 
-// see if location is past a line perpendicular to
-// the line between point1 and point2. If point1 is
-// our previous waypoint and point2 is our target waypoint
-// then this function returns true if we have flown past
-// the target waypoint
-bool location_passed_point(const struct Location &location,
-                           const struct Location &point1,
-                           const struct Location &point2)
-{
-    return location_path_proportion(location, point1, point2) >= 1.0f;
-}
-
-
-/*
-  return the proportion we are along the path from point1 to
-  point2, along a line parallel to point1<->point2.
-
-  This will be less than >1 if we have passed point2
- */
-float location_path_proportion(const struct Location &location,
-                               const struct Location &point1,
-                               const struct Location &point2)
-{
-    const Vector2f vec1 = point1.get_distance_NE(point2);
-    const Vector2f vec2 = point1.get_distance_NE(location);
-    float dsquared = sq(vec1.x) + sq(vec1.y);
-    if (dsquared < 0.001f) {
-        // the two points are very close together
-        return 1.0f;
-    }
-    return (vec1 * vec2) / dsquared;
-}
-
-
-
-
-
-
-
 // return true when lat and lng are within range
 bool check_lat(float lat)
 {

libraries/AP_Math/location.h

@@ -7,13 +7,6 @@
 
 #include "vector2.h"
 #include "vector3.h"
-#include <AP_Common/Location.h>
-
-// scaling factor from 1e-7 degrees to meters at equator
-// == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH
-#define LOCATION_SCALING_FACTOR 0.011131884502145034f
-// inverse of LOCATION_SCALING_FACTOR
-#define LOCATION_SCALING_FACTOR_INV 89.83204953368922f
 
 /*
  * LOCATION
@@ -25,24 +18,6 @@ float        get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &
 // return bearing in centi-degrees between two positions
 float        get_bearing_cd(const Vector3f &origin, const Vector3f &destination);
 
-// see if location is past a line perpendicular to
-// the line between point1 and point2. If point1 is
-// our previous waypoint and point2 is our target waypoint
-// then this function returns true if we have flown past
-// the target waypoint
-bool        location_passed_point(const struct Location & location,
-                                  const struct Location & point1,
-                                  const struct Location & point2);
-
-/*
-  return the proportion we are along the path from point1 to
-  point2. This will be less than >1 if we have passed point2
- */
-float       location_path_proportion(const struct Location &location,
-                               const struct Location &point1,
-                               const struct Location &point2);
-
-
 // Converts from WGS84 geodetic coordinates (lat, lon, height)
 // into WGS84 Earth Centered, Earth Fixed (ECEF) coordinates
 // (X, Y, Z)

libraries/SITL/SIM_Precland.h

@@ -17,6 +17,7 @@
 #include "stdint.h"
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_Common/Location.h>
 
 namespace SITL {