Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Common: move longitude_scale into Location class

This commit is contained in:
Peter Barker 2019-03-06 18:27:27 +11:00 committed by Peter Barker
parent d7735a316a
commit a1c751919c
2 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Common/Location.cpp
View File

@ -188,7 +188,7 @@ bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
return false; return false;
} }
vec_ne.x = (lat-ekf_origin.lat) * LATLON_TO_CM; vec_ne.x = (lat-ekf_origin.lat) * LATLON_TO_CM;
vec_ne.y = (lng-ekf_origin.lng) * LATLON_TO_CM * longitude_scale(ekf_origin); vec_ne.y = (lng-ekf_origin.lng) * LATLON_TO_CM * ekf_origin.longitude_scale();
return true; return true;
} }
@ -216,7 +216,7 @@ bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const
float Location::get_distance(const struct Location &loc2) const float Location::get_distance(const struct Location &loc2) const
{ {
float dlat = (float)(loc2.lat - lat); float dlat = (float)(loc2.lat - lat);
float dlng = ((float)(loc2.lng - lng)) * longitude_scale(loc2); float dlng = ((float)(loc2.lng - lng)) * loc2.longitude_scale();
return norm(dlat, dlng) * LOCATION_SCALING_FACTOR; return norm(dlat, dlng) * LOCATION_SCALING_FACTOR;
} }
@ -226,11 +226,17 @@ void Location::offset(float ofs_north, float ofs_east)
// use is_equal() because is_zero() is a local class conflict and is_zero() in AP_Math does not belong to a class // use is_equal() because is_zero() is a local class conflict and is_zero() in AP_Math does not belong to a class
if (!is_equal(ofs_north, 0.0f) || !is_equal(ofs_east, 0.0f)) { if (!is_equal(ofs_north, 0.0f) || !is_equal(ofs_east, 0.0f)) {
int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV; int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale(*this); int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale();
lat += dlat; lat += dlat;
lng += dlng; lng += dlng;
} }
} }
float Location::longitude_scale() const
{
float scale = cosf(lat * (1.0e-7f * DEG_TO_RAD));
return constrain_float(scale, 0.01f, 1.0f);
}
// make sure we know what size the Location object is: // make sure we know what size the Location object is:
assert_storage_size<Location, 16> _assert_storage_size_Location; assert_storage_size<Location, 16> _assert_storage_size_Location;

6
libraries/AP_Common/Location.h
View File

@ -75,6 +75,12 @@ public:
// extrapolate latitude/longitude given distances (in meters) north and east // extrapolate latitude/longitude given distances (in meters) north and east
void offset(float ofs_north, float ofs_east); void offset(float ofs_north, float ofs_east);
// 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;
bool is_zero(void) const; bool is_zero(void) const;
void zero(void); void zero(void);