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

3 files changed, 64 insertions, 3 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°)
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 71de33bcc..98887a0e4 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -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 */