Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: use const references not pointers for location functions

Browse Source 
this makes life a bit easier for the new AP_Mission library

Pair-Programmed-With: Brandon Jones <brnjones@gmail.com>
This commit is contained in:
Andrew Tridgell 2013-08-05 10:14:33 +10:00
parent 47f5c9b7a4
commit 1d75b52411
3 changed files with 29 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Math/AP_Math.h
View File

@ -65,16 +65,16 @@ enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2,
// longitude_scale - returns the scaler to compensate for shrinking longitude as you move north or south from the equator // longitude_scale - returns the scaler to compensate for shrinking longitude as you move north or south from the equator
// Note: this does not include the scaling to convert longitude/latitude points to meters or centimeters // Note: this does not include the scaling to convert longitude/latitude points to meters or centimeters
float longitude_scale(const struct Location *loc); float longitude_scale(const struct Location &loc);
// return distance in meters between two locations // return distance in meters between two locations
float get_distance(const struct Location *loc1, const struct Location *loc2); float get_distance(const struct Location &loc1, const struct Location &loc2);
// return distance in centimeters between two locations // return distance in centimeters between two locations
uint32_t get_distance_cm(const struct Location *loc1, const struct Location *loc2); uint32_t get_distance_cm(const struct Location &loc1, const struct Location &loc2);
// return bearing in centi-degrees between two locations // return bearing in centi-degrees between two locations
int32_t get_bearing_cd(const struct Location *loc1, const struct Location *loc2); int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2);
// see if location is past a line perpendicular to // see if location is past a line perpendicular to
// the line between point1 and point2. If point1 is // the line between point1 and point2. If point1 is
@ -86,10 +86,10 @@ bool location_passed_point(const struct Location & location,
const struct Location & point2); const struct Location & point2);
// extrapolate latitude/longitude given bearing and distance // extrapolate latitude/longitude given bearing and distance
void location_update(struct Location *loc, float bearing, float distance); void location_update(struct Location &loc, float bearing, float distance);
// extrapolate latitude/longitude given distances north and east // extrapolate latitude/longitude given distances north and east
void location_offset(struct Location *loc, float ofs_north, float ofs_east); void location_offset(struct Location &loc, float ofs_north, float ofs_east);
/* /*
wrap an angle in centi-degrees wrap an angle in centi-degrees

2
libraries/AP_Math/examples/location/location.pde
View File

@ -76,7 +76,7 @@ static void test_one_offset(const struct Location &loc,
location_offset(&loc2, ofs_north, ofs_east); location_offset(&loc2, ofs_north, ofs_east);
hal.console->printf("location_offset took %u usec\n", hal.console->printf("location_offset took %u usec\n",
(unsigned)(hal.scheduler->micros() - t1)); (unsigned)(hal.scheduler->micros() - t1));
dist2 = get_distance(&loc, &loc2); dist2 = get_distance(loc, loc2);
bearing2 = get_bearing_cd(&loc, &loc2) * 0.01; bearing2 = get_bearing_cd(&loc, &loc2) * 0.01;
float brg_error = bearing2-bearing; float brg_error = bearing2-bearing;
if (brg_error > 180) { if (brg_error > 180) {

44
libraries/AP_Math/location.cpp
View File

@ -26,42 +26,42 @@
// radius of earth in meters // radius of earth in meters
#define RADIUS_OF_EARTH 6378100 #define RADIUS_OF_EARTH 6378100
float longitude_scale(const struct Location *loc) float longitude_scale(const struct Location &loc)
{ {
static int32_t last_lat; static int32_t last_lat;
static float scale = 1.0; static float scale = 1.0;
if (labs(last_lat - loc->lat) < 100000) { if (labs(last_lat - loc.lat) < 100000) {
// we are within 0.01 degrees (about 1km) of the // we are within 0.01 degrees (about 1km) of the
// same latitude. We can avoid the cos() and return // same latitude. We can avoid the cos() and return
// the same scale factor. // the same scale factor.
return scale; return scale;
} }
scale = cosf((fabsf((float)loc->lat)/1.0e7f) * DEG_TO_RAD); scale = cosf((fabsf((float)loc.lat)/1.0e7f) * DEG_TO_RAD);
last_lat = loc->lat; last_lat = loc.lat;
return scale; return scale;
} }
// return distance in meters between two locations // return distance in meters between two locations
float get_distance(const struct Location *loc1, const struct Location *loc2) float get_distance(const struct Location &loc1, const struct Location &loc2)
{ {
float dlat = (float)(loc2->lat - loc1->lat); float dlat = (float)(loc2.lat - loc1.lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * longitude_scale(loc2); float dlong = ((float)(loc2.lng - loc1.lng)) * longitude_scale(loc2);
return pythagorous2(dlat, dlong) * 0.01113195f; return pythagorous2(dlat, dlong) * 0.01113195f;
} }
// return distance in centimeters to between two locations // return distance in centimeters to between two locations
uint32_t get_distance_cm(const struct Location *loc1, const struct Location *loc2) uint32_t get_distance_cm(const struct Location &loc1, const struct Location &loc2)
{ {
return get_distance(loc1, loc2) * 100; return get_distance(loc1, loc2) * 100;
} }
// return bearing in centi-degrees between two locations // return bearing in centi-degrees between two locations
int32_t get_bearing_cd(const struct Location *loc1, const struct Location *loc2) int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2)
{ {
int32_t off_x = loc2->lng - loc1->lng; int32_t off_x = loc2.lng - loc1.lng;
int32_t off_y = (loc2->lat - loc1->lat) / longitude_scale(loc2); int32_t off_y = (loc2.lat - loc1.lat) / longitude_scale(loc2);
int32_t bearing = 9000 + atan2f(-off_y, off_x) * 5729.57795f; int32_t bearing = 9000 + atan2f(-off_y, off_x) * 5729.57795f;
if (bearing < 0) bearing += 36000; if (bearing < 0) bearing += 36000;
return bearing; return bearing;
@ -87,7 +87,7 @@ bool location_passed_point(const struct Location &location,
// two of the points are co-located. // two of the points are co-located.
// If location is equal to point2 then say we have passed the // If location is equal to point2 then say we have passed the
// waypoint, otherwise say we haven't // waypoint, otherwise say we haven't
if (get_distance(&location, &point2) == 0) { if (get_distance(location, point2) == 0) {
return true; return true;
} }
return false; return false;
@ -96,8 +96,8 @@ bool location_passed_point(const struct Location &location,
// point2 then we are past the waypoint if the // point2 then we are past the waypoint if the
// distance from location to point1 is greater then // distance from location to point1 is greater then
// the distance from point2 to point1 // the distance from point2 to point1
return get_distance(&location, &point1) > return get_distance(location, point1) >
get_distance(&point2, &point1); get_distance(point2, point1);
} }
if (degrees(angle) > 90) { if (degrees(angle) > 90) {
@ -112,10 +112,10 @@ bool location_passed_point(const struct Location &location,
* *
* This function is precise, but costs about 1.7 milliseconds on an AVR2560 * This function is precise, but costs about 1.7 milliseconds on an AVR2560
*/ */
void location_update(struct Location *loc, float bearing, float distance) void location_update(struct Location &loc, float bearing, float distance)
{ {
float lat1 = radians(loc->lat*1.0e-7f); float lat1 = radians(loc.lat*1.0e-7f);
float lon1 = radians(loc->lng*1.0e-7f); float lon1 = radians(loc.lng*1.0e-7f);
float brng = radians(bearing); float brng = radians(bearing);
float dr = distance/RADIUS_OF_EARTH; float dr = distance/RADIUS_OF_EARTH;
@ -123,21 +123,21 @@ void location_update(struct Location *loc, float bearing, float distance)
cosf(lat1)*sinf(dr)*cosf(brng)); cosf(lat1)*sinf(dr)*cosf(brng));
float lon2 = lon1 + atan2f(sinf(brng)*sinf(dr)*cosf(lat1), float lon2 = lon1 + atan2f(sinf(brng)*sinf(dr)*cosf(lat1),
cosf(dr)-sinf(lat1)*sinf(lat2)); cosf(dr)-sinf(lat1)*sinf(lat2));
loc->lat = degrees(lat2)*1.0e7f; loc.lat = degrees(lat2)*1.0e7f;
loc->lng = degrees(lon2)*1.0e7f; loc.lng = degrees(lon2)*1.0e7f;
} }
/* /*
* extrapolate latitude/longitude given distances north and east * extrapolate latitude/longitude given distances north and east
* This function costs about 80 usec on an AVR2560 * This function costs about 80 usec on an AVR2560
*/ */
void location_offset(struct Location *loc, float ofs_north, float ofs_east) void location_offset(struct Location &loc, float ofs_north, float ofs_east)
{ {
if (ofs_north != 0 || ofs_east != 0) { if (ofs_north != 0 || ofs_east != 0) {
float dlat = ofs_north * 89.831520982f; float dlat = ofs_north * 89.831520982f;
float dlng = (ofs_east * 89.831520982f) / longitude_scale(loc); float dlng = (ofs_east * 89.831520982f) / longitude_scale(loc);
loc->lat += dlat; loc.lat += dlat;
loc->lng += dlng; loc.lng += dlng;
} }
} }