Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -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
// 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
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
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
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
// 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);
// 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
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

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);
hal.console->printf("location_offset took %u usec\n",
(unsigned)(hal.scheduler->micros() - t1));
dist2 = get_distance(&loc, &loc2);
dist2 = get_distance(loc, loc2);
bearing2 = get_bearing_cd(&loc, &loc2) * 0.01;
float brg_error = bearing2-bearing;
if (brg_error > 180) {

44
libraries/AP_Math/location.cpp
View File

@ -26,42 +26,42 @@
// radius of earth in meters
#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 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
// same latitude. We can avoid the cos() and return
// the same scale factor.
return scale;
}
scale = cosf((fabsf((float)loc->lat)/1.0e7f) * DEG_TO_RAD);
last_lat = loc->lat;
scale = cosf((fabsf((float)loc.lat)/1.0e7f) * DEG_TO_RAD);
last_lat = loc.lat;
return scale;
}
// 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 dlong = ((float)(loc2->lng - loc1->lng)) * longitude_scale(loc2);
float dlat = (float)(loc2.lat - loc1.lat);
float dlong = ((float)(loc2.lng - loc1.lng)) * longitude_scale(loc2);
return pythagorous2(dlat, dlong) * 0.01113195f;
}
// 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 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_y = (loc2->lat - loc1->lat) / longitude_scale(loc2);
int32_t off_x = loc2.lng - loc1.lng;
int32_t off_y = (loc2.lat - loc1.lat) / longitude_scale(loc2);
int32_t bearing = 9000 + atan2f(-off_y, off_x) * 5729.57795f;
if (bearing < 0) bearing += 36000;
return bearing;
@ -87,7 +87,7 @@ bool location_passed_point(const struct Location &location,
// two of the points are co-located.
// If location is equal to point2 then say we have passed the
// waypoint, otherwise say we haven't
if (get_distance(&location, &point2) == 0) {
if (get_distance(location, point2) == 0) {
return true;
}
return false;
@ -96,8 +96,8 @@ bool location_passed_point(const struct Location &location,
// point2 then we are past the waypoint if the
// distance from location to point1 is greater then
// the distance from point2 to point1
return get_distance(&location, &point1) >
get_distance(&point2, &point1);
return get_distance(location, point1) >
get_distance(point2, point1);
}
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
*/
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 lon1 = radians(loc->lng*1.0e-7f);
float lat1 = radians(loc.lat*1.0e-7f);
float lon1 = radians(loc.lng*1.0e-7f);
float brng = radians(bearing);
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));
float lon2 = lon1 + atan2f(sinf(brng)*sinf(dr)*cosf(lat1),
cosf(dr)-sinf(lat1)*sinf(lat2));
loc->lat = degrees(lat2)*1.0e7f;
loc->lng = degrees(lon2)*1.0e7f;
loc.lat = degrees(lat2)*1.0e7f;
loc.lng = degrees(lon2)*1.0e7f;
}
/*
* extrapolate latitude/longitude given distances north and east
* 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) {
float dlat = ofs_north * 89.831520982f;
float dlng = (ofs_east * 89.831520982f) / longitude_scale(loc);
loc->lat += dlat;
loc->lng += dlng;
loc.lat += dlat;
loc.lng += dlng;
}
}