aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Common: added double methods for SITL

Browse Source
This commit is contained in:
Andrew Tridgell 2021-06-22 15:44:00 +10:00
parent fb275c9874
commit 61faeb2d7f
2 changed files with 19 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Common/Location.cpp
View File

@ -242,7 +242,15 @@ Vector3f Location::get_distance_NED(const Location &loc2) const
{
return Vector3f((loc2.lat - lat) * LOCATION_SCALING_FACTOR,
(loc2.lng - lng) * LOCATION_SCALING_FACTOR * longitude_scale(),
(alt - loc2.alt) * 0.01f);
(alt - loc2.alt) * 0.01);
}
// return the distance in meters in North/East/Down plane as a N/E/D vector to loc2
Vector3d Location::get_distance_NED_double(const Location &loc2) const
{
return Vector3d((loc2.lat - lat) * double(LOCATION_SCALING_FACTOR),
(loc2.lng - lng) * double(LOCATION_SCALING_FACTOR) * longitude_scale(),
(alt - loc2.alt) * 0.01);
}
// extrapolate latitude/longitude given distances (in meters) north and east
@ -254,6 +262,14 @@ void Location::offset(float ofs_north, float ofs_east)
lng += dlng;
}
void Location::offset_double(double ofs_north, double ofs_east)
{
const int32_t dlat = ofs_north * double(LOCATION_SCALING_FACTOR_INV);
const int32_t dlng = (ofs_east * double(LOCATION_SCALING_FACTOR_INV)) / longitude_scale();
lat += dlat;
lng += dlng;
}
/*
* extrapolate latitude/longitude given bearing and distance
* Note that this function is accurate to about 1mm at a distance of

2
libraries/AP_Common/Location.h
View File

@ -64,12 +64,14 @@ public:
// return the distance in meters in North/East/Down plane as a N/E/D vector to loc2
Vector3f get_distance_NED(const Location &loc2) const;
Vector3d get_distance_NED_double(const Location &loc2) const;
// return the distance in meters in North/East plane as a N/E vector to loc2
Vector2f get_distance_NE(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);
// extrapolate latitude/longitude given bearing and distance
void offset_bearing(float bearing_deg, float distance);