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

AP_Common: make longitude_scale() a static 

this changes the use of longitude_scale() to use the average latitude
instead of the lattitude at one end of the line when calculating
positions and distances. It means we obey the basic geometry rule
that:

 pos1 + offs = pos2
 pos2 - offs == pos1
This commit is contained in:
Andrew Tridgell 2021-07-22 13:36:57 +10:00 committed by Randy Mackay
parent c9b9a6a616
commit 01156ed348
2 changed files with 23 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_Common/Location.cpp
View File

@ -210,7 +210,7 @@ bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const
return false;
}
vec_ne.x = (lat-ekf_origin.lat) * LATLON_TO_CM;
vec_ne.y = diff_longitude(lng,ekf_origin.lng) * LATLON_TO_CM * ekf_origin.longitude_scale();
vec_ne.y = diff_longitude(lng,ekf_origin.lng) * LATLON_TO_CM * longitude_scale((lat+ekf_origin.lat)/2);
return true;
}
@ -235,10 +235,10 @@ bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const
}
// return distance in meters between two locations
float Location::get_distance(const struct Location &loc2) const
ftype Location::get_distance(const struct Location &loc2) const
{
float dlat = (float)(loc2.lat - lat);
float dlng = ((float)diff_longitude(loc2.lng,lng)) * loc2.longitude_scale();
ftype dlat = (ftype)(loc2.lat - lat);
ftype dlng = ((ftype)diff_longitude(loc2.lng,lng)) * longitude_scale((lat+loc2.lat)/2);
return norm(dlat, dlng) * LOCATION_SCALING_FACTOR;
}
@ -250,14 +250,14 @@ float Location::get_distance(const struct Location &loc2) const
Vector2f Location::get_distance_NE(const Location &loc2) const
{
return Vector2f((loc2.lat - lat) * LOCATION_SCALING_FACTOR,
diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale());
diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale((loc2.lat+lat)/2));
}
// return the distance in meters in North/East/Down plane as a N/E/D vector to loc2
Vector3f Location::get_distance_NED(const Location &loc2) const
{
return Vector3f((loc2.lat - lat) * LOCATION_SCALING_FACTOR,
diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale(),
diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale((lat+loc2.lat)/2),
(alt - loc2.alt) * 0.01);
}
@ -265,27 +265,27 @@ Vector3f Location::get_distance_NED(const Location &loc2) const
Vector3d Location::get_distance_NED_double(const Location &loc2) const
{
return Vector3d((loc2.lat - lat) * double(LOCATION_SCALING_FACTOR),
diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale(),
diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale((lat+loc2.lat)/2),
(alt - loc2.alt) * 0.01);
}
Vector2d Location::get_distance_NE_double(const Location &loc2) const
{
return Vector2d((loc2.lat - lat) * double(LOCATION_SCALING_FACTOR),
diff_longitude(loc2.lng,lng) * double(LOCATION_SCALING_FACTOR) * longitude_scale());
diff_longitude(loc2.lng,lng) * double(LOCATION_SCALING_FACTOR) * longitude_scale((lat+loc2.lat)/2));
}
Vector2F Location::get_distance_NE_ftype(const Location &loc2) const
{
return Vector2F((loc2.lat - lat) * ftype(LOCATION_SCALING_FACTOR),
diff_longitude(loc2.lng,lng) * ftype(LOCATION_SCALING_FACTOR) * longitude_scale());
diff_longitude(loc2.lng,lng) * ftype(LOCATION_SCALING_FACTOR) * longitude_scale((lat+loc2.lat)/2));
}
// extrapolate latitude/longitude given distances (in meters) north and east
void Location::offset_latlng(int32_t &lat, int32_t &lng, ftype ofs_north, ftype ofs_east)
{
const int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
const int64_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale();
const int64_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale(lat+dlat/2);
lat += dlat;
lat = limit_lattitude(lat);
lng = wrap_longitude(dlng+lng);
@ -304,18 +304,18 @@ void Location::offset(ftype ofs_north, ftype ofs_east)
* positions, so it keeps the accuracy even when dealing with small
* distances and floating point numbers
*/
void Location::offset_bearing(float bearing_deg, float distance)
void Location::offset_bearing(ftype bearing_deg, ftype distance)
{
const float ofs_north = cosf(radians(bearing_deg)) * distance;
const float ofs_east = sinf(radians(bearing_deg)) * distance;
const ftype ofs_north = cosF(radians(bearing_deg)) * distance;
const ftype ofs_east = sinF(radians(bearing_deg)) * distance;
offset(ofs_north, ofs_east);
}
// extrapolate latitude/longitude given bearing, pitch and distance
void Location::offset_bearing_and_pitch(float bearing_deg, float pitch_deg, float distance)
void Location::offset_bearing_and_pitch(ftype bearing_deg, ftype pitch_deg, ftype distance)
{
const float ofs_north = cosf(radians(pitch_deg)) * cosf(radians(bearing_deg)) * distance;
const float ofs_east = cosf(radians(pitch_deg)) * sinf(radians(bearing_deg)) * distance;
const ftype ofs_north = cosF(radians(pitch_deg)) * cosF(radians(bearing_deg)) * distance;
const ftype ofs_east = cosF(radians(pitch_deg)) * sinF(radians(bearing_deg)) * distance;
offset(ofs_north, ofs_east);
const int32_t dalt = sinf(radians(pitch_deg)) * distance *100.0f;
alt += dalt;
@ -366,7 +366,7 @@ assert_storage_size<Location, 16> _assert_storage_size_Location;
int32_t Location::get_bearing_to(const struct Location &loc2) const
{
const int32_t off_x = diff_longitude(loc2.lng,lng);
const int32_t off_y = (loc2.lat - lat) / loc2.longitude_scale();
const int32_t off_y = (loc2.lat - lat) / loc2.longitude_scale((lat+loc2.lat)/2);
int32_t bearing = 9000 + atan2f(-off_y, off_x) * DEGX100;
if (bearing < 0) {
bearing += 36000;
@ -408,7 +408,7 @@ float Location::line_path_proportion(const Location &point1, const Location &poi
{
const Vector2f vec1 = point1.get_distance_NE(point2);
const Vector2f vec2 = point1.get_distance_NE(*this);
const float dsquared = sq(vec1.x) + sq(vec1.y);
const ftype dsquared = sq(vec1.x) + sq(vec1.y);
if (dsquared < 0.001f) {
// the two points are very close together
return 1.0f;

9
libraries/AP_Common/Location.h
View File

@ -33,6 +33,7 @@ public:
Location();
Location(int32_t latitude, int32_t longitude, int32_t alt_in_cm, AltFrame frame);
Location(const Vector3f &ekf_offset_neu, AltFrame frame);
Location(const Vector3d &ekf_offset_neu, AltFrame frame);
static void set_terrain(AP_Terrain* terrain) { _terrain = terrain; }
@ -60,7 +61,7 @@ public:
bool get_vector_from_origin_NEU(Vector3f &vec_neu) const WARN_IF_UNUSED;
// return distance in meters between two locations
float get_distance(const struct Location &loc2) const;
ftype get_distance(const struct Location &loc2) const;
// return the distance in meters in North/East/Down plane as a N/E/D vector to loc2
Vector3f get_distance_NED(const Location &loc2) const;
@ -76,10 +77,10 @@ public:
void offset(ftype ofs_north, ftype ofs_east);
// extrapolate latitude/longitude given bearing and distance
void offset_bearing(float bearing_deg, float distance);
void offset_bearing(ftype bearing_deg, ftype distance);
// extrapolate latitude/longitude given bearing, pitch and distance
void offset_bearing_and_pitch(float bearing_deg, float pitch_deg, float distance);
void offset_bearing_and_pitch(ftype bearing_deg, ftype pitch_deg, ftype distance);
// longitude_scale - returns the scaler to compensate for
// shrinking longitude as you move north or south from the equator
@ -94,7 +95,7 @@ public:
// return bearing in centi-degrees from location to loc2
int32_t get_bearing_to(const struct Location &loc2) const;
// return the bearing in radians
float get_bearing(const struct Location &loc2) const { return radians(get_bearing_to(loc2) * 0.01f); } ;
ftype get_bearing(const struct Location &loc2) const { return radians(get_bearing_to(loc2) * 0.01); } ;
// check if lat and lng match. Ignore altitude and options
bool same_latlon_as(const Location &loc2) const;