Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Common: added ftype precision offset call

Browse Source
This commit is contained in:
Andrew Tridgell 2021-07-09 14:58:09 +10:00
parent ef48d706ed
commit 3ded1e6c10
3 changed files with 14 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Common/Location.cpp
View File

@ -61,7 +61,7 @@ Location::Location(const Vector3d &ekf_offset_neu, AltFrame frame)
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);
offset(ekf_offset_neu.x * 0.01, ekf_offset_neu.y * 0.01);
}
}
@ -284,7 +284,7 @@ Vector2F Location::get_distance_NE_ftype(const Location &loc2) const
}
// extrapolate latitude/longitude given distances (in meters) north and east
void Location::offset(float ofs_north, float ofs_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(lat+dlat/2);
@ -293,12 +293,10 @@ void Location::offset(float ofs_north, float ofs_east)
lng = wrap_longitude(dlng+lng);
}
void Location::offset_double(double ofs_north, double ofs_east)
// extrapolate latitude/longitude given distances (in meters) north and east
void Location::offset(ftype ofs_north, ftype 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(lat+dlat/2);
lat = limit_lattitude(int64_t(lat)+dlat);
lng = wrap_longitude(int64_t(lng)+dlng);
offset_latlng(lat, lng, ofs_north, ofs_east);
}
/*
@ -326,10 +324,10 @@ void Location::offset_bearing_and_pitch(float bearing_deg, float pitch_deg, floa
}
float Location::longitude_scale(int32_t lat)
ftype Location::longitude_scale(int32_t lat)
{
float scale = cosf(lat * (1.0e-7f * DEG_TO_RAD));
return MAX(scale, 0.01f);
ftype scale = cosF(lat * (1.0e-7 * DEG_TO_RAD));
return MAX(scale, 0.01);
}
/*

6
libraries/AP_Common/Location.h
View File

@ -69,8 +69,8 @@ public:
Vector2F get_distance_NE_ftype(const Location &loc2) const;
// extrapolate latitude/longitude given distances (in meters) north and east
void offset(float ofs_north, float ofs_east);
void offset_double(double ofs_north, double ofs_east);
static void offset_latlng(int32_t &lat, int32_t &lng, ftype ofs_north, ftype ofs_east);
void offset(ftype ofs_north, ftype ofs_east);
// extrapolate latitude/longitude given bearing and distance
void offset_bearing(float bearing_deg, float distance);
@ -82,7 +82,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
static float longitude_scale(int32_t lat);
static ftype longitude_scale(int32_t lat);
bool is_zero(void) const WARN_IF_UNUSED;

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

@ -146,8 +146,8 @@ TEST(Location, LocOffsetDouble)
Location home{test.home_lat, test.home_lng, 0, Location::AltFrame::ABOVE_HOME};
Location loc1 = home;
Location loc2 = home;
loc1.offset_double(test.delta_metres_ne1.x, test.delta_metres_ne1.y);
loc2.offset_double(test.delta_metres_ne2.x, test.delta_metres_ne2.y);
loc1.offset(test.delta_metres_ne1.x, test.delta_metres_ne1.y);
loc2.offset(test.delta_metres_ne2.x, test.delta_metres_ne2.y);
Vector2d diff = loc1.get_distance_NE_double(loc2);
EXPECT_FLOAT_EQ(diff.x, test.expected_pos_change.x);
EXPECT_FLOAT_EQ(diff.y, test.expected_pos_change.y);