AP_Math: replace location_offset() and get_distance() C function calls with Location object member (C++) function calls

This allows removing duplicated code in the AP_Math/location.cpp file

libraries/AP_Math/examples/location/location.cpp

@@ -72,10 +72,10 @@ static void test_one_offset(const struct Location &loc,
 
     loc2 = loc;
     uint32_t t1 = AP_HAL::micros();
-    location_offset(loc2, ofs_north, ofs_east);
+    loc2.offset(ofs_north, ofs_east);
     hal.console->printf("location_offset took %u usec\n",
                         (unsigned)(AP_HAL::micros() - t1));
-    dist2 = get_distance(loc, loc2);
+    dist2 = loc.get_distance(loc2);
     bearing2 = get_bearing_cd(loc, loc2) * 0.01f;
     float brg_error = bearing2-bearing;
     if (brg_error > 180) {
@@ -134,19 +134,19 @@ static void test_accuracy(void)
     loc2 = loc;
     loc2.lat += 10000000;
     v2 = Vector2f(loc2.lat * 1.0e-7f, loc2.lng * 1.0e-7f);
-    hal.console->printf("1 degree lat dist=%.4f\n", (double)get_distance(loc, loc2));
+    hal.console->printf("1 degree lat dist=%.4f\n", (double)loc.get_distance(loc2));
 
     loc2 = loc;
     loc2.lng += 10000000;
     v2 = Vector2f(loc2.lat * 1.0e-7f, loc2.lng * 1.0e-7f);
-    hal.console->printf("1 degree lng dist=%.4f\n", (double)get_distance(loc, loc2));
+    hal.console->printf("1 degree lng dist=%.4f\n", (double)loc.get_distance(loc2));
 
     for (int32_t i = 0; i < 100; i++) {
         loc2 = loc;
         loc2.lat += i;
         v2 = Vector2f((loc.lat + i) * 1.0e-7f, loc.lng * 1.0e-7f);
         if (v2 != v) {
-            hal.console->printf("lat v2 != v at i=%d dist=%.4f\n", (int)i, (double)get_distance(loc, loc2));
+            hal.console->printf("lat v2 != v at i=%d dist=%.4f\n", (int)i, (double)loc.get_distance(loc2));
             break;
         }
     }
@@ -155,7 +155,7 @@ static void test_accuracy(void)
         loc2.lng += i;
         v2 = Vector2f(loc.lat * 1.0e-7f, (loc.lng + i) * 1.0e-7f);
         if (v2 != v) {
-            hal.console->printf("lng v2 != v at i=%d dist=%.4f\n", (int)i, (double)get_distance(loc, loc2));
+            hal.console->printf("lng v2 != v at i=%d dist=%.4f\n", (int)i, (double)loc.get_distance(loc2));
             break;
         }
     }
@@ -165,7 +165,7 @@ static void test_accuracy(void)
         loc2.lat -= i;
         v2 = Vector2f((loc.lat - i) * 1.0e-7f, loc.lng * 1.0e-7f);
         if (v2 != v) {
-            hal.console->printf("-lat v2 != v at i=%d dist=%.4f\n", (int)i, (double)get_distance(loc, loc2));
+            hal.console->printf("-lat v2 != v at i=%d dist=%.4f\n", (int)i, (double)loc.get_distance(loc2));
             break;
         }
     }
@@ -174,7 +174,7 @@ static void test_accuracy(void)
         loc2.lng -= i;
         v2 = Vector2f(loc.lat * 1.0e-7f, (loc.lng - i) * 1.0e-7f);
         if (v2 != v) {
-            hal.console->printf("-lng v2 != v at i=%d dist=%.4f\n", (int)i, (double)get_distance(loc, loc2));
+            hal.console->printf("-lng v2 != v at i=%d dist=%.4f\n", (int)i, (double)loc.get_distance(loc2));
             break;
         }
     }

libraries/AP_Math/location.cpp

@@ -31,22 +31,6 @@ float longitude_scale(const struct Location &loc)
     return constrain_float(scale, 0.01f, 1.0f);
 }
 
-
-
-// return distance in meters between two locations
-float get_distance(const struct Location &loc1, const struct Location &loc2)
-{
-    float dlat              = (float)(loc2.lat - loc1.lat);
-    float dlong             = ((float)(loc2.lng - loc1.lng)) * longitude_scale(loc2);
-    return norm(dlat, dlong) * LOCATION_SCALING_FACTOR;
-}
-
-// return distance in centimeters to between two locations
-uint32_t get_distance_cm(const struct Location &loc1, const struct Location &loc2)
-{
-    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)
 {
@@ -117,20 +101,7 @@ void location_update(struct Location &loc, float bearing, float distance)
 {
     float ofs_north = cosf(radians(bearing))*distance;
     float ofs_east  = sinf(radians(bearing))*distance;
-    location_offset(loc, ofs_north, ofs_east);
-}
-
-/*
- *  extrapolate latitude/longitude given distances north and east
- */
-void location_offset(struct Location &loc, float ofs_north, float ofs_east)
-{
-    if (!is_zero(ofs_north) || !is_zero(ofs_east)) {
-        int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
-        int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale(loc);
-        loc.lat += dlat;
-        loc.lng += dlng;
-    }
+    loc.offset(ofs_north, ofs_east);
 }
 
 /*

libraries/AP_Math/location.h

@@ -21,12 +21,6 @@
 // Note: this does not include the scaling to convert longitude/latitude points to meters or centimeters
 float        longitude_scale(const struct Location &loc);
 
-// return distance in meters between two locations
-float        get_distance(const struct Location &loc1, const struct Location &loc2);
-
-// 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);
 
@@ -56,9 +50,6 @@ float       location_path_proportion(const struct Location &location,
 //  extrapolate latitude/longitude given bearing and distance
 void        location_update(struct Location &loc, float bearing, float distance);
 
-// extrapolate latitude/longitude given distances north and east
-void        location_offset(struct Location &loc, float ofs_north, float ofs_east);
-
 /*
   return the distance in meters in North/East plane as a N/E vector
   from loc1 to loc2