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);
+	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);
+	y = static_cast<float>(k * cos_lat * sin(lon_rad - _ref_lon) * CONSTANTS_RADIUS_OF_EARTH);
-
-	return 0;
 }
 
-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 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 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);
+		lat = math::degrees(lat_rad);
-		*lon = math::degrees(lon_rad);
+		lon = math::degrees(lon_rad);
 
 	} else {
-		*lat = math::degrees(ref->lat_rad);
+		lat = math::degrees(_ref_lat);
-		*lon = math::degrees(ref->lon_rad);
+		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;
+		proj.initReference(math::radians(473566094 / 1e7), math::radians(85190237 / 1e7), 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;
 	}
 
 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);

src/modules/commander/Commander.cpp

@@ -979,11 +979,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						home.manual_home = true;
 
 						// update local projection reference including altitude
-						struct map_projection_reference_s ref_pos;
+						MapProjection ref_pos{local_pos.ref_lat, local_pos.ref_lon};
-						map_projection_init(&ref_pos, local_pos.ref_lat, local_pos.ref_lon);
 						float home_x;
 						float home_y;
-						map_projection_project(&ref_pos, lat, lon, &home_x, &home_y);
+						ref_pos.project(lat, lon, home_x, home_y);
 						const float home_z = -(alt - local_pos.ref_alt);
 						fillLocalHomePos(home, home_x, home_y, home_z, yaw);
 
@@ -1688,11 +1687,10 @@ Commander::set_in_air_home_position()
 			if (_home_pub.get().valid_lpos) {
 				// Back-compute lon, lat and alt of home position given the home
 				// and current positions in local frame
-				map_projection_reference_s ref_pos;
+				MapProjection ref_pos{gpos.lat, gpos.lon};
 				double home_lat;
 				double home_lon;
-				map_projection_init(&ref_pos, gpos.lat, gpos.lon);
+				ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, home_lon);
-				map_projection_reproject(&ref_pos, home.x - lpos.x, home.y - lpos.y, &home_lat, &home_lon);
 				const float home_alt = gpos.alt + home.z;
 				fillGlobalHomePos(home, home_lat, home_lon, home_alt);
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -700,8 +700,8 @@ matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
 bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const
 {
 	origin_time = _last_gps_origin_time_us;
-	latitude    = math::degrees(_pos_ref.lat_rad);
+	latitude = _pos_ref.getProjectionReferenceLat();
-	longitude   = math::degrees(_pos_ref.lon_rad);
+	longitude = _pos_ref.getProjectionReferenceLon();
 	origin_alt  = _gps_alt_ref;
 	return _NED_origin_initialised;
 }
@@ -714,33 +714,29 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 	float current_alt  = 0.f;
 
 	// if we are already doing aiding, correct for the change in position since the EKF started navigating
-	if (map_projection_initialized(&_pos_ref) && isHorizontalAidingActive()) {
+	if (_pos_ref.isInitialized() && isHorizontalAidingActive()) {
-		map_projection_reproject(&_pos_ref, _state.pos(0), _state.pos(1), &current_lat, &current_lon);
+		_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
 		current_alt = -_state.pos(2) + _gps_alt_ref;
 		current_pos_available = true;
 	}
 
 	// reinitialize map projection to latitude, longitude, altitude, and reset position
-	if (map_projection_init_timestamped(&_pos_ref, latitude, longitude, _time_last_imu) == 0) {
+	_pos_ref.initReference(latitude, longitude, _time_last_imu);
-		if (current_pos_available) {
+	if (current_pos_available) {
-			// reset horizontal position
+		// reset horizontal position
-			Vector2f position;
+		Vector2f position = _pos_ref.project(current_lat, current_lon);
-			map_projection_project(&_pos_ref, current_lat, current_lon, &position(0), &position(1));
+		resetHorizontalPositionTo(position);
-			resetHorizontalPositionTo(position);
 
-			// reset altitude
+		// reset altitude
-			_gps_alt_ref = altitude;
+		_gps_alt_ref = altitude;
-			resetVerticalPositionTo(_gps_alt_ref - current_alt);
+		resetVerticalPositionTo(_gps_alt_ref - current_alt);
 
-		} else {
+	} else {
-			// reset altitude
+		// reset altitude
-			_gps_alt_ref = altitude;
+		_gps_alt_ref = altitude;
-		}
-
-		return true;
 	}
 
-	return false;
+	return true;
 }
 
 /*

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -197,7 +197,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 		if (collect_gps(gps)) {
 			float lpos_x = 0.0f;
 			float lpos_y = 0.0f;
-			map_projection_project(&_pos_ref, (gps.lat / 1.0e7), (gps.lon / 1.0e7), &lpos_x, &lpos_y);
+			_pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7), lpos_x, lpos_y);
 			gps_sample_new.pos(0) = lpos_x;
 			gps_sample_new.pos(1) = lpos_y;
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -253,7 +253,7 @@ public:
 	const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; }
 
 	const bool &global_origin_valid() const { return _NED_origin_initialised; }
-	const map_projection_reference_s &global_origin() const { return _pos_ref; }
+	const MapProjection &global_origin() const { return _pos_ref; }
 
 	void print_status();
 
@@ -324,8 +324,8 @@ protected:
 	bool _gps_speed_valid{false};
 	float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin
 	float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin
-	struct map_projection_reference_s _pos_ref {};   // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
+	MapProjection _pos_ref {};   // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
-	struct map_projection_reference_s _gps_pos_prev {};   // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
+	MapProjection _gps_pos_prev {};   // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
 	float _gps_alt_prev{0.0f};	// height from the previous GPS message (m)
 	float _gps_yaw_offset{0.0f};	// Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
 

src/modules/ekf2/EKF/gps_checks.cpp

@@ -65,22 +65,23 @@ bool Ekf::collect_gps(const gps_message &gps)
 		const double lat = gps.lat * 1.0e-7;
 		const double lon = gps.lon * 1.0e-7;
 
-		if (!map_projection_initialized(&_pos_ref)) {
+		if (!_pos_ref.isInitialized()) {
-			map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
+			_pos_ref.initReference(lat, lon, _time_last_imu);
 
 			// if we are already doing aiding, correct for the change in position since the EKF started navigating
 			if (isHorizontalAidingActive()) {
 				double est_lat;
 				double est_lon;
-				map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon);
+				_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
-				map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu);
+				_pos_ref.initReference(est_lat, est_lon, _time_last_imu);
 			}
 		}
 
 		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
 		_gps_alt_ref = 1e-3f * (float)gps.alt + _state.pos(2);
 		_NED_origin_initialised = true;
-		_earth_rate_NED = calcEarthRateNED((float)_pos_ref.lat_rad);
+
+		_earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat()));
 		_last_gps_origin_time_us = _time_last_imu;
 
 		const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps);
@@ -169,7 +170,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
 
 	// Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient
 	constexpr float filt_time_const = 10.0f;
-	const float dt = math::constrain(float(int64_t(_time_last_imu) - int64_t(_gps_pos_prev.timestamp)) * 1e-6f, 0.001f, filt_time_const);
+	const float dt = math::constrain(float(int64_t(_time_last_imu) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const);
 	const float filter_coef = dt / filt_time_const;
 
 	// The following checks are only valid when the vehicle is at rest
@@ -182,12 +183,12 @@ bool Ekf::gps_is_good(const gps_message &gps)
 		float delta_pos_e = 0.0f;
 
 		// calculate position movement since last GPS fix
-		if (_gps_pos_prev.timestamp > 0) {
+		if (_gps_pos_prev.getProjectionReferenceTimestamp() > 0) {
-			map_projection_project(&_gps_pos_prev, lat, lon, &delta_pos_n, &delta_pos_e);
+			_gps_pos_prev.project(lat, lon, delta_pos_n, delta_pos_e);
 
 		} else {
 			// no previous position has been set
-			map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
+			_gps_pos_prev.initReference(lat, lon, _time_last_imu);
 			_gps_alt_prev = 1e-3f * (float)gps.alt;
 		}
 
@@ -235,7 +236,7 @@ bool Ekf::gps_is_good(const gps_message &gps)
 	}
 
 	// save GPS fix for next time
-	map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
+	_gps_pos_prev.initReference(lat, lon, _time_last_imu);
 	_gps_alt_prev = 1e-3f * (float)gps.alt;
 
 	// Check  the filtered difference between GPS and EKF vertical velocity

src/modules/ekf2/EKF2.cpp

@@ -716,7 +716,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 		global_pos.timestamp_sample = timestamp;
 
 		// Position of local NED origin in GPS / WGS84 frame
-		map_projection_reproject(&_ekf.global_origin(), position(0), position(1), &global_pos.lat, &global_pos.lon);
+		_ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon);
 
 		float delta_xy[2];
 		_ekf.get_posNE_reset(delta_xy, &global_pos.lat_lon_reset_counter);
@@ -867,9 +867,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 
 	// Position of local NED origin in GPS / WGS84 frame
 	if (_ekf.global_origin_valid()) {
-		lpos.ref_timestamp = _ekf.global_origin().timestamp;
+		lpos.ref_timestamp = _ekf.global_origin().getProjectionReferenceTimestamp();
-		lpos.ref_lat = math::degrees(_ekf.global_origin().lat_rad); // Reference point latitude in degrees
+		lpos.ref_lat = _ekf.global_origin().getProjectionReferenceLat(); // Reference point latitude in degrees
-		lpos.ref_lon = math::degrees(_ekf.global_origin().lon_rad); // Reference point longitude in degrees
+		lpos.ref_lon = _ekf.global_origin().getProjectionReferenceLon(); // Reference point longitude in degrees
 		lpos.ref_alt = _ekf.getEkfGlobalOriginAltitude();           // Reference point in MSL altitude meters
 		lpos.xy_global = true;
 		lpos.z_global = true;

src/modules/ekf2/test/sensor_simulator/gps.cpp

@@ -98,11 +98,11 @@ void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change)
 	double lat_new {0.0};
 	double lon_new {0.0};
 
-	map_projection_project(&_ekf->global_origin(), _gps_data.lat * 1e-7, _gps_data.lon * 1e-7, &hposN_curr, &hposE_curr);
+	_ekf->global_origin().project(_gps_data.lat * 1e-7, _gps_data.lon * 1e-7, hposN_curr, hposE_curr);
 
 	Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change;
 
-	map_projection_reproject(&_ekf->global_origin(), hpos_new(0), hpos_new(1), &lat_new, &lon_new);
+	_ekf->global_origin().reproject(hpos_new(0), hpos_new(1), lat_new, lon_new);
 
 	_gps_data.lon = static_cast<int32_t>(lon_new * 1e7);
 	_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -365,8 +365,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_lock_position_xy.setAll(NAN);
 
 		// Convert from global to local frame.
-		map_projection_project(&_reference_position,
+		_reference_position.project(_sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon,
-				       _sub_triplet_setpoint.get().current.lat, _sub_triplet_setpoint.get().current.lon, &tmp_target(0), &tmp_target(1));
+					    tmp_target(0), tmp_target(1));
 	}
 
 	tmp_target(2) = -(_sub_triplet_setpoint.get().current.alt - _reference_altitude);
@@ -407,8 +407,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_prev_prev_wp = _triplet_prev_wp;
 
 		if (_isFinite(_sub_triplet_setpoint.get().previous) && _sub_triplet_setpoint.get().previous.valid) {
-			map_projection_project(&_reference_position, _sub_triplet_setpoint.get().previous.lat,
+			_reference_position.project(_sub_triplet_setpoint.get().previous.lat,
-					       _sub_triplet_setpoint.get().previous.lon, &_triplet_prev_wp(0), &_triplet_prev_wp(1));
+						    _sub_triplet_setpoint.get().previous.lon, _triplet_prev_wp(0), _triplet_prev_wp(1));
 			_triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude);
 
 		} else {
@@ -421,8 +421,8 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_triplet_next_wp = _triplet_target;
 
 		} else if (_isFinite(_sub_triplet_setpoint.get().next) && _sub_triplet_setpoint.get().next.valid) {
-			map_projection_project(&_reference_position, _sub_triplet_setpoint.get().next.lat,
+			_reference_position.project(_sub_triplet_setpoint.get().next.lat,
-					       _sub_triplet_setpoint.get().next.lon, &_triplet_next_wp(0), &_triplet_next_wp(1));
+						    _sub_triplet_setpoint.get().next.lon, _triplet_next_wp(0), _triplet_next_wp(1));
 			_triplet_next_wp(2) = -(_sub_triplet_setpoint.get().next.alt - _reference_altitude);
 
 		} else {
@@ -591,7 +591,7 @@ bool FlightTaskAuto::_evaluateGlobalReference()
 	}
 
 	// init projection
-	map_projection_init(&_reference_position, ref_lat, ref_lon);
+	_reference_position.initReference(ref_lat, ref_lon);
 
 	// check if everything is still finite
 	return PX4_ISFINITE(_reference_altitude) && PX4_ISFINITE(ref_lat) && PX4_ISFINITE(ref_lon);

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -196,7 +196,7 @@ private:
 	_triplet_next_wp; /**< next triplet from navigator which may differ from the intenal one (_next_wp) depending on the vehicle state.*/
 	matrix::Vector2f _closest_pt; /**< closest point to the vehicle position on the line previous - target */
 
-	map_projection_reference_s _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
+	MapProjection _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
 	float _reference_altitude{NAN};  /**< Altitude relative to ground. */
 	hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */
 

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp

@@ -154,12 +154,11 @@ void FlightTask::_evaluateVehicleLocalPosition()
 
 		// global frame reference coordinates to enable conversions
 		if (_sub_vehicle_local_position.get().xy_global && _sub_vehicle_local_position.get().z_global) {
-			if (!map_projection_initialized(&_global_local_proj_ref)
+			if (!_geo_projection.isInitialized()
-			    || (_global_local_proj_ref.timestamp != _sub_vehicle_local_position.get().ref_timestamp)) {
+			    || (_geo_projection.getProjectionReferenceTimestamp() != _sub_vehicle_local_position.get().ref_timestamp)) {
 
-				map_projection_init_timestamped(&_global_local_proj_ref,
+				_geo_projection.initReference(_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
-								_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
+							      _sub_vehicle_local_position.get().ref_timestamp);
-								_sub_vehicle_local_position.get().ref_timestamp);
 
 				_global_local_alt0 = _sub_vehicle_local_position.get().ref_alt;
 			}

src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp

@@ -206,7 +206,7 @@ protected:
 	virtual void _ekfResetHandlerVelocityZ(float delta_vz) {};
 	virtual void _ekfResetHandlerHeading(float delta_psi) {};
 
-	map_projection_reference_s _global_local_proj_ref{};
+	MapProjection 	_geo_projection{};
 	float                      _global_local_alt0{NAN};
 
 	/* Time abstraction */

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp

@@ -84,10 +84,8 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 
 	// commanded center coordinates
 	if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {
-		if (map_projection_initialized(&_global_local_proj_ref)) {
+		if (_geo_projection.isInitialized()) {
-			map_projection_project(&_global_local_proj_ref,
+			_center.xy() = _geo_projection.project(command.param5, command.param6);
-					       command.param5, command.param6,
-					       &_center(0), &_center(1));
 
 		} else {
 			ret = false;
@@ -96,7 +94,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 
 	// commanded altitude
 	if (PX4_ISFINITE(command.param7)) {
-		if (map_projection_initialized(&_global_local_proj_ref)) {
+		if (_geo_projection.isInitialized()) {
 			_center(2) = _global_local_alt0 - command.param7;
 
 		} else {
@@ -121,9 +119,9 @@ bool FlightTaskOrbit::sendTelemetry()
 	orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
 	orbit_status.yaw_behaviour = _yaw_behaviour;
 
-	if (map_projection_initialized(&_global_local_proj_ref)) {
+	if (_geo_projection.isInitialized()) {
 		// local -> global
-		map_projection_reproject(&_global_local_proj_ref, _center(0), _center(1), &orbit_status.x, &orbit_status.y);
+		_geo_projection.reproject(_center(0), _center(1), orbit_status.x, orbit_status.y);
 		orbit_status.z = _global_local_alt0 - _position_setpoint(2);
 
 	} else {

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1915,11 +1915,11 @@ FixedwingPositionControl::Run()
 
 		if (_control_mode.flag_control_offboard_enabled) {
 			// Convert Local setpoints to global setpoints
-			if (!map_projection_initialized(&_global_local_proj_ref)
+			if (!_global_local_proj_ref.isInitialized()
-			    || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
+			    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)) {
 
-				map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
+				_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
-								_local_pos.ref_timestamp);
+								     _local_pos.ref_timestamp);
 				_global_local_alt0 = _local_pos.ref_alt;
 			}
 
@@ -1930,7 +1930,8 @@ FixedwingPositionControl::Run()
 					double lat;
 					double lon;
 
-					if (map_projection_reproject(&_global_local_proj_ref, trajectory_setpoint.x, trajectory_setpoint.y, &lat, &lon) == 0) {
+					if (_global_local_proj_ref.isInitialized()) {
+						_global_local_proj_ref.reproject(trajectory_setpoint.x, trajectory_setpoint.y, lat, lon);
 						_pos_sp_triplet = {}; // clear any existing
 
 						_pos_sp_triplet.timestamp = trajectory_setpoint.timestamp;

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -172,7 +172,7 @@ private:
 
 	perf_counter_t	_loop_perf;				///< loop performance counter
 
-	map_projection_reference_s _global_local_proj_ref{};
+	MapProjection _global_local_proj_ref{};
 	float	_global_local_alt0{NAN};
 
 	float	_takeoff_ground_alt{0.0f};			///< ground altitude at which plane was launched

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -24,9 +24,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	// this block has no parent, and has name LPE
 	SuperBlock(nullptr, "LPE"),
 
-	// map projection
-	_map_ref(),
-
 	// flow gyro
 	_flow_gyro_x_high_pass(this, "FGYRO_HP"),
 	_flow_gyro_y_high_pass(this, "FGYRO_HP"),
@@ -123,9 +120,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 	_u.setZero();
 	initSS();
 
-	// map
-	_map_ref.init_done = false;
-
 	// print fusion settings to console
 	PX4_INFO("fuse gps: %d, flow: %d, vis_pos: %d, "
 		 "landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
@@ -176,7 +170,7 @@ void BlockLocalPositionEstimator::Run()
 				const double longitude = vehicle_command.param6;
 				const float altitude = vehicle_command.param7;
 
-				map_projection_init_timestamped(&_global_local_proj_ref, latitude, longitude, vehicle_command.timestamp);
+				_global_local_proj_ref.initReference(latitude, longitude, vehicle_command.timestamp);
 				_global_local_alt0 = altitude;
 
 				PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast<double>(altitude));
@@ -351,10 +345,10 @@ void BlockLocalPositionEstimator::Run()
 	checkTimeouts();
 
 	// if we have no lat, lon initialize projection to LPE_LAT, LPE_LON parameters
-	if (!_map_ref.init_done && (_estimatorInitialized & EST_XY) && _param_lpe_fake_origin.get()) {
+	if (!_map_ref.isInitialized() && (_estimatorInitialized & EST_XY) && _param_lpe_fake_origin.get()) {
-		map_projection_init(&_map_ref,
+		_map_ref.initReference(
-				    (double)_param_lpe_lat.get(),
+			(double)_param_lpe_lat.get(),
-				    (double)_param_lpe_lon.get());
+			(double)_param_lpe_lon.get());
 
 		// set timestamp when origin was set to current time
 		_time_origin = _timeStamp;
@@ -519,7 +513,7 @@ void BlockLocalPositionEstimator::Run()
 		_pub_innov_var.get().timestamp = hrt_absolute_time();
 		_pub_innov_var.update();
 
-		if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) {
+		if ((_estimatorInitialized & EST_XY) && (_map_ref.isInitialized() || _param_lpe_fake_origin.get())) {
 			publishGlobalPos();
 		}
 	}
@@ -623,8 +617,8 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().xy_global = _estimatorInitialized & EST_XY;
 		_pub_lpos.get().z_global = !(_sensorTimeout & SENSOR_BARO) && _altOriginGlobal;
 		_pub_lpos.get().ref_timestamp = _time_origin;
-		_pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI;
+		_pub_lpos.get().ref_lat = _map_ref.getProjectionReferenceLat();
-		_pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI;
+		_pub_lpos.get().ref_lon = _map_ref.getProjectionReferenceLon();
 		_pub_lpos.get().ref_alt = _altOrigin;
 		_pub_lpos.get().dist_bottom = _aglLowPass.getState();
 		// we estimate agl even when we don't have terrain info
@@ -785,7 +779,7 @@ void BlockLocalPositionEstimator::publishGlobalPos()
 	double lat = 0;
 	double lon = 0;
 	const Vector<float, n_x> &xLP = _xLowPass.getState();
-	map_projection_reproject(&_map_ref, xLP(X_x), xLP(X_y), &lat, &lon);
+	_map_ref.reproject(xLP(X_x), xLP(X_y), lat, lon);
 	float alt = -xLP(X_z) + _altOrigin;
 
 	// lie about eph/epv to allow visual odometry only navigation when velocity est. good

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -294,9 +294,9 @@ private:
 	uORB::PublicationData<estimator_innovations_s> _pub_innov_var{ORB_ID(estimator_innovation_variances)};
 
 	// map projection
-	struct map_projection_reference_s _map_ref;
+	MapProjection _map_ref;
 
-	map_projection_reference_s _global_local_proj_ref{};
+	MapProjection _global_local_proj_ref{};
 	float                      _global_local_alt0{NAN};
 
 	// target mode paramters from landing_target_estimator module

src/modules/local_position_estimator/sensors/gps.cpp

@@ -57,15 +57,15 @@ void BlockLocalPositionEstimator::gpsInit()
 			// find lat, lon of current origin by subtracting x and y
 			// if not using vision position since vision will
 			// have it's own origin, not necessarily where vehicle starts
-			if (!_map_ref.init_done) {
+			if (!_map_ref.isInitialized()) {
 				double gpsLatOrigin = 0;
 				double gpsLonOrigin = 0;
 				// reproject at current coordinates
-				map_projection_init(&_map_ref, gpsLat, gpsLon);
+				_map_ref.initReference(gpsLat, gpsLon);
 				// find origin
-				map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
+				_map_ref.reproject(-_x(X_x), -_x(X_y), gpsLatOrigin, gpsLonOrigin);
 				// reinit origin
-				map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);
+				_map_ref.initReference(gpsLatOrigin, gpsLonOrigin);
 				// set timestamp when origin was set to current time
 				_time_origin = _timeStamp;
 
@@ -119,7 +119,7 @@ void BlockLocalPositionEstimator::gpsCorrect()
 	float px = 0;
 	float py = 0;
 	float pz = -(alt - _gpsAltOrigin);
-	map_projection_project(&_map_ref, lat, lon, &px, &py);
+	_map_ref.project(lat, lon, px, py);
 	Vector<float, n_y_gps> y;
 	y.setZero();
 	y(Y_gps_x) = px;

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -37,17 +37,17 @@ void BlockLocalPositionEstimator::mocapInit()
 		_sensorFault &= ~SENSOR_MOCAP;
 
 		// get reference for global position
-		_ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
+		_ref_lat = _global_local_proj_ref.getProjectionReferenceLat();
-		_ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
+		_ref_lon = _global_local_proj_ref.getProjectionReferenceLon();
 		_ref_alt = _global_local_alt0;
 
-		_is_global_cov_init = map_projection_initialized(&_global_local_proj_ref);
+		_is_global_cov_init = _global_local_proj_ref.isInitialized();
 
-		if (!_map_ref.init_done && _is_global_cov_init && !_visionUpdated) {
+		if (!_map_ref.isInitialized() && _is_global_cov_init && !_visionUpdated) {
 			// initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well)
 			mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (mocap) : lat %6.2f lon %6.2f alt %5.1f m",
 					 double(_ref_lat), double(_ref_lon), double(_ref_alt));
-			map_projection_init(&_map_ref, _ref_lat, _ref_lon);
+			_map_ref.initReference(_ref_lat, _ref_lon);
 			// set timestamp when origin was set to current time
 			_time_origin = _timeStamp;
 		}
@@ -55,7 +55,7 @@ void BlockLocalPositionEstimator::mocapInit()
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
 			_altOriginGlobal = true;
-			_altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f;
+			_altOrigin = _global_local_proj_ref.isInitialized() ? _ref_alt : 0.0f;
 		}
 	}
 }

src/modules/local_position_estimator/sensors/vision.cpp

@@ -42,17 +42,17 @@ void BlockLocalPositionEstimator::visionInit()
 		_sensorFault &= ~SENSOR_VISION;
 
 		// get reference for global position
-		_ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
+		_ref_lat = _global_local_proj_ref.getProjectionReferenceLat();
-		_ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
+		_ref_lon = _global_local_proj_ref.getProjectionReferenceLon();
 		_ref_alt = _global_local_alt0;
 
-		_is_global_cov_init = map_projection_initialized(&_global_local_proj_ref);
+		_is_global_cov_init = _global_local_proj_ref.isInitialized();
 
-		if (!_map_ref.init_done && _is_global_cov_init) {
+		if (!_map_ref.isInitialized() && _is_global_cov_init) {
 			// initialize global origin using the visual estimator reference
 			mavlink_log_info(&mavlink_log_pub, "[lpe] global origin init (vision) : lat %6.2f lon %6.2f alt %5.1f m",
 					 double(_ref_lat), double(_ref_lon), double(_ref_alt));
-			map_projection_init(&_map_ref, _ref_lat, _ref_lon);
+			_map_ref.initReference(_ref_lat, _ref_lon);
 			// set timestamp when origin was set to current time
 			_time_origin = _timeStamp;
 		}
@@ -60,7 +60,7 @@ void BlockLocalPositionEstimator::visionInit()
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
 			_altOriginGlobal = true;
-			_altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f;
+			_altOrigin = _global_local_proj_ref.isInitialized() ? _ref_alt : 0.0f;
 		}
 	}
 }

src/modules/mavlink/mavlink_receiver.cpp

@@ -1150,13 +1150,12 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 				return;
 			}
 
-			map_projection_reference_s global_local_proj_ref{};
+			MapProjection global_local_proj_ref{local_pos.ref_lat, local_pos.ref_lon, local_pos.ref_timestamp};
-			map_projection_init_timestamped(&global_local_proj_ref, local_pos.ref_lat, local_pos.ref_lon, local_pos.ref_timestamp);
 
 			// global -> local
 			const double lat = target_global_int.lat_int / 1e7;
 			const double lon = target_global_int.lon_int / 1e7;
-			map_projection_project(&global_local_proj_ref, lat, lon, &setpoint.x, &setpoint.y);
+			global_local_proj_ref.project(lat, lon, setpoint.x, setpoint.y);
 
 			if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_INT) {
 				setpoint.z = local_pos.ref_alt - target_global_int.alt;
@@ -2705,20 +2704,20 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		const double lat = hil_state.lat * 1e-7;
 		const double lon = hil_state.lon * 1e-7;
 
-		if (!map_projection_initialized(&_global_local_proj_ref) || !PX4_ISFINITE(_global_local_alt0)) {
+		if (!_global_local_proj_ref.isInitialized() || !PX4_ISFINITE(_global_local_alt0)) {
-			map_projection_init(&_global_local_proj_ref, lat, lon);
+			_global_local_proj_ref.initReference(lat, lon);
 			_global_local_alt0 = hil_state.alt / 1000.f;
 		}
 
 		float x = 0.f;
 		float y = 0.f;
-		map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y);
+		_global_local_proj_ref.project(lat, lon, x, y);
 
 		vehicle_local_position_s hil_local_pos{};
 		hil_local_pos.timestamp_sample = timestamp_sample;
-		hil_local_pos.ref_timestamp = _global_local_proj_ref.timestamp;
+		hil_local_pos.ref_timestamp = _global_local_proj_ref.getProjectionReferenceTimestamp();
-		hil_local_pos.ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
+		hil_local_pos.ref_lat = _global_local_proj_ref.getProjectionReferenceLat();
-		hil_local_pos.ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
+		hil_local_pos.ref_lon = _global_local_proj_ref.getProjectionReferenceLon();
 		hil_local_pos.ref_alt = _global_local_alt0;
 		hil_local_pos.xy_valid = true;
 		hil_local_pos.z_valid = true;

src/modules/mavlink/mavlink_receiver.h

@@ -362,7 +362,7 @@ private:
 	PX4Magnetometer *_px4_mag{nullptr};
 
 	float _global_local_alt0{NAN};
-	map_projection_reference_s _global_local_proj_ref{};
+	MapProjection _global_local_proj_ref{};
 
 	hrt_abstime			_last_utm_global_pos_com{0};
 

src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp

@@ -52,17 +52,15 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
 {
 
 	GeofenceBreachAvoidance gf_avoidance(nullptr);
-	struct map_projection_reference_s ref = {};
 	Vector2d home_global(42.1, 8.2);
-	map_projection_init(&ref, home_global(0), home_global(1));
+
+	MapProjection ref{home_global(0), home_global(1)};
 
 	Vector2f waypoint_north_east_local(1.0, 1.0);
 	waypoint_north_east_local = waypoint_north_east_local.normalized() * 10.5;
 	Vector2d waypoint_north_east_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, 10.5);
 
-	float x, y;
+	Vector2f waypoint_north_east_reprojected = ref.project(waypoint_north_east_global(0), waypoint_north_east_global(1));
-	map_projection_project(&ref, waypoint_north_east_global(0), waypoint_north_east_global(1), &x, &y);
-	Vector2f waypoint_north_east_reprojected(x, y);
 
 	EXPECT_FLOAT_EQ(waypoint_north_east_local(0), waypoint_north_east_reprojected(0));
 	EXPECT_FLOAT_EQ(waypoint_north_east_local(1), waypoint_north_east_reprojected(1));
@@ -72,8 +70,7 @@ TEST_F(GeofenceBreachAvoidanceTest, waypointFromBearingAndDistance)
 
 	Vector2d waypoint_southwest_global = gf_avoidance.waypointFromBearingAndDistance(home_global, M_PI_F * 0.25f, -10.5);
 
-	map_projection_project(&ref, waypoint_southwest_global(0), waypoint_southwest_global(1), &x, &y);
+	Vector2f waypoint_south_west_reprojected = ref.project(waypoint_southwest_global(0), waypoint_southwest_global(1));
-	Vector2f waypoint_south_west_reprojected(x, y);
 
 	EXPECT_FLOAT_EQ(waypoint_south_west_local(0), waypoint_south_west_reprojected(0));
 	EXPECT_FLOAT_EQ(waypoint_south_west_local(1), waypoint_south_west_reprojected(1));
@@ -88,9 +85,8 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing)
 {
 	GeofenceBreachAvoidance gf_avoidance(nullptr);
 	FakeGeofence geo;
-	struct map_projection_reference_s ref = {};
 	Vector2d home_global(42.1, 8.2);
-	map_projection_init(&ref, home_global(0), home_global(1));
+	MapProjection ref{home_global(0), home_global(1)};
 
 	geofence_violation_type_u gf_violation;
 	gf_violation.flags.fence_violation = true;
@@ -136,9 +132,8 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor)
 {
 	GeofenceBreachAvoidance gf_avoidance(nullptr);
 	FakeGeofence geo;
-	struct map_projection_reference_s ref = {};
 	Vector2d home_global(42.1, 8.2);
-	map_projection_init(&ref, home_global(0), home_global(1));
+	MapProjection ref{42.1, 8.2};
 
 	param_t param = param_handle(px4::params::MPC_ACC_HOR);
 
@@ -245,9 +240,8 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter)
 {
 	GeofenceBreachAvoidance gf_avoidance(nullptr);
 	FakeGeofence geo;
-	struct map_projection_reference_s ref = {};
 	Vector2d home_global(42.1, 8.2);
-	map_projection_init(&ref, home_global(0), home_global(1));
+	MapProjection ref{home_global(0), home_global(1)};
 	geofence_violation_type_u gf_violation;
 	gf_violation.flags.dist_to_home_exceeded = true;
 
@@ -278,9 +272,8 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing)
 {
 	GeofenceBreachAvoidance gf_avoidance(nullptr);
 	FakeGeofence geo;
-	struct map_projection_reference_s ref = {};
 	Vector2d home_global(42.1, 8.2);
-	map_projection_init(&ref, home_global(0), home_global(1));
+	MapProjection ref{home_global(0), home_global(1)};
 	geofence_violation_type_u gf_violation;
 	gf_violation.flags.dist_to_home_exceeded = true;
 

src/modules/navigator/GeofenceBreachAvoidance/fake_geofence.hpp

@@ -122,13 +122,10 @@ private:
 
 	bool _gf_boundary_is_20m_north(double lat, double lon, float alt)
 	{
-		struct map_projection_reference_s ref = {};
 		matrix::Vector2<double> home_global(42.1, 8.2);
-		map_projection_init(&ref, home_global(0), home_global(1));
 
-		float x, y;
+		MapProjection projection{home_global(0), home_global(1)};
-		map_projection_project(&ref, lat, lon, &x, &y);
+		matrix::Vector2f waypoint_local = projection.project(lat, lon);
-		matrix::Vector2f waypoint_local(x, y);
 
 		if (waypoint_local(0) >= 20.0f) {
 			return false;

src/modules/navigator/follow_target.cpp

@@ -94,7 +94,7 @@ void FollowTarget::on_activation()
 
 void FollowTarget::on_active()
 {
-	struct map_projection_reference_s target_ref;
+	MapProjection target_ref;
 	follow_target_s target_motion_with_offset = {};
 	uint64_t current_time = hrt_absolute_time();
 	bool radius_entered = false;
@@ -134,9 +134,9 @@ void FollowTarget::on_active()
 
 		// get distance to target
 
-		map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+		target_ref.initReference(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-		map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
+		target_ref.project(_current_target_motion.lat, _current_target_motion.lon, _target_distance(0),
-				       &_target_distance(1));
+				   _target_distance(1));
 
 	}
 
@@ -149,11 +149,11 @@ void FollowTarget::on_active()
 		// ignore a small dt
 		if (dt_ms > 10.0F) {
 			// get last gps known reference for target
-			map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);
+			target_ref.initReference(_previous_target_motion.lat, _previous_target_motion.lon);
 
 			// calculate distance the target has moved
-			map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon,
+			target_ref.project(_current_target_motion.lat, _current_target_motion.lon,
-					       &(_target_position_delta(0)), &(_target_position_delta(1)));
+					   (_target_position_delta(0)), (_target_position_delta(1)));
 
 			// update the average velocity of the target based on the position
 			if (PX4_ISFINITE(_current_target_motion.vx) && PX4_ISFINITE(_current_target_motion.vy)) {
@@ -229,9 +229,9 @@ void FollowTarget::on_active()
 
 		// get the target position using the calculated offset
 
-		map_projection_init(&target_ref,  _current_target_motion.lat, _current_target_motion.lon);
+		target_ref.initReference(_current_target_motion.lat, _current_target_motion.lon);
-		map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1),
+		target_ref.reproject(_target_position_offset(0), _target_position_offset(1),
-					 &target_motion_with_offset.lat, &target_motion_with_offset.lon);
+				     target_motion_with_offset.lat, target_motion_with_offset.lon);
 	}
 
 	// clamp yaw rate smoothing if we are with in

src/modules/navigator/geofence.cpp

@@ -447,13 +447,13 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon,
 		return false;
 	}
 
-	if (!map_projection_initialized(&_projection_reference)) {
+	if (!_projection_reference.isInitialized()) {
-		map_projection_init(&_projection_reference, lat, lon);
+		_projection_reference.initReference(lat, lon);
 	}
 
 	float x1, y1, x2, y2;
-	map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
+	_projection_reference.project(lat, lon, x1, y1);
-	map_projection_project(&_projection_reference, circle_point.lat, circle_point.lon, &x2, &y2);
+	_projection_reference.project(circle_point.lat, circle_point.lon, x2, y2);
 	float dx = x1 - x2, dy = y1 - y2;
 	return dx * dx + dy * dy < circle_point.circle_radius * circle_point.circle_radius;
 }

src/modules/navigator/geofence.h

@@ -173,7 +173,7 @@ private:
 	PolygonInfo *_polygons{nullptr};
 	int _num_polygons{0};
 
-	map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
+	MapProjection _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::GF_ACTION>) _param_gf_action,

src/modules/navigator/mission_block.cpp

@@ -248,17 +248,14 @@ MissionBlock::is_mission_item_reached()
 			if (curr_sp->valid) {
 
 				// location of gate (mission item)
-				struct map_projection_reference_s ref_pos;
+				MapProjection ref_pos{_mission_item.lat, _mission_item.lon};
-				map_projection_init(&ref_pos, _mission_item.lat, _mission_item.lon);
 
 				// current setpoint
-				matrix::Vector2f gate_to_curr_sp;
+				matrix::Vector2f gate_to_curr_sp = ref_pos.project(curr_sp->lat, curr_sp->lon);
-				map_projection_project(&ref_pos, curr_sp->lat, curr_sp->lon, &gate_to_curr_sp(0), &gate_to_curr_sp(1));
 
 				// system position
-				matrix::Vector2f vehicle_pos;
+				matrix::Vector2f vehicle_pos = ref_pos.project(_navigator->get_global_position()->lat,
-				map_projection_project(&ref_pos, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+							       _navigator->get_global_position()->lon);
-						       &vehicle_pos(0), &vehicle_pos(1));
 				const float dot_product = vehicle_pos.dot(gate_to_curr_sp.normalized());
 
 				// if the dot product (projected vector) is positive, then

src/modules/navigator/precland.cpp

@@ -78,8 +78,8 @@ PrecLand::on_activation()
 
 	vehicle_local_position_s *vehicle_local_position = _navigator->get_local_position();
 
-	if (!map_projection_initialized(&_map_ref)) {
+	if (!_map_ref.isInitialized()) {
-		map_projection_init(&_map_ref, vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
+		_map_ref.initReference(vehicle_local_position->ref_lat, vehicle_local_position->ref_lon);
 	}
 
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -279,7 +279,7 @@ PrecLand::run_state_horizontal_approach()
 
 	// XXX need to transform to GPS coords because mc_pos_control only looks at that
 	double lat, lon;
-	map_projection_reproject(&_map_ref, x, y, &lat, &lon);
+	_map_ref.reproject(x, y, lat, lon);
 
 	pos_sp_triplet->current.lat = lat;
 	pos_sp_triplet->current.lon = lon;
@@ -316,7 +316,7 @@ PrecLand::run_state_descend_above_target()
 
 	// XXX need to transform to GPS coords because mc_pos_control only looks at that
 	double lat, lon;
-	map_projection_reproject(&_map_ref, _target_pose.x_abs, _target_pose.y_abs, &lat, &lon);
+	_map_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, lat, lon);
 
 	pos_sp_triplet->current.lat = lat;
 	pos_sp_triplet->current.lon = lon;
@@ -555,8 +555,8 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
 		dt = 50000 / SEC2USEC;
 
 		// set a best guess for previous setpoints for smooth transition
-		map_projection_project(&_map_ref, _navigator->get_position_setpoint_triplet()->current.lat,
+		_map_ref.project(_navigator->get_position_setpoint_triplet()->current.lat,
-				       _navigator->get_position_setpoint_triplet()->current.lon, &_sp_pev(0), &_sp_pev(1));
+				 _navigator->get_position_setpoint_triplet()->current.lon, _sp_pev(0), _sp_pev(1));
 		_sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt;
 		_sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt;
 	}

src/modules/navigator/precland.h

@@ -111,7 +111,7 @@ private:
 	bool _target_pose_valid{false}; /**< whether we have received a landing target position message */
 	bool _target_pose_updated{false}; /**< wether the landing target position message is updated */
 
-	struct map_projection_reference_s _map_ref {}; /**< reference for local/global projections */
+	MapProjection _map_ref {}; /**< reference for local/global projections */
 
 	uint64_t _state_start_time{0}; /**< time when we entered current state */
 	uint64_t _last_slewrate_time{0}; /**< time when we last limited setpoint changes */

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -419,11 +419,11 @@ RoverPositionControl::Run()
 
 			// Convert Local setpoints to global setpoints
 			if (_control_mode.flag_control_offboard_enabled) {
-				if (!map_projection_initialized(&_global_local_proj_ref)
+				if (!_global_local_proj_ref.isInitialized()
-				    || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
+				    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp)) {
 
-					map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
+					_global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon,
-									_local_pos.ref_timestamp);
+									     _local_pos.ref_timestamp);
 
 					_global_local_alt0 = _local_pos.ref_alt;
 				}
@@ -431,9 +431,9 @@ RoverPositionControl::Run()
 				_trajectory_setpoint_sub.update(&_trajectory_setpoint);
 
 				// local -> global
-				map_projection_reproject(&_global_local_proj_ref,
+				_global_local_proj_ref.reproject(
-							 _trajectory_setpoint.x, _trajectory_setpoint.y,
+					_trajectory_setpoint.x, _trajectory_setpoint.y,
-							 &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon);
+					_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon);
 
 				_pos_sp_triplet.current.alt = _global_local_alt0 - _trajectory_setpoint.z;
 				_pos_sp_triplet.current.valid = true;

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -134,7 +134,7 @@ private:
 	hrt_abstime _control_position_last_called{0}; 	/**<last call of control_position  */
 	hrt_abstime _manual_setpoint_last_called{0};
 
-	map_projection_reference_s _global_local_proj_ref{};
+	MapProjection _global_local_proj_ref{};
 	float                      _global_local_alt0{NAN};
 
 	/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on

src/modules/simulator/simulator.h

@@ -264,7 +264,7 @@ private:
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 
 	// hil map_ref data
-	map_projection_reference_s	_global_local_proj_ref{};
+	MapProjection	_global_local_proj_ref{};
 	float						_global_local_alt0{NAN};
 
 	vehicle_status_s _vehicle_status{};

src/modules/simulator/simulator_mavlink.cpp

@@ -542,14 +542,14 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
 		double lat = hil_state.lat * 1e-7;
 		double lon = hil_state.lon * 1e-7;
 
-		if (!map_projection_initialized(&_global_local_proj_ref)) {
+		if (!_global_local_proj_ref.isInitialized()) {
-			map_projection_init(&_global_local_proj_ref, lat, lon);
+			_global_local_proj_ref.initReference(lat, lon);
 			_global_local_alt0 = hil_state.alt / 1000.f;
 		}
 
 		float x;
 		float y;
-		map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y);
+		_global_local_proj_ref.project(lat, lon, x, y);
 		hil_lpos.timestamp = timestamp;
 		hil_lpos.xy_valid = true;
 		hil_lpos.z_valid = true;
@@ -569,9 +569,9 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
 		hil_lpos.heading = euler.psi();
 		hil_lpos.xy_global = true;
 		hil_lpos.z_global = true;
-		hil_lpos.ref_timestamp = _global_local_proj_ref.timestamp;
+		hil_lpos.ref_timestamp = _global_local_proj_ref.getProjectionReferenceTimestamp();
-		hil_lpos.ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
+		hil_lpos.ref_lat = _global_local_proj_ref.getProjectionReferenceLat();
-		hil_lpos.ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
+		hil_lpos.ref_lon = _global_local_proj_ref.getProjectionReferenceLon();
 		hil_lpos.ref_alt = _global_local_alt0;
 		hil_lpos.vxy_max = std::numeric_limits<float>::infinity();
 		hil_lpos.vz_max = std::numeric_limits<float>::infinity();

src/modules/vmount/output.cpp

@@ -75,13 +75,13 @@ void OutputBase::publish()
 float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
 				   const vehicle_global_position_s &global_position)
 {
-	if (!map_projection_initialized(&_projection_reference)) {
+	if (!_projection_reference.isInitialized()) {
-		map_projection_init(&_projection_reference, global_position.lat, global_position.lon);
+		_projection_reference.initReference(global_position.lat, global_position.lon);
 	}
 
 	float x1, y1, x2, y2;
-	map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
+	_projection_reference.project(lat, lon, x1, y1);
-	map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2);
+	_projection_reference.project(global_position.lat, global_position.lon, x2, y2);
 	float dx = x1 - x2, dy = y1 - y2;
 	float target_distance = sqrtf(dx * dx + dy * dy);
 	float z = altitude - global_position.alt;

src/modules/vmount/output.h

@@ -101,7 +101,7 @@ protected:
 	float _calculate_pitch(double lon, double lat, float altitude,
 			       const vehicle_global_position_s &global_position);
 
-	map_projection_reference_s _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
+	MapProjection _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
 
 	const OutputConfig &_config;