geo: interface to get reference lat lon, option to set reference timestamp on initialization

2 files changed, 21 insertions, 3 deletions

diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 725a04a29..a2117ecd0 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -67,7 +67,7 @@ __EXPORT uint64_t map_projection_timestamp()
 	return mp_ref.timestamp;
 }
 
-__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
+__EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
 {
 	mp_ref.lat = lat_0 / 180.0 * M_PI;
 	mp_ref.lon = lon_0 / 180.0 * M_PI;
@@ -75,10 +75,22 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
 	mp_ref.sin_lat = sin(mp_ref.lat);
 	mp_ref.cos_lat = cos(mp_ref.lat);
 
-	mp_ref.timestamp = hrt_absolute_time();
+	mp_ref.timestamp = timestamp;
 	mp_ref.init_done = true;
 }
 
+__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon)
+{
+	if (!map_projection_initialized()) {
+		return false;
+	}
+
+	*ref_lat = mp_ref.lat;
+	*ref_lon = mp_ref.lon;
+
+	return true;
+}
+
 __EXPORT bool map_projection_project(double lat, double lon, float *x, float *y)
 {
 	if (!map_projection_initialized()) {
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 66e4dc75e..b5e659685 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -89,13 +89,19 @@ __EXPORT bool map_projection_initialized();
 __EXPORT uint64_t map_projection_timestamp();
 
 /**
+ * Writes the reference values to ref_lat and ref_lon
+ * @return true if map_projection_init was called before, false else
+ */
+__EXPORT bool map_projection_reference(double *ref_lat, double *ref_lon);
+
+/**
  * Initializes the 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°)
  */
-__EXPORT void map_projection_init(double lat_0, double lon_0);
+__EXPORT void map_projection_init(double lat_0, double lon_0, uint64_t timestamp);
 
 /**
  * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane