geo: Moved the map_projection_* functions and state into a self-contained C++ class

src/lib/geo/geo.cpp

@@ -60,61 +60,30 @@ using matrix::wrap_2pi;
  * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
  */
 
-bool map_projection_initialized(const map_projection_reference_s *ref)
+
+void MapProjection::initReference(double lat_0, double lon_0, uint64_t timestamp)
 {
-	return ref->init_done;
+	_ref_lat = math::radians(lat_0);
+	_ref_lon = math::radians(lon_0);
+	_ref_sin_lat = sin(_ref_lat);
+	_ref_cos_lat = cos(_ref_lat);
+
+	_ref_timestamp = timestamp;
+	_ref_init_done = true;
 }
 
-uint64_t map_projection_timestamp(const map_projection_reference_s *ref)
+
+void MapProjection::project(double lat, double lon, float &x, float &y) const
 {
-	return ref->timestamp;
-}
-
-int map_projection_init_timestamped(map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp)
-{
-	ref->lat_rad = math::radians(lat_0);
-	ref->lon_rad = math::radians(lon_0);
-	ref->sin_lat = sin(ref->lat_rad);
-	ref->cos_lat = cos(ref->lat_rad);
-
-	ref->timestamp = timestamp;
-	ref->init_done = true;
-
-	return 0;
-}
-
-int map_projection_init(map_projection_reference_s *ref, double lat_0, double lon_0)
-{
-	return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time());
-}
-
-int map_projection_reference(const map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad)
-{
-	if (!map_projection_initialized(ref)) {
-		return -1;
-	}
-
-	*ref_lat_rad = ref->lat_rad;
-	*ref_lon_rad = ref->lon_rad;
-
-	return 0;
-}
-
-int map_projection_project(const map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
-{
-	if (!map_projection_initialized(ref)) {
-		return -1;
-	}
-
 	const double lat_rad = math::radians(lat);
 	const double lon_rad = math::radians(lon);
 
 	const double sin_lat = sin(lat_rad);
 	const double cos_lat = cos(lat_rad);
 
-	const double cos_d_lon = cos(lon_rad - ref->lon_rad);
+	const double cos_d_lon = cos(lon_rad - _ref_lon);
 
-	const double arg = math::constrain(ref->sin_lat * sin_lat + ref->cos_lat * cos_lat * cos_d_lon, -1.0,  1.0);
+	const double arg = math::constrain(_ref_sin_lat * sin_lat + _ref_cos_lat * cos_lat * cos_d_lon, -1.0,  1.0);
 	const double c = acos(arg);
 
 	double k = 1.0;
@@ -123,18 +92,12 @@ int map_projection_project(const map_projection_reference_s *ref, double lat, do
 		k = (c / sin(c));
 	}
 
-	*x = static_cast<float>(k * (ref->cos_lat * sin_lat - ref->sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH);
-	*y = static_cast<float>(k * cos_lat * sin(lon_rad - ref->lon_rad) * CONSTANTS_RADIUS_OF_EARTH);
-
-	return 0;
+	x = static_cast<float>(k * (_ref_cos_lat * sin_lat - _ref_sin_lat * cos_lat * cos_d_lon) * CONSTANTS_RADIUS_OF_EARTH);
+	y = static_cast<float>(k * cos_lat * sin(lon_rad - _ref_lon) * CONSTANTS_RADIUS_OF_EARTH);
 }
 
-int map_projection_reproject(const map_projection_reference_s *ref, float x, float y, double *lat, double *lon)
+void MapProjection::reproject(float x, float y, double &lat, double &lon) const
 {
-	if (!map_projection_initialized(ref)) {
-		return -1;
-	}
-
 	const double x_rad = (double)x / CONSTANTS_RADIUS_OF_EARTH;
 	const double y_rad = (double)y / CONSTANTS_RADIUS_OF_EARTH;
 	const double c = sqrt(x_rad * x_rad + y_rad * y_rad);
@@ -143,18 +106,17 @@ int map_projection_reproject(const map_projection_reference_s *ref, float x, flo
 		const double sin_c = sin(c);
 		const double cos_c = cos(c);
 
-		const double lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
-		const double lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));
+		const double lat_rad = asin(cos_c * _ref_sin_lat + (x_rad * sin_c * _ref_cos_lat) / c);
+		const double lon_rad = (_ref_lon + atan2(y_rad * sin_c, c * _ref_cos_lat * cos_c - x_rad * _ref_sin_lat * sin_c));
 
-		*lat = math::degrees(lat_rad);
-		*lon = math::degrees(lon_rad);
+		lat = math::degrees(lat_rad);
+		lon = math::degrees(lon_rad);
 
 	} else {
-		*lat = math::degrees(ref->lat_rad);
-		*lon = math::degrees(ref->lon_rad);
+		lat = math::degrees(_ref_lat);
+		lon = math::degrees(_ref_lon);
 	}
 
-	return 0;
 }
 
 float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)

src/lib/geo/geo.h

@@ -48,6 +48,10 @@
 #include <stdbool.h>
 #include <stdint.h>
 
+#include <matrix/Matrix.hpp>
+#include <mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
+
 static constexpr float CONSTANTS_ONE_G = 9.80665f;						// m/s^2
 
 static constexpr float CONSTANTS_STD_PRESSURE_PA = 101325.0f;					// pascals (Pa)
@@ -72,76 +76,6 @@ struct crosstrack_error_s {
 	float bearing;		// Bearing in radians to closest point on line/arc
 } ;
 
-/* lat/lon are in radians */
-struct map_projection_reference_s {
-	uint64_t timestamp;
-	double lat_rad;
-	double lon_rad;
-	double sin_lat;
-	double cos_lat;
-	bool init_done;
-};
-
-/**
- * Checks if projection given as argument was initialized
- * @return true if map was initialized before, false else
- */
-bool map_projection_initialized(const struct map_projection_reference_s *ref);
-
-/**
- * Get the timestamp of the map projection given by the argument
- * @return the timestamp of the map_projection
- */
-uint64_t map_projection_timestamp(const struct map_projection_reference_s *ref);
-
-/**
- * Writes the reference values of the projection given by the argument to ref_lat and ref_lon
- * @return 0 if map_projection_init was called before, -1 else
- */
-int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad);
-
-/**
- * Initializes the map transformation given by the argument.
- *
- * 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°)
- */
-int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0,
-				    uint64_t timestamp);
-
-/**
- * Initializes the map transformation given by the argument and sets the timestamp to now.
- *
- * 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°)
- */
-int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0);
-
-/* Transforms a point in the geographic coordinate system to the local
- * azimuthal equidistant plane using the projection given by the argument
-* @param x north
-* @param y east
-* @param lat in degrees (47.1234567°, not 471234567°)
-* @param lon in degrees (8.1234567°, not 81234567°)
-* @return 0 if map_projection_init was called before, -1 else
-*/
-int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y);
-
-/**
- * Transforms a point in the local azimuthal equidistant plane to the
- * geographic coordinate system using the projection given by the argument
- *
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- * @return 0 if map_projection_init was called before, -1 else
- */
-int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon);
 
 /**
  * Returns the distance to the next waypoint in meters.
@@ -221,3 +155,134 @@ float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float a
 float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
 		float x_next, float y_next, float z_next,
 		float *dist_xy, float *dist_z);
+
+
+/**
+ * @brief C++ class for mapping lat/lon coordinates to local coordinated using a reference position
+ */
+class MapProjection final
+{
+private:
+	uint64_t _ref_timestamp{0};
+	double _ref_lat{0.f};
+	double _ref_lon{0.f};
+	double _ref_sin_lat{0.f};
+	double _ref_cos_lat{0.f};
+	bool _ref_init_done {false};
+
+public:
+	/**
+	 * @brief Construct a new Map Projection object
+	 * The generated object will be uninitialized.
+	 * To initialize, use the `initReference` function
+	 */
+	MapProjection() = default;
+
+	/**
+	 * @brief Construct and initializes a new Map Projection object
+	 */
+	MapProjection(double lat_0, double lon_0)
+	{
+		initReference(lat_0, lon_0);
+	}
+
+	/**
+	 * @brief Construct and initializes a new Map Projection object
+	 */
+	MapProjection(double lat_0, double lon_0, uint64_t timestamp)
+	{
+		initReference(lat_0, lon_0, timestamp);
+	}
+
+	/**
+	 * 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°)
+	 */
+	void initReference(double lat_0, double lon_0, uint64_t timestamp);
+
+	/**
+	 * 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°)
+	 */
+	inline void initReference(double lat_0, double lon_0)
+	{
+		initReference(lat_0, lon_0, hrt_absolute_time());
+	}
+
+	/**
+	 * @brief Returns true, if the map reference has been initialized before
+	 */
+	inline bool isInitialized() const
+	{
+		return _ref_init_done;
+	}
+
+	/**
+	 * Get the timestamp of the map projection
+	 * @return the timestamp of the map_projection
+	 */
+	inline uint64_t getProjectionReferenceTimestamp() const
+	{
+		return _ref_timestamp;
+	}
+
+	/**
+	 * @brief Get the Projection Reference latitude in degrees
+	 */
+	inline double getProjectionReferenceLat() const
+	{
+		return math::degrees(_ref_lat);
+	}
+
+	/**
+	 * @brief Get the Projection Reference longitude in degrees
+	 */
+	inline double getProjectionReferenceLon() const
+	{
+		return math::degrees(_ref_lon);
+	}
+
+	/**
+	 * Transforms a point in the geographic coordinate system to the local
+	 * azimuthal equidistant plane using the projection
+	 * @param x north
+	 * @param y east
+	 * @param lat in degrees (47.1234567°, not 471234567°)
+	 * @param lon in degrees (8.1234567°, not 81234567°)
+	 */
+	void project(double lat, double lon, float &x, float &y) const;
+
+	/**
+	 * Transforms a point in the geographic coordinate system to the local
+	 * azimuthal equidistant plane using the projection
+	 * @param lat in degrees (47.1234567°, not 471234567°)
+	 * @param lon in degrees (8.1234567°, not 81234567°)
+	 * @return the point in local coordinates as north / east
+	 */
+	inline matrix::Vector2f project(double lat, double lon) const
+	{
+		matrix::Vector2f res;
+		project(lat, lon, res(0), res(1));
+		return res;
+	}
+
+	/**
+	 * Transforms a point in the local azimuthal equidistant plane to the
+	 * geographic coordinate system using the projection
+	 *
+	 * @param x north
+	 * @param y east
+	 * @param lat in degrees (47.1234567°, not 471234567°)
+	 * @param lon in degrees (8.1234567°, not 81234567°)
+	 * @return 0 if map_projection_init was called before, -1 else
+	 */
+	void reproject(float x, float y, double &lat, double &lon) const;
+};

src/lib/geo/test_geo.cpp

@@ -42,16 +42,11 @@ class GeoTest : public ::testing::Test
 public:
 	void SetUp() override
 	{
-		origin.timestamp = 0;
-		origin.lat_rad = math::radians(473566094 / 1e7);
-		origin.lon_rad = math::radians(85190237 / 1e7);
-		origin.sin_lat = sin(origin.lat_rad);
-		origin.cos_lat = cos(origin.lat_rad);
-		origin.init_done = true;
+		proj.initReference(math::radians(473566094 / 1e7), math::radians(85190237 / 1e7), 0);
 	}
 
 protected:
-	map_projection_reference_s origin;
+	MapProjection proj;
 
 };
 
@@ -63,13 +58,13 @@ TEST_F(GeoTest, reprojectProject)
 	float y = 1;
 	double lat;
 	double lon;
-	map_projection_reproject(&origin, x, y, &lat, &lon);
+	proj.reproject(x, y, lat, lon);
 	float x_new;
 	float y_new;
-	map_projection_project(&origin, lat, lon, &x_new, &y_new);
+	proj.project(lat, lon, x_new, y_new);
 	double lat_new;
 	double lon_new;
-	map_projection_reproject(&origin, x_new, y_new, &lat_new, &lon_new);
+	proj.reproject(x_new, y_new, lat_new, lon_new);
 	EXPECT_FLOAT_EQ(x, x_new);
 	EXPECT_FLOAT_EQ(y, y_new);
 	EXPECT_FLOAT_EQ(lat, lat_new);
@@ -83,13 +78,13 @@ TEST_F(GeoTest, projectReproject)
 	double lon = 8.5190505981445313;
 	float x;
 	float y;
-	map_projection_project(&origin, lat, lon, &x, &y);
+	proj.project(lat, lon, x, y);
 	double lat_new;
 	double lon_new;
-	map_projection_reproject(&origin, x, y, &lat_new, &lon_new);
+	proj.reproject(x, y, lat_new, lon_new);
 	float x_new;
 	float y_new;
-	map_projection_project(&origin, lat_new, lon_new, &x_new, &y_new);
+	proj.project(lat_new, lon_new, x_new, y_new);
 	// WHEN: apply the mapping and its inverse the output should stay the same
 	EXPECT_FLOAT_EQ(x, x_new);
 	EXPECT_FLOAT_EQ(y, y_new);