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

AP_Common: added ftype precision offset call 

# Conflicts:
#	libraries/AP_Common/Location.cpp
#	libraries/AP_Common/Location.h
This commit is contained in:
Andrew Tridgell 2021-07-22 12:34:43 +10:00 committed by Randy Mackay
parent 87b4ca47e8
commit a8844a8428
3 changed files with 30 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
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)) { if (AP::ahrs().get_origin(ekf_origin)) {
lat = ekf_origin.lat; lat = ekf_origin.lat;
lng = ekf_origin.lng; 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(ekf_offset_neu.x * 0.01, ekf_offset_neu.y * 0.01);
} }
} }
@ -266,7 +282,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(); const int64_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / longitude_scale();
@ -275,12 +291,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 = limit_lattitude(int64_t(lat)+dlat);
lng = wrap_longitude(int64_t(lng)+dlng);
} }
/* /*
@ -308,10 +322,10 @@ void Location::offset_bearing_and_pitch(float bearing_deg, float pitch_deg, floa
} }
float Location::longitude_scale() const 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

@ -72,8 +72,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);
@ -85,7 +85,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
float longitude_scale() const; static ftype longitude_scale(int32_t lat);
bool is_zero(void) const WARN_IF_UNUSED; bool is_zero(void) const WARN_IF_UNUSED;
@ -131,7 +131,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:
static AP_Terrain *_terrain; static AP_Terrain *_terrain;

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);