From 548c7f4aaf93bddeb05053cd4dede945fede22ef Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 6 May 2014 14:43:23 +0200
Subject: geo: introduce global/local coordinate frame converter which uses the
 map projection but also converts altitude

---
 src/lib/geo/geo.c | 36 ++++++++++++++++++++++++++++++++++++
 src/lib/geo/geo.h | 26 ++++++++++++++++++++++++++
 2 files changed, 62 insertions(+)

(limited to 'src/lib')

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
@@ -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.
  *
-- 
cgit v1.2.3