Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -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)) { if (AP::ahrs().get_origin(ekf_origin)) {
lat = ekf_origin.lat; lat = ekf_origin.lat;
lng = ekf_origin.lng; 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 // 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 int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
const int64_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale(lat+dlat/2); 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); 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); offset_latlng(lat, lng, ofs_north, ofs_east);
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);
} }
/* /*
@ -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)); ftype scale = cosF(lat * (1.0e-7 * DEG_TO_RAD));
return MAX(scale, 0.01f); return MAX(scale, 0.01);
} }
/* /*

8
libraries/AP_Common/Location.h
View File

@ -69,8 +69,8 @@ public:
Vector2F get_distance_NE_ftype(const Location &loc2) const; Vector2F get_distance_NE_ftype(const Location &loc2) const;
// 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); static void offset_latlng(int32_t &lat, int32_t &lng, ftype ofs_north, ftype ofs_east);
void offset_double(double ofs_north, double ofs_east); void offset(ftype ofs_north, ftype ofs_east);
// extrapolate latitude/longitude given bearing and distance // extrapolate latitude/longitude given bearing and distance
void offset_bearing(float bearing_deg, float 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 // shrinking longitude as you move north or south from the equator
// Note: this does not include the scaling to convert // Note: this does not include the scaling to convert
// longitude/latitude points to meters or centimeters // 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; bool is_zero(void) const WARN_IF_UNUSED;
@ -128,7 +128,7 @@ public:
// get lon1-lon2, wrapping at -180e7 to 180e7 // get lon1-lon2, wrapping at -180e7 to 180e7
static int32_t diff_longitude(int32_t lon1, int32_t lon2); static int32_t diff_longitude(int32_t lon1, int32_t lon2);
private: private:
// scaling factor from 1e-7 degrees to meters at equator // scaling factor from 1e-7 degrees to meters at equator

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 home{test.home_lat, test.home_lng, 0, Location::AltFrame::ABOVE_HOME};
Location loc1 = home; Location loc1 = home;
Location loc2 = home; Location loc2 = home;
loc1.offset_double(test.delta_metres_ne1.x, test.delta_metres_ne1.y); loc1.offset(test.delta_metres_ne1.x, test.delta_metres_ne1.y);
loc2.offset_double(test.delta_metres_ne2.x, test.delta_metres_ne2.y); loc2.offset(test.delta_metres_ne2.x, test.delta_metres_ne2.y);
Vector2d diff = loc1.get_distance_NE_double(loc2); Vector2d diff = loc1.get_distance_NE_double(loc2);
EXPECT_FLOAT_EQ(diff.x, test.expected_pos_change.x); EXPECT_FLOAT_EQ(diff.x, test.expected_pos_change.x);
EXPECT_FLOAT_EQ(diff.y, test.expected_pos_change.y); EXPECT_FLOAT_EQ(diff.y, test.expected_pos_change.y);