Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
This allows removing duplicated code in the AP_Math/location.cpp file
This commit is contained in:
Dr.-Ing. Amilcar do Carmo Lucas 2019-02-12 17:38:10 +01:00 committed by Peter Barker
parent 3bea6229ad
commit 65ccef04b9
3 changed files with 9 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Math/examples/location/location.cpp
View File

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

31
libraries/AP_Math/location.cpp
View File

@ -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);
}
/*

9
libraries/AP_Math/location.h
View File

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