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)) {
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(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
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();
@ -275,12 +291,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 = limit_lattitude(int64_t(lat)+dlat);
lng = wrap_longitude(int64_t(lng)+dlng);
offset_latlng(lat, lng, ofs_north, ofs_east);
}
/*
@ -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));
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

@ -72,8 +72,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);
@ -85,7 +85,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 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);