fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

geo: introduce global/local coordinate frame converter which uses the map projection but also converts altitude

Browse Source
This commit is contained in:
Thomas Gubler 2014-05-06 14:43:23 +02:00
parent fc204a1890
commit 548c7f4aaf
3 changed files with 64 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/lib/geo/geo.c
View File

@ -56,6 +56,7 @@
*/
static struct map_projection_reference_s mp_ref = {0};
static struct globallocal_converter_reference_s gl_ref = {0};
__EXPORT bool map_projection_global_initialized()
{
@ -185,6 +186,41 @@ __EXPORT int map_projection_reproject(const struct map_projection_reference_s *r
return 0;
}
__EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
{
if (strcmp("commander", getprogname() == 0)) {
gl_ref.alt = alt_0;
gl_ref.init_done = true;
return map_projection_global_init(lat_0, lon_0, timestamp);
} else {
return -1;
}
}
__EXPORT bool globallocalconverter_initialized()
{
return gl_ref.init_done && map_projection_global_initialized();
}
__EXPORT 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;
}
__EXPORT 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;
}
__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
{

26
src/lib/geo/geo.h
View File

@ -77,6 +77,11 @@ struct map_projection_reference_s {
uint64_t timestamp;
};
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
@ -190,6 +195,27 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub
*/
__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
/**
* Initialize the global mapping between global position (spherical) and local position (NED).
*/
__EXPORT 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
*/
__EXPORT bool globallocalconverter_initialized();
/**
* Convert from global position coordinates to local position coordinates using the global reference
*/
__EXPORT 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
*/
__EXPORT int globallocalconverter_toglobal(float x, float y, float z, double *lat, double *lon, float *alt);
/**
* Returns the distance to the next waypoint in meters.
*

5
src/modules/commander/commander.cpp
View File

@ -1132,9 +1132,8 @@ int commander_thread_main(int argc, char *argv[])
&& (gps_position.eph_m < eph_epv_threshold)
&& (gps_position.epv_m < eph_epv_threshold)
&& hrt_elapsed_time((hrt_abstime*)&gps_position.timestamp_position) < 1e6) {
/* set reference for map _projection */
map_projection_global_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, hrt_absolute_time());
/* set reference for global coordinates <--> local coordiantes conversion and map_projection */
globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time());
}
/* start RC input check */