mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-26 09:26:25 +08:00
geo: purge old globallocal_converter
This commit is contained in:
-111
@@ -57,39 +57,19 @@ using matrix::wrap_2pi;
|
|||||||
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
|
* formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
|
||||||
*/
|
*/
|
||||||
|
|
||||||
static struct map_projection_reference_s mp_ref;
|
|
||||||
static struct globallocal_converter_reference_s gl_ref = {0.0f, false};
|
|
||||||
|
|
||||||
bool map_projection_global_initialized()
|
|
||||||
{
|
|
||||||
return map_projection_initialized(&mp_ref);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool map_projection_initialized(const struct map_projection_reference_s *ref)
|
bool map_projection_initialized(const struct map_projection_reference_s *ref)
|
||||||
{
|
{
|
||||||
return ref->init_done;
|
return ref->init_done;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint64_t map_projection_global_timestamp()
|
|
||||||
{
|
|
||||||
return map_projection_timestamp(&mp_ref);
|
|
||||||
}
|
|
||||||
|
|
||||||
uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
|
uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref)
|
||||||
{
|
{
|
||||||
return ref->timestamp;
|
return ref->timestamp;
|
||||||
}
|
}
|
||||||
|
|
||||||
// lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
|
|
||||||
int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp)
|
|
||||||
{
|
|
||||||
return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp);
|
|
||||||
}
|
|
||||||
|
|
||||||
// lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
|
// lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
|
||||||
int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp)
|
int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp)
|
||||||
{
|
{
|
||||||
|
|
||||||
ref->lat_rad = math::radians(lat_0);
|
ref->lat_rad = math::radians(lat_0);
|
||||||
ref->lon_rad = math::radians(lon_0);
|
ref->lon_rad = math::radians(lon_0);
|
||||||
ref->sin_lat = sin(ref->lat_rad);
|
ref->sin_lat = sin(ref->lat_rad);
|
||||||
@@ -107,11 +87,6 @@ int map_projection_init(struct map_projection_reference_s *ref, double lat_0, do
|
|||||||
return map_projection_init_timestamped(ref, lat_0, lon_0, ecl_absolute_time());
|
return map_projection_init_timestamped(ref, lat_0, lon_0, ecl_absolute_time());
|
||||||
}
|
}
|
||||||
|
|
||||||
int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad)
|
|
||||||
{
|
|
||||||
return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad);
|
|
||||||
}
|
|
||||||
|
|
||||||
int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
|
int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
|
||||||
{
|
{
|
||||||
if (!map_projection_initialized(ref)) {
|
if (!map_projection_initialized(ref)) {
|
||||||
@@ -124,11 +99,6 @@ int map_projection_reference(const struct map_projection_reference_s *ref, doubl
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int map_projection_global_project(double lat, double lon, float *x, float *y)
|
|
||||||
{
|
|
||||||
return map_projection_project(&mp_ref, lat, lon, x, y);
|
|
||||||
}
|
|
||||||
|
|
||||||
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
|
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
|
||||||
{
|
{
|
||||||
if (!map_projection_initialized(ref)) {
|
if (!map_projection_initialized(ref)) {
|
||||||
@@ -158,11 +128,6 @@ int map_projection_project(const struct map_projection_reference_s *ref, double
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int map_projection_global_reproject(float x, float y, double *lat, double *lon)
|
|
||||||
{
|
|
||||||
return map_projection_reproject(&mp_ref, x, y, lat, lon);
|
|
||||||
}
|
|
||||||
|
|
||||||
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
|
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
|
||||||
{
|
{
|
||||||
if (!map_projection_initialized(ref)) {
|
if (!map_projection_initialized(ref)) {
|
||||||
@@ -191,82 +156,6 @@ int map_projection_reproject(const struct map_projection_reference_s *ref, float
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int map_projection_global_getref(double *lat_0, double *lon_0)
|
|
||||||
{
|
|
||||||
if (!map_projection_global_initialized()) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (lat_0 != nullptr) {
|
|
||||||
*lat_0 = math::degrees(mp_ref.lat_rad);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (lon_0 != nullptr) {
|
|
||||||
*lon_0 = math::degrees(mp_ref.lon_rad);
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
|
|
||||||
}
|
|
||||||
int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
|
|
||||||
{
|
|
||||||
gl_ref.alt = alt_0;
|
|
||||||
|
|
||||||
if (!map_projection_global_init(lat_0, lon_0, timestamp)) {
|
|
||||||
gl_ref.init_done = true;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
gl_ref.init_done = false;
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool globallocalconverter_initialized()
|
|
||||||
{
|
|
||||||
return gl_ref.init_done && map_projection_global_initialized();
|
|
||||||
}
|
|
||||||
|
|
||||||
int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z)
|
|
||||||
{
|
|
||||||
if (!map_projection_global_initialized()) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
map_projection_global_project(lat, lon, x, y);
|
|
||||||
*z = gl_ref.alt - alt;
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt)
|
|
||||||
{
|
|
||||||
if (!map_projection_global_initialized()) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
map_projection_global_reproject(x, y, lat, lon);
|
|
||||||
*alt = gl_ref.alt - z;
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0)
|
|
||||||
{
|
|
||||||
if (map_projection_global_initialized() != 0) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (map_projection_global_getref(lat_0, lon_0)) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (alt_0 != nullptr) {
|
|
||||||
*alt_0 = gl_ref.alt;
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
|
float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
|
||||||
{
|
{
|
||||||
const double lat_now_rad = math::radians(lat_now);
|
const double lat_now_rad = math::radians(lat_now);
|
||||||
|
|||||||
@@ -81,57 +81,24 @@ struct map_projection_reference_s {
|
|||||||
bool init_done;
|
bool init_done;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct globallocal_converter_reference_s {
|
|
||||||
float alt;
|
|
||||||
bool init_done;
|
|
||||||
};
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Checks if global projection was initialized
|
|
||||||
* @return true if map was initialized before, false else
|
|
||||||
*/
|
|
||||||
bool map_projection_global_initialized();
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Checks if projection given as argument was initialized
|
* Checks if projection given as argument was initialized
|
||||||
* @return true if map was initialized before, false else
|
* @return true if map was initialized before, false else
|
||||||
*/
|
*/
|
||||||
bool map_projection_initialized(const struct map_projection_reference_s *ref);
|
bool map_projection_initialized(const struct map_projection_reference_s *ref);
|
||||||
|
|
||||||
/**
|
|
||||||
* Get the timestamp of the global map projection
|
|
||||||
* @return the timestamp of the map_projection
|
|
||||||
*/
|
|
||||||
uint64_t map_projection_global_timestamp(void);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Get the timestamp of the map projection given by the argument
|
* Get the timestamp of the map projection given by the argument
|
||||||
* @return the timestamp of the map_projection
|
* @return the timestamp of the map_projection
|
||||||
*/
|
*/
|
||||||
uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
|
uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
|
||||||
|
|
||||||
/**
|
|
||||||
* Writes the reference values of the global projection to ref_lat and ref_lon
|
|
||||||
* @return 0 if map_projection_init was called before, -1 else
|
|
||||||
*/
|
|
||||||
int map_projection_global_reference(double *ref_lat_rad, double *ref_lon_rad);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Writes the reference values of the projection given by the argument to ref_lat and ref_lon
|
* Writes the reference values of the projection given by the argument to ref_lat and ref_lon
|
||||||
* @return 0 if map_projection_init was called before, -1 else
|
* @return 0 if map_projection_init was called before, -1 else
|
||||||
*/
|
*/
|
||||||
int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad);
|
int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad);
|
||||||
|
|
||||||
/**
|
|
||||||
* Initializes the global map transformation.
|
|
||||||
*
|
|
||||||
* Initializes the transformation between the geographic coordinate system and
|
|
||||||
* the azimuthal equidistant plane
|
|
||||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
|
||||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
|
||||||
*/
|
|
||||||
int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Initializes the map transformation given by the argument.
|
* Initializes the map transformation given by the argument.
|
||||||
*
|
*
|
||||||
@@ -152,17 +119,6 @@ int map_projection_init_timestamped(struct map_projection_reference_s *ref, doub
|
|||||||
*/
|
*/
|
||||||
int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
|
int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
|
||||||
|
|
||||||
/**
|
|
||||||
* Transforms a point in the geographic coordinate system to the local
|
|
||||||
* azimuthal equidistant plane using the global projection
|
|
||||||
* @param x north
|
|
||||||
* @param y east
|
|
||||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
|
||||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
|
||||||
* @return 0 if map_projection_init was called before, -1 else
|
|
||||||
*/
|
|
||||||
int map_projection_global_project(double lat, double lon, float *x, float *y);
|
|
||||||
|
|
||||||
/* Transforms a point in the geographic coordinate system to the local
|
/* Transforms a point in the geographic coordinate system to the local
|
||||||
* azimuthal equidistant plane using the projection given by the argument
|
* azimuthal equidistant plane using the projection given by the argument
|
||||||
* @param x north
|
* @param x north
|
||||||
@@ -173,18 +129,6 @@ int map_projection_global_project(double lat, double lon, float *x, float *y);
|
|||||||
*/
|
*/
|
||||||
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
|
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
|
||||||
|
|
||||||
/**
|
|
||||||
* Transforms a point in the local azimuthal equidistant plane to the
|
|
||||||
* geographic coordinate system using the global projection
|
|
||||||
*
|
|
||||||
* @param x north
|
|
||||||
* @param y east
|
|
||||||
* @param lat in degrees (47.1234567°, not 471234567°)
|
|
||||||
* @param lon in degrees (8.1234567°, not 81234567°)
|
|
||||||
* @return 0 if map_projection_init was called before, -1 else
|
|
||||||
*/
|
|
||||||
int map_projection_global_reproject(float x, float y, double *lat, double *lon);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Transforms a point in the local azimuthal equidistant plane to the
|
* Transforms a point in the local azimuthal equidistant plane to the
|
||||||
* geographic coordinate system using the projection given by the argument
|
* geographic coordinate system using the projection given by the argument
|
||||||
@@ -197,37 +141,6 @@ int map_projection_global_reproject(float x, float y, double *lat, double *lon);
|
|||||||
*/
|
*/
|
||||||
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
|
int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
|
||||||
|
|
||||||
/**
|
|
||||||
* Get reference position of the global map projection
|
|
||||||
*/
|
|
||||||
int map_projection_global_getref(double *lat_0, double *lon_0);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Initialize the global mapping between global position (spherical) and local position (NED).
|
|
||||||
*/
|
|
||||||
int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Checks if globallocalconverter was initialized
|
|
||||||
* @return true if map was initialized before, false else
|
|
||||||
*/
|
|
||||||
bool globallocalconverter_initialized(void);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Convert from global position coordinates to local position coordinates using the global reference
|
|
||||||
*/
|
|
||||||
int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Convert from local position coordinates to global position coordinates using the global reference
|
|
||||||
*/
|
|
||||||
int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Get reference position of the global to local converter
|
|
||||||
*/
|
|
||||||
int globallocalconverter_getref(double *lat_0, double *lon_0, float *alt_0);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Returns the distance to the next waypoint in meters.
|
* Returns the distance to the next waypoint in meters.
|
||||||
*
|
*
|
||||||
|
|||||||
Reference in New Issue
Block a user