ekf2 support SET_GPS_GLOBAL_ORIGIN and remove globallocalconverter usage

- vehicle_command cmd extended from uint16 to support PX4 internal commands that don't map to mavlink

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

@@ -148,8 +148,15 @@ 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) {
-			globallocalconverter_init(_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
-						  _sub_vehicle_local_position.get().ref_alt, _sub_vehicle_local_position.get().ref_timestamp);
+			if (!map_projection_initialized(&_global_local_proj_ref)
+			    || (_global_local_proj_ref.timestamp != _sub_vehicle_local_position.get().ref_timestamp)) {
+
+				map_projection_init_timestamped(&_global_local_proj_ref,
+								_sub_vehicle_local_position.get().ref_lat, _sub_vehicle_local_position.get().ref_lon,
+								_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

@@ -53,6 +53,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
 #include <uORB/topics/home_position.h>
+#include <lib/ecl/geo/geo.h>
 #include <lib/weather_vane/WeatherVane.hpp>
 
 struct ekf_reset_counters_s {
@@ -212,10 +213,15 @@ protected:
 	virtual void _ekfResetHandlerVelocityZ() {};
 	virtual void _ekfResetHandlerHeading(float delta_psi) {};
 
+	map_projection_reference_s _global_local_proj_ref{};
+	float                      _global_local_alt0{NAN};
+
 	/* Time abstraction */
 	static constexpr uint64_t _timeout = 500000; /**< maximal time in us before a loop or data times out */
+
 	float _time{}; /**< passed time in seconds since the task was activated */
 	float _deltatime{}; /**< passed time in seconds since the task was last updated */
+
 	hrt_abstime _time_stamp_activate{}; /**< time stamp when task was activated */
 	hrt_abstime _time_stamp_current{}; /**< time stamp at the beginning of the current task update */
 	hrt_abstime _time_stamp_last{}; /**< time stamp when task was last updated */
@@ -223,6 +229,7 @@ protected:
 	/* Current vehicle state */
 	matrix::Vector3f _position; /**< current vehicle position */
 	matrix::Vector3f _velocity; /**< current vehicle velocity */
+
 	float _yaw{}; /**< current vehicle yaw heading */
 	float _dist_to_bottom{}; /**< current height above ground level */
 	float _dist_to_ground{}; /**< equals _dist_to_bottom if valid, height above home otherwise */
@@ -239,8 +246,10 @@ protected:
 	matrix::Vector3f _velocity_setpoint;
 	matrix::Vector3f _acceleration_setpoint;
 	matrix::Vector3f _jerk_setpoint;
+
 	float _yaw_setpoint{};
 	float _yawspeed_setpoint{};
+
 	matrix::Vector3f _velocity_setpoint_feedback;
 	matrix::Vector3f _acceleration_setpoint_feedback;
 

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

@@ -76,22 +76,15 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 	// save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING
 	_initial_heading = _yaw;
 
-	// TODO: apply x,y / z independently in geo library
 	// commanded center coordinates
-	// if(PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {
-	// 	map_projection_global_project(command.param5, command.param6, &_center(0), &_center(1));
-	// }
+	if (map_projection_initialized(&_global_local_proj_ref)) {
+		if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {
+			map_projection_global_project(command.param5, command.param6, &_center(0), &_center(1));
+		}
 
-	// commanded altitude
-	// if(PX4_ISFINITE(command.param7)) {
-	// 	_position_setpoint(2) = gl_ref.alt - command.param7;
-	// }
-
-	if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6) && PX4_ISFINITE(command.param7)) {
-		if (globallocalconverter_tolocal(command.param5, command.param6, command.param7, &_center(0), &_center(1),
-						 &_position_setpoint(2))) {
-			// global to local conversion failed
-			ret = false;
+		// commanded altitude
+		if (PX4_ISFINITE(command.param7)) {
+			_position_setpoint(2) = _global_local_alt0 - command.param7;
 		}
 	}
 
@@ -109,8 +102,12 @@ bool FlightTaskOrbit::sendTelemetry()
 	orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
 	orbit_status.yaw_behaviour = _yaw_behaviour;
 
-	if (globallocalconverter_toglobal(_center(0), _center(1), _position_setpoint(2), &orbit_status.x, &orbit_status.y,
-					  &orbit_status.z)) {
+	if (map_projection_initialized(&_global_local_proj_ref)) {
+		// local -> global
+		map_projection_reproject(&_global_local_proj_ref, _center(0), _center(1), &orbit_status.x, &orbit_status.y);
+		orbit_status.z = _global_local_alt0 - _position_setpoint(2);
+
+	} else {
 		return false; // don't send the message if the transformation failed
 	}