fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 16:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

geo: Moved the map_projection_* functions and state into a self-contained C++ class

Browse Source
This commit is contained in:
Thomas Debrunner
2021-12-06 14:30:14 +01:00
committed by Matthias Grob
parent fe23718e2c
commit 8db7a6225b
38 changed files with 324 additions and 329 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/geo/geo.cpp
+22 -60
View File
@@ -60,61 +60,30 @@ using matrix::wrap_2pi;
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
*/
bool map_projection_initialized(const map_projection_reference_s *ref)
void MapProjection::initReference(double lat_0, double lon_0, uint64_t timestamp)
{
return ref->init_done;
_ref_lat = math::radians(lat_0);
_ref_lon = math::radians(lon_0);
_ref_sin_lat = sin(_ref_lat);
_ref_cos_lat = cos(_ref_lat);
_ref_timestamp = timestamp;
_ref_init_done = true;
}
uint64_t map_projection_timestamp(const map_projection_reference_s *ref)
void MapProjection::project(double lat, double lon, float &x, float &y) const
{
return ref->timestamp;
}
int map_projection_init_timestamped(map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp)
{
ref->lat_rad = math::radians(lat_0);
ref->lon_rad = math::radians(lon_0);
ref->sin_lat = sin(ref->lat_rad);
ref->cos_lat = cos(ref->lat_rad);
ref->timestamp = timestamp;
ref->init_done = true;
return 0;
}
int map_projection_init(map_projection_reference_s *ref, double lat_0, double lon_0)
{
return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
}
int map_projection_reference(const map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
{
if (!map_projection_initialized(ref)) {
return -1;
}
*ref_lat_rad = ref->lat_rad;
*ref_lon_rad = ref->lon_rad;
return 0;
}
int map_projection_project(const map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
{
if (!map_projection_initialized(ref)) {
return -1;
}
const double lat_rad = math::radians(lat);
const double lon_rad = math::radians(lon);
const double sin_lat = sin(lat_rad);
const double cos_lat = cos(lat_rad);
const double cos_d_lon = cos(lon_rad - ref->lon_rad);
const double cos_d_lon = cos(lon_rad - _ref_lon);
const double arg = math::constrain(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon, -1.0, 1.0);
const double arg = math::constrain(_ref_sin_lat * sin_lat + _ref_cos_lat * cos_lat * cos_d_lon, -1.0, 1.0);
const double c = acos(arg);
double k = 1.0;
@@ -123,18 +92,12 @@ int map_projection_project(const map_projection_reference_s *ref, double lat, do
k = (c / sin(c));
}
*x = static_cast<float>(k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH);
*y = static_cast<float>(k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH);
return 0;
x = static_cast<float>(k * (_ref_cos_lat * sin_lat - _ref_sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH);
y = static_cast<float>(k * cos_lat * sin(lon_rad - _ref_lon) * CONSTANTS_RADIUS_OF_EARTH);
}
int map_projection_reproject(const map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
void MapProjection::reproject(float x, float y, double &lat, double &lon) const
{
if (!map_projection_initialized(ref)) {
return -1;
}
const double x_rad = (double)x / CONSTANTS_RADIUS_OF_EARTH;
const double y_rad = (double)y / CONSTANTS_RADIUS_OF_EARTH;
const double c = sqrt(x_rad * x_rad + y_rad * y_rad);
@@ -143,18 +106,17 @@ int map_projection_reproject(const map_projection_reference_s *ref, float x, flo
const double sin_c = sin(c);
const double cos_c = cos(c);
const double lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
const double lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
const double lat_rad = asin(cos_c * _ref_sin_lat + (x_rad * sin_c * _ref_cos_lat) / c);
const double lon_rad = (_ref_lon + atan2(y_rad * sin_c, c * _ref_cos_lat * cos_c - x_rad * _ref_sin_lat * sin_c));
*lat = math::degrees(lat_rad);
*lon = math::degrees(lon_rad);
lat = math::degrees(lat_rad);
lon = math::degrees(lon_rad);
} else {
*lat = math::degrees(ref->lat_rad);
*lon = math::degrees(ref->lon_rad);
lat = math::degrees(_ref_lat);
lon = math::degrees(_ref_lon);
}
return 0;
}
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)