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-01 09:18:33 +10:00
parent a8c6d742aa
commit 6a3b12956a
3 changed files with 50 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
libraries/AP_Common/Location.cpp
View File

@ -47,7 +47,23 @@ Location::Location(const Vector3f &ekf_offset_neu, AltFrame frame)
if (AP::ahrs().get_origin(ekf_origin)) {
lat = ekf_origin.lat;
lng = ekf_origin.lng;
offset(ekf_offset_neu.x / 100.0f, ekf_offset_neu.y / 100.0f);
offset(ekf_offset_neu.x * 0.01, ekf_offset_neu.y * 0.01);
}
}
Location::Location(const Vector3d &ekf_offset_neu, AltFrame frame)
{
zero();
// store alt and alt frame
set_alt_cm(ekf_offset_neu.z, frame);
// calculate lat, lon
Location ekf_origin;
if (AP::ahrs().get_origin(ekf_origin)) {
lat = ekf_origin.lat;
lng = ekf_origin.lng;
offset_double(ekf_offset_neu.x * 0.01, ekf_offset_neu.y * 0.01);
}
}
@ -194,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;
}
@ -222,7 +238,7 @@ bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const
float 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();
float dlng = ((float)diff_longitude(loc2.lng,lng)) * longitude_scale((lat+loc2.lat)/2);
return norm(dlat, dlng) * LOCATION_SCALING_FACTOR;
}
@ -234,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);
}
@ -249,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(float ofs_north, float 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);
@ -278,7 +294,7 @@ void Location::offset(float ofs_north, float ofs_east)
void Location::offset_double(double ofs_north, double ofs_east)
{
const int64_t dlat = ofs_north * double(LOCATION_SCALING_FACTOR_INV);
const int64_t dlng = (ofs_east * double(LOCATION_SCALING_FACTOR_INV)) / longitude_scale();
const int64_t dlng = (ofs_east * double(LOCATION_SCALING_FACTOR_INV)) / longitude_scale(lat+dlat/2);
lat = limit_lattitude(int64_t(lat)+dlat);
lng = wrap_longitude(int64_t(lng)+dlng);
}
@ -308,7 +324,7 @@ void Location::offset_bearing_and_pitch(float bearing_deg, float pitch_deg, floa
}
float Location::longitude_scale() const
float Location::longitude_scale(int32_t lat)
{
float scale = cosf(lat * (1.0e-7f * DEG_TO_RAD));
return MAX(scale, 0.01f);
@ -352,7 +368,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;

3
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; }
@ -85,7 +86,7 @@ public:
// 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;
static float longitude_scale(int32_t lat);
bool is_zero(void) const WARN_IF_UNUSED;

21
libraries/AP_Common/tests/test_location.cpp
View File

@ -139,7 +139,7 @@ TEST(Location, LocOffsetDouble)
-353632620, 1491652373,
Vector2d{4682795.4576701336, 5953662.7673837934},
Vector2d{4682797.1904749088, 5953664.1586009059},
Vector2d{1.7365739867091179,1.7025913001452864},
Vector2d{1.7365739867091179,1.4261966},
};
for (auto &test : tests) {
@ -409,4 +409,23 @@ TEST(Location, Line)
EXPECT_TRUE(test_wp.past_interval_finish_line(test_home, test_wp_last));
}
/*
check if we obey basic euclidian geometry rules of position
addition/subtraction
*/
TEST(Location, OffsetError)
{
// test at 30km from origin
const float ofs_ne = 10e3 / sqrtf(2.0);
for (float lat = -80; lat <= 80; lat += 10.0) {
Location origin{int32_t(lat*1e7), 0, 0, Location::AltFrame::ABOVE_HOME};
Location loc = origin;
loc.offset(ofs_ne, ofs_ne);
Location loc2 = loc;
loc2.offset(-ofs_ne, -ofs_ne);
float dist = origin.get_distance(loc2);
EXPECT_FLOAT_EQ(dist, 0);
}
}
AP_GTEST_MAIN()