diff --git a/libraries/AP_Math/examples/location/location.cpp b/libraries/AP_Math/examples/location/location.cpp index 99612572dd..2bb626ea70 100644 --- a/libraries/AP_Math/examples/location/location.cpp +++ b/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; } } diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index 1fdb5c4512..e11416dd8d 100644 --- a/libraries/AP_Math/location.cpp +++ b/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); } /* diff --git a/libraries/AP_Math/location.h b/libraries/AP_Math/location.h index 60ad6de915..f7a6bde1ce 100644 --- a/libraries/AP_Math/location.h +++ b/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