AP_Math: add overloaded functions get_horizontal_distance_cm() and get_bearing_cd() (NFC)

libraries/AP_Math/location.cpp

@@ -46,6 +46,12 @@ uint32_t get_distance_cm(const struct Location &loc1, const struct Location &loc
     return get_distance(loc1, loc2) * 100;
 }
 
+// return horizontal distance between two positions in cm
+float get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
+{
+    return norm(destination.x-origin.x,destination.y-origin.y);
+}
+
 // return bearing in centi-degrees between two locations
 int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2)
 {
@@ -56,6 +62,16 @@ int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2)
     return bearing;
 }
 
+// return bearing in centi-degrees between two positions
+float get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
+{
+    float bearing = atan2f(destination.y-origin.y, destination.x-origin.x) * DEGX100;
+    if (bearing < 0) {
+        bearing += 36000.0f;
+    }
+    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

libraries/AP_Math/location.h

@@ -8,7 +8,7 @@
 #include "vector2.h"
 #include "vector3.h"
 
-// scaling factor from 1e-7 degrees to meters at equater
+// 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
@@ -27,9 +27,15 @@ float        get_distance(const struct Location &loc1, const struct Location &lo
 // return distance in centimeters between two locations
 uint32_t     get_distance_cm(const struct Location &loc1, const struct Location &loc2);
 
+// return horizontal distance in centimeters between two positions
+float        get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination);
+
 // return bearing in centi-degrees between two locations
 int32_t      get_bearing_cd(const struct Location &loc1, const struct Location &loc2);
 
+// 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