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/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -106,7 +106,6 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
 
 	// local to global coversion related variables
 	_is_global_cov_init(false),
-	_global_ref_timestamp(0.0),
 	_ref_lat(0.0),
 	_ref_lon(0.0),
 	_ref_alt(0.0)
@@ -168,6 +167,23 @@ void BlockLocalPositionEstimator::Run()
 		return;
 	}
 
+	if (_vehicle_command_sub.updated()) {
+		vehicle_command_s vehicle_command;
+
+		if (_vehicle_command_sub.update(&vehicle_command)) {
+			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) {
+				const double latitude = vehicle_command.param5;
+				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_alt0 = altitude;
+
+				PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast<double>(altitude));
+			}
+		}
+	}
+
 	sensor_combined_s imu;
 
 	if (!_sensors_sub.update(&imu)) {

src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp

@@ -12,6 +12,7 @@
 // uORB Subscriptions
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
@@ -264,7 +265,7 @@ private:
 	uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::SubscriptionData<actuator_armed_s> _sub_armed{ORB_ID(actuator_armed)};
 	uORB::SubscriptionData<vehicle_land_detected_s> _sub_land{ORB_ID(vehicle_land_detected)};
 	uORB::SubscriptionData<vehicle_attitude_s> _sub_att{ORB_ID(vehicle_attitude)};
@@ -295,6 +296,9 @@ private:
 	// map projection
 	struct map_projection_reference_s _map_ref;
 
+	map_projection_reference_s _global_local_proj_ref{};
+	float                      _global_local_alt0{NAN};
+
 	// target mode paramters from landing_target_estimator module
 	enum TargetMode {
 		Target_Moving = 0,
@@ -379,7 +383,6 @@ private:
 
 	// local to global coversion related variables
 	bool _is_global_cov_init;
-	uint64_t _global_ref_timestamp;
 	double _ref_lat;
 	double _ref_lon;
 	float _ref_alt;

src/modules/local_position_estimator/sensors/mocap.cpp

@@ -37,9 +37,11 @@ void BlockLocalPositionEstimator::mocapInit()
 		_sensorFault &= ~SENSOR_MOCAP;
 
 		// get reference for global position
-		globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt);
-		_global_ref_timestamp = _timeStamp;
-		_is_global_cov_init = globallocalconverter_initialized();
+		_ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
+		_ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
+		_ref_alt = _global_local_alt0;
+
+		_is_global_cov_init = map_projection_initialized(&_global_local_proj_ref);
 
 		if (!_map_ref.init_done && _is_global_cov_init && !_visionUpdated) {
 			// initialize global origin using the mocap estimator reference (only if the vision estimation is not being fused as well)
@@ -53,7 +55,7 @@ void BlockLocalPositionEstimator::mocapInit()
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
 			_altOriginGlobal = true;
-			_altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f;
+			_altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f;
 		}
 	}
 }

src/modules/local_position_estimator/sensors/vision.cpp

@@ -42,9 +42,11 @@ void BlockLocalPositionEstimator::visionInit()
 		_sensorFault &= ~SENSOR_VISION;
 
 		// get reference for global position
-		globallocalconverter_getref(&_ref_lat, &_ref_lon, &_ref_alt);
-		_global_ref_timestamp = _timeStamp;
-		_is_global_cov_init = globallocalconverter_initialized();
+		_ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
+		_ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
+		_ref_alt = _global_local_alt0;
+
+		_is_global_cov_init = map_projection_initialized(&_global_local_proj_ref);
 
 		if (!_map_ref.init_done && _is_global_cov_init) {
 			// initialize global origin using the visual estimator reference
@@ -58,7 +60,7 @@ void BlockLocalPositionEstimator::visionInit()
 		if (!_altOriginInitialized) {
 			_altOriginInitialized = true;
 			_altOriginGlobal = true;
-			_altOrigin = globallocalconverter_initialized() ? _ref_alt : 0.0f;
+			_altOrigin = map_projection_initialized(&_global_local_proj_ref) ? _ref_alt : 0.0f;
 		}
 	}
 }