geo: refactoring on comments and usage

src/lib/geo/geo.cpp

@@ -44,8 +44,6 @@
 
 #include "geo.h"
 
-#include <mathlib/mathlib.h>
-#include <matrix/math.hpp>
 #include <float.h>
 
 using matrix::wrap_pi;
@@ -60,19 +58,16 @@ using matrix::wrap_2pi;
  * formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html
  */
 
-
 void MapProjection::initReference(double lat_0, double lon_0, uint64_t timestamp)
 {
+	_ref_timestamp = timestamp;
 	_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;
 }
 
-
 void MapProjection::project(double lat, double lon, float &x, float &y) const
 {
 	const double lat_rad = math::radians(lat);
@@ -116,7 +111,6 @@ void MapProjection::reproject(float x, float y, double &lat, double &lon) const
 		lat = math::degrees(_ref_lat);
 		lon = math::degrees(_ref_lon);
 	}
-
 }
 
 float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)

src/lib/geo/geo.h

@@ -48,9 +48,8 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include <matrix/Matrix.hpp>
+#include <lib/mathlib/mathlib.h>
-#include <mathlib/mathlib.h>
+#include <lib/matrix/matrix/math.hpp>
-#include <drivers/drv_hrt.h>
 
 static constexpr float CONSTANTS_ONE_G = 9.80665f;						// m/s^2
 
@@ -164,11 +163,11 @@ class MapProjection final
 {
 private:
 	uint64_t _ref_timestamp{0};
-	double _ref_lat{0.f};
+	double _ref_lat{0.0};
-	double _ref_lon{0.f};
+	double _ref_lon{0.0};
-	double _ref_sin_lat{0.f};
+	double _ref_sin_lat{0.0};
-	double _ref_cos_lat{0.f};
+	double _ref_cos_lat{0.0};
-	bool _ref_init_done {false};
+	bool _ref_init_done{false};
 
 public:
 	/**
@@ -179,7 +178,7 @@ public:
 	MapProjection() = default;
 
 	/**
-	 * @brief Construct and initializes a new Map Projection object
+	 * @brief Construct and initialize a new Map Projection object
 	 */
 	MapProjection(double lat_0, double lon_0)
 	{
@@ -187,7 +186,7 @@ public:
 	}
 
 	/**
-	 * @brief Construct and initializes a new Map Projection object
+	 * @brief Construct and initialize a new Map Projection object
 	 */
 	MapProjection(double lat_0, double lon_0, uint64_t timestamp)
 	{
@@ -195,7 +194,7 @@ public:
 	}
 
 	/**
-	 * Initializes the map transformation
+	 * Initialize the map transformation
 	 *
 	 * Initializes the transformation between the geographic coordinate system and
 	 * the azimuthal equidistant plane
@@ -205,10 +204,10 @@ public:
 	void initReference(double lat_0, double lon_0, uint64_t timestamp);
 
 	/**
-	 * Initializes the map transformation
+	 * Initialize the map transformation
 	 *
-	 * Initializes the transformation between the geographic coordinate system and
+	 * with reference coordinates on the geographic coordinate system
-	 * the azimuthal equidistant plane
+	 * where the azimuthal equidistant plane's origin is located
 	 * @param lat in degrees (47.1234567°, not 471234567°)
 	 * @param lon in degrees (8.1234567°, not 81234567°)
 	 */
@@ -218,50 +217,37 @@ public:
 	}
 
 	/**
-	 * @brief Returns true, if the map reference has been initialized before
+	 * @return true, if the map reference has been initialized before
 	 */
-	inline bool isInitialized() const
+	bool isInitialized() const { return _ref_init_done; };
-	{
-		return _ref_init_done;
-	}
 
 	/**
-	 * Get the timestamp of the map projection
+	 * @return the timestamp of the reference which the map projection was initialized with
-	 * @return the timestamp of the map_projection
 	 */
-	inline uint64_t getProjectionReferenceTimestamp() const
+	uint64_t getProjectionReferenceTimestamp() const { return _ref_timestamp; };
-	{
-		return _ref_timestamp;
-	}
 
 	/**
-	 * @brief Get the Projection Reference latitude in degrees
+	 * @return the projection reference latitude in degrees
 	 */
-	inline double getProjectionReferenceLat() const
+	double getProjectionReferenceLat() const { return math::degrees(_ref_lat); };
-	{
-		return math::degrees(_ref_lat);
-	}
 
 	/**
-	 * @brief Get the Projection Reference longitude in degrees
+	 * @return the projection reference longitude in degrees
 	 */
-	inline double getProjectionReferenceLon() const
+	double getProjectionReferenceLon() const { return math::degrees(_ref_lon); };
-	{
-		return math::degrees(_ref_lon);
-	}
 
 	/**
-	 * Transforms a point in the geographic coordinate system to the local
+	 * Transform 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°)
+	 * @param x north
+	 * @param y east
 	 */
 	void project(double lat, double lon, float &x, float &y) const;
 
 	/**
-	 * Transforms a point in the geographic coordinate system to the local
+	 * Transform 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°)
@@ -275,14 +261,13 @@ public:
 	}
 
 	/**
-	 * Transforms a point in the local azimuthal equidistant plane to the
+	 * Transform 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/modules/ekf2/EKF/ekf.h

@@ -157,7 +157,7 @@ public:
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
 	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
-	bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude);
+	void setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude);
 
 	float getEkfGlobalOriginAltitude() const { return _gps_alt_ref; }
 	bool setEkfGlobalOriginAltitude(const float altitude);

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -706,7 +706,7 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo
 	return _NED_origin_initialised;
 }
 
-bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude)
+void Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude)
 {
 	bool current_pos_available = false;
 	double current_lat = static_cast<double>(NAN);
@@ -735,8 +735,6 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
 		// reset altitude
 		_gps_alt_ref = altitude;
 	}
-
-	return true;
 }
 
 /*

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -195,11 +195,7 @@ void EstimatorInterface::setGpsData(const gps_message &gps)
 
 		// Only calculate the relative position if the WGS-84 location of the origin is set
 		if (collect_gps(gps)) {
-			float lpos_x = 0.0f;
+			gps_sample_new.pos = _pos_ref.project((gps.lat / 1.0e7), (gps.lon / 1.0e7));
-			float lpos_y = 0.0f;
-			_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;
 
 		} else {
 			gps_sample_new.pos(0) = 0.0f;

src/modules/ekf2/EKF/estimator_interface.h

@@ -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
-	MapProjection _pos_ref {};   // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
+	MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
-	MapProjection _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 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/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp

@@ -196,8 +196,8 @@ 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 */
 
-	MapProjection _reference_position{}; /**< Structure used to project lat/lon setpoint into local frame. */
+	MapProjection _reference_position{}; /**< Class used to project lat/lon setpoint into local frame. */
-	float _reference_altitude{NAN};  /**< Altitude relative to ground. */
+	float _reference_altitude{NAN}; /**< Altitude relative to ground. */
 	hrt_abstime _time_stamp_reference{0}; /**< time stamp when last reference update occured. */
 
 	WeatherVane *_ext_yaw_handler{nullptr};	/**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */

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

@@ -206,8 +206,8 @@ protected:
 	virtual void _ekfResetHandlerVelocityZ(float delta_vz) {};
 	virtual void _ekfResetHandlerHeading(float delta_psi) {};
 
-	MapProjection 	_geo_projection{};
+	MapProjection _geo_projection{};
-	float                      _global_local_alt0{NAN};
+	float _global_local_alt0{NAN};
 
 	/* Time abstraction */
 	static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -297,7 +297,7 @@ private:
 	MapProjection _map_ref;
 
 	MapProjection _global_local_proj_ref{};
-	float                      _global_local_alt0{NAN};
+	float _global_local_alt0{NAN};
 
 	// target mode paramters from landing_target_estimator module
 	enum TargetMode {

src/modules/navigator/follow_target.cpp

@@ -135,8 +135,8 @@ void FollowTarget::on_active()
 		// get distance to target
 
 		target_ref.initReference(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-		target_ref.project(_current_target_motion.lat, _current_target_motion.lon, _target_distance(0),
+		target_ref.project(_current_target_motion.lat, _current_target_motion.lon,
-				   _target_distance(1));
+				   _target_distance(0), _target_distance(1));
 
 	}
 

src/modules/navigator/geofence.h

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

src/modules/navigator/precland.cpp

@@ -278,11 +278,8 @@ PrecLand::run_state_horizontal_approach()
 	slewrate(x, y);
 
 	// XXX need to transform to GPS coords because mc_pos_control only looks at that
-	double lat, lon;
+	_map_ref.reproject(x, y, pos_sp_triplet->current.lat, pos_sp_triplet->current.lon);
-	_map_ref.reproject(x, y, lat, lon);
 
-	pos_sp_triplet->current.lat = lat;
-	pos_sp_triplet->current.lon = lon;
 	pos_sp_triplet->current.alt = _approach_alt;
 	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 
@@ -315,11 +312,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_ref.reproject(_target_pose.x_abs, _target_pose.y_abs, pos_sp_triplet->current.lat, pos_sp_triplet->current.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;
 
 	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
 
@@ -555,8 +548,8 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
 		dt = 50000 / SEC2USEC;
 
 		// set a best guess for previous setpoints for smooth transition
-		_map_ref.project(_navigator->get_position_setpoint_triplet()->current.lat,
+		_sp_pev = _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_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 */
 
-	MapProjection _map_ref {}; /**< reference for local/global projections */
+	MapProjection _map_ref{}; /**< class 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/simulator/simulator.h

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

src/modules/simulator/simulator_mavlink.cpp

@@ -547,16 +547,12 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
 			_global_local_alt0 = hil_state.alt / 1000.f;
 		}
 
-		float x;
-		float y;
-		_global_local_proj_ref.project(lat, lon, x, y);
 		hil_lpos.timestamp = timestamp;
 		hil_lpos.xy_valid = true;
 		hil_lpos.z_valid = true;
 		hil_lpos.v_xy_valid = true;
 		hil_lpos.v_z_valid = true;
-		hil_lpos.x = x;
+		_global_local_proj_ref.project(lat, lon, hil_lpos.x, hil_lpos.y);
-		hil_lpos.y = y;
 		hil_lpos.z = _global_local_alt0 - hil_state.alt / 1000.0f;
 		hil_lpos.vx = hil_state.vx / 100.0f;
 		hil_lpos.vy = hil_state.vy / 100.0f;

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);
 
-	MapProjection _projection_reference = {}; ///< reference to convert (lon, lat) to local [m]
+	MapProjection _projection_reference{}; ///< class to convert (lon, lat) to local [m]
 
 	const OutputConfig &_config;