diff options
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/geo/geo.c | 36 | ||||
-rw-r--r-- | src/lib/geo/geo.h | 26 |
2 files changed, 62 insertions, 0 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 220ea149d..c9d48491e 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -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) { diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 20fb34ca3..98dfbd14b 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -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 @@ -191,6 +196,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. * * @param lat_now current position in degrees (47.1234567°, not 471234567°) |