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/commander/Commander.cpp

@@ -132,8 +132,9 @@ static int power_button_state_notification_cb(board_power_button_state_notificat
 #endif // BOARD_HAS_POWER_CONTROL
 
 #ifndef CONSTRAINED_FLASH
-static bool send_vehicle_command(uint16_t cmd, float param1 = NAN, float param2 = NAN, float param3 = NAN,
-				 float param4 = NAN, float param5 = NAN, float param6 = NAN, float param7 = NAN)
+static bool send_vehicle_command(const uint32_t cmd, const float param1 = NAN, const float param2 = NAN,
+				 const float param3 = NAN,  const float param4 = NAN, const double param5 = static_cast<double>(NAN),
+				 const double param6 = static_cast<double>(NAN), const float param7 = NAN)
 {
 	vehicle_command_s vcmd{};
 
@@ -174,7 +175,7 @@ int Commander::custom_command(int argc, char *argv[])
 		if (argc > 1) {
 			if (!strcmp(argv[1], "gyro")) {
 				// gyro calibration: param1 = 1
-				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f);
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 1.f, 0.f, 0.f, 0.f, 0.0, 0.0, 0.f);
 
 			} else if (!strcmp(argv[1], "mag")) {
 				if (argc > 2 && (strcmp(argv[2], "quick") == 0)) {
@@ -183,30 +184,30 @@ int Commander::custom_command(int argc, char *argv[])
 
 				} else {
 					// magnetometer calibration: param2 = 1
-					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 0.f);
+					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 1.f, 0.f, 0.f, 0.0, 0.0, 0.f);
 				}
 
 			} else if (!strcmp(argv[1], "accel")) {
 				if (argc > 2 && (strcmp(argv[2], "quick") == 0)) {
 					// accelerometer quick calibration: param5 = 3
-					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 4.f, 0.f, 0.f);
+					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 4.0, 0.0, 0.f);
 
 				} else {
 					// accelerometer calibration: param5 = 1
-					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f);
+					send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 1.0, 0.0, 0.f);
 				}
 
 			} else if (!strcmp(argv[1], "level")) {
 				// board level calibration: param5 = 2
-				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.f, 0.f, 0.f);
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 2.0, 0.0, 0.f);
 
 			} else if (!strcmp(argv[1], "airspeed")) {
 				// airspeed calibration: param6 = 2
-				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.f, 2.f, 0.f);
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 2.0, 0.f);
 
 			} else if (!strcmp(argv[1], "esc")) {
 				// ESC calibration: param7 = 1
-				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 1.f);
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION, 0.f, 0.f, 0.f, 0.f, 0.0, 0.0, 1.f);
 
 			} else {
 				PX4_ERR("argument %s unsupported.", argv[1]);
@@ -359,6 +360,25 @@ int Commander::custom_command(int argc, char *argv[])
 		return (ret ? 0 : 1);
 	}
 
+	if (!strcmp(argv[0], "set_ekf_origin")) {
+		if (argc > 3) {
+
+			double latitude  = atof(argv[1]);
+			double longitude = atof(argv[2]);
+			float  altitude  = atof(argv[3]);
+
+			// Set the ekf NED origin global coordinates.
+			bool ret = send_vehicle_command(vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN,
+							0.f, 0.f, 0.0, 0.0, latitude, longitude, altitude);
+			return (ret ? 0 : 1);
+
+		} else {
+			PX4_ERR("missing argument");
+			return 0;
+		}
+	}
+
+
 #endif
 
 	return print_usage("unknown command");
@@ -1137,9 +1157,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			} else {
 				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
-				// parameter 1: Yaw in degrees
-				// parameter 3: Latitude
-				// parameter 4: Longitude
+				// parameter 1: Heading   (degrees)
+				// parameter 3: Latitude  (degrees)
+				// parameter 4: Longitude (degrees)
 
 				// assume vehicle pointing north (0 degrees) if heading isn't specified
 				const float heading_radians = PX4_ISFINITE(cmd.param1) ? math::radians(roundf(cmd.param1)) : 0.f;
@@ -1223,6 +1243,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
 	case vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE:
+	case vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 
@@ -1255,6 +1276,7 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd)
 	test_motor_s test_motor{};
 	test_motor.timestamp = hrt_absolute_time();
 	test_motor.motor_number = (int)(cmd.param1 + 0.5f) - 1;
+
 	int throttle_type = (int)(cmd.param2 + 0.5f);
 
 	if (throttle_type != 0) { // 0: MOTOR_TEST_THROTTLE_PERCENT
@@ -4041,6 +4063,9 @@ The commander module contains the state machine for mode switching and failsafe
 			"Flight mode", false);
 	PRINT_MODULE_USAGE_COMMAND("lockdown");
 	PRINT_MODULE_USAGE_ARG("off", "Turn lockdown off", true);
+	PRINT_MODULE_USAGE_COMMAND("set_ekf_origin");
+	PRINT_MODULE_USAGE_ARG("lat, lon, alt", "Origin Latitude, Longitude, Altitude", false);
+	PRINT_MODULE_USAGE_COMMAND_DESCR("lat|lon|alt", "Origin latitude longitude altitude");
 #endif
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 

src/modules/ekf2/EKF2.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -265,6 +265,28 @@ void EKF2::Run()
 		}
 	}
 
+	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) {
+				if (!_ekf.control_status_flags().in_air) {
+
+					uint64_t origin_time {};
+					double latitude = vehicle_command.param5;
+					double longitude = vehicle_command.param6;
+					float altitude = vehicle_command.param7;
+
+					_ekf.setEkfGlobalOrigin(latitude, longitude, altitude);
+
+					// Validate the ekf origin status.
+					_ekf.getEkfGlobalOrigin(origin_time, latitude, longitude, altitude);
+					PX4_INFO("New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", latitude, longitude, static_cast<double>(altitude));
+				}
+			}
+		}
+	}
+
 	bool imu_updated = false;
 	imuSample imu_sample_new {};
 

src/modules/ekf2/EKF2.hpp

@@ -77,6 +77,7 @@
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_imu.h>
@@ -219,6 +220,7 @@ private:
 	uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 

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
 	}
 

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;
 		}
 	}
 }

src/modules/mavlink/mavlink_command_sender.cpp

@@ -65,8 +65,13 @@ MavlinkCommandSender::~MavlinkCommandSender()
 	px4_sem_destroy(&_lock);
 }
 
-int MavlinkCommandSender::handle_vehicle_command(const struct vehicle_command_s &command, mavlink_channel_t channel)
+int MavlinkCommandSender::handle_vehicle_command(const vehicle_command_s &command, mavlink_channel_t channel)
 {
+	// commands > uint16 are PX4 internal only
+	if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) {
+		return 0;
+	}
+
 	lock();
 	CMD_DEBUG("new command: %d (channel: %d)", command.command, channel);
 

src/modules/mavlink/mavlink_main.cpp

@@ -2298,7 +2298,7 @@ Mavlink::task_main(int argc, char *argv[])
 			vehicle_command_ack_s command_ack;
 
 			while ((get_free_tx_buf() >= COMMAND_ACK_TOTAL_LEN) && _vehicle_command_ack_sub.update(&command_ack)) {
-				if (!command_ack.from_external) {
+				if (!command_ack.from_external && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) {
 					mavlink_command_ack_t msg;
 					msg.result = command_ack.result;
 					msg.command = command_ack.command;

src/modules/mavlink/mavlink_messages.cpp

@@ -537,7 +537,7 @@ protected:
 						_vehicle_command_sub.get_last_generation());
 				}
 
-				if (!cmd.from_external) {
+				if (!cmd.from_external && cmd.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) {
 					PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
 
 					MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());

src/modules/mavlink/mavlink_receiver.cpp

@@ -987,20 +987,20 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 			vehicle_local_position_s local_pos{};
 			_vehicle_local_position_sub.copy(&local_pos);
 
-			if (!map_projection_initialized(&_global_local_proj_ref)
-			    || (_global_local_proj_ref.timestamp != 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_alt0 = local_pos.ref_alt;
+			if (!local_pos.xy_global || !local_pos.z_global) {
+				return;
 			}
 
+			map_projection_reference_s global_local_proj_ref{};
+			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);
+			map_projection_project(&global_local_proj_ref, lat, lon, &setpoint.x, &setpoint.y);
 
 			if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_INT) {
-				setpoint.z = _global_local_alt0 - target_global_int.alt;
+				setpoint.z = local_pos.ref_alt - target_global_int.alt;
 
 			} else if (target_global_int.coordinate_frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
 				home_position_s home_position{};
@@ -1008,7 +1008,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 
 				if (home_position.valid_alt) {
 					const float alt = home_position.alt - target_global_int.alt;
-					setpoint.z = alt - _global_local_alt0;
+					setpoint.z = alt - local_pos.ref_alt;
 
 				} else {
 					// home altitude required
@@ -1021,7 +1021,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t
 
 				if (vehicle_global_position.terrain_alt_valid) {
 					const float alt = target_global_int.alt + vehicle_global_position.terrain_alt;
-					setpoint.z = _global_local_alt0 - alt;
+					setpoint.z = local_pos.ref_alt - alt;
 
 				} else {
 					// valid terrain alt required
@@ -1152,14 +1152,23 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
 void
 MavlinkReceiver::handle_message_set_gps_global_origin(mavlink_message_t *msg)
 {
-	mavlink_set_gps_global_origin_t origin;
-	mavlink_msg_set_gps_global_origin_decode(msg, &origin);
+	mavlink_set_gps_global_origin_t gps_global_origin;
+	mavlink_msg_set_gps_global_origin_decode(msg, &gps_global_origin);
 
-	if (!globallocalconverter_initialized() && (origin.target_system == _mavlink->get_system_id())) {
-		/* Set reference point conversion of local coordiantes <--> global coordinates */
-		globallocalconverter_init((double)origin.latitude * 1.0e-7, (double)origin.longitude * 1.0e-7,
-					  (float)origin.altitude * 1.0e-3f, hrt_absolute_time());
-		_global_ref_timestamp = hrt_absolute_time();
+	if (gps_global_origin.target_system == _mavlink->get_system_id()) {
+		vehicle_command_s vcmd{};
+		vcmd.param5 = (double)gps_global_origin.latitude * 1.e-7;
+		vcmd.param6 = (double)gps_global_origin.longitude * 1.e-7;
+		vcmd.param7 = (float)gps_global_origin.altitude * 1.e-3f;
+		vcmd.command = vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN;
+		vcmd.target_system = _mavlink->get_system_id();
+		vcmd.target_component = MAV_COMP_ID_ALL;
+		vcmd.source_system = msg->sysid;
+		vcmd.source_component = msg->compid;
+		vcmd.confirmation = false;
+		vcmd.from_external = true;
+		vcmd.timestamp = hrt_absolute_time();
+		_cmd_pub.publish(vcmd);
 	}
 
 	handle_request_message_command(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN);
@@ -2484,34 +2493,32 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 
 	/* local position */
 	{
-		double lat = hil_state.lat * 1e-7;
-		double lon = hil_state.lon * 1e-7;
+		const double lat = hil_state.lat * 1e-7;
+		const double lon = hil_state.lon * 1e-7;
 
-		if (!_hil_local_proj_inited) {
-			_hil_local_proj_inited = true;
-			_hil_local_alt0 = hil_state.alt / 1000.0f;
+		map_projection_reference_s global_local_proj_ref;
+		map_projection_init(&global_local_proj_ref, lat, lon);
 
-			map_projection_init(&_hil_local_proj_ref, lat, lon);
-		}
+		float global_local_alt0 = hil_state.alt / 1000.f;
 
 		float x = 0.0f;
 		float y = 0.0f;
-		map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
+		map_projection_project(&global_local_proj_ref, lat, lon, &x, &y);
 
 		vehicle_local_position_s hil_local_pos{};
 		hil_local_pos.timestamp = timestamp;
 
-		hil_local_pos.ref_timestamp = _hil_local_proj_ref.timestamp;
-		hil_local_pos.ref_lat = math::radians(_hil_local_proj_ref.lat_rad);
-		hil_local_pos.ref_lon = math::radians(_hil_local_proj_ref.lon_rad);
-		hil_local_pos.ref_alt = _hil_local_alt0;
+		hil_local_pos.ref_timestamp = global_local_proj_ref.timestamp;
+		hil_local_pos.ref_lat = math::degrees(global_local_proj_ref.lat_rad);
+		hil_local_pos.ref_lon = math::degrees(global_local_proj_ref.lon_rad);
+		hil_local_pos.ref_alt = global_local_alt0;
 		hil_local_pos.xy_valid = true;
 		hil_local_pos.z_valid = true;
 		hil_local_pos.v_xy_valid = true;
 		hil_local_pos.v_z_valid = true;
 		hil_local_pos.x = x;
 		hil_local_pos.y = y;
-		hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f;
+		hil_local_pos.z = global_local_alt0 - hil_state.alt / 1000.0f;
 		hil_local_pos.vx = hil_state.vx / 100.0f;
 		hil_local_pos.vy = hil_state.vy / 100.0f;
 		hil_local_pos.vz = hil_state.vz / 100.0f;

src/modules/mavlink/mavlink_receiver.h

@@ -395,10 +395,10 @@ private:
 	uORB::PublicationMulti<radio_status_s>			_radio_status_pub{ORB_ID(radio_status)};
 
 	// ORB publications (queue length > 1)
-	uORB::Publication<gps_inject_data_s>	_gps_inject_data_pub{ORB_ID(gps_inject_data)};
-	uORB::Publication<transponder_report_s>	_transponder_report_pub{ORB_ID(transponder_report)};
-	uORB::Publication<vehicle_command_ack_s>	_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
-	uORB::Publication<vehicle_command_s>	_cmd_pub{ORB_ID(vehicle_command)};
+	uORB::Publication<gps_inject_data_s>     _gps_inject_data_pub{ORB_ID(gps_inject_data)};
+	uORB::Publication<transponder_report_s>  _transponder_report_pub{ORB_ID(transponder_report)};
+	uORB::Publication<vehicle_command_s>     _cmd_pub{ORB_ID(vehicle_command)};
+	uORB::Publication<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	// ORB subscriptions
 	uORB::Subscription	_actuator_armed_sub{ORB_ID(actuator_armed)};
@@ -428,15 +428,6 @@ private:
 	uint8_t				_mom_switch_pos[MOM_SWITCH_COUNT] {};
 	uint16_t			_mom_switch_state{0};
 
-	map_projection_reference_s	_global_local_proj_ref{};
-	float				_global_local_alt0{NAN};
-
-	uint64_t			_global_ref_timestamp{0};
-
-	map_projection_reference_s	_hil_local_proj_ref{};
-	float				_hil_local_alt0{0.0f};
-	bool				_hil_local_proj_inited{false};
-
 	hrt_abstime			_last_utm_global_pos_com{0};
 
 	// Allocated if needed.

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -410,17 +410,24 @@ RoverPositionControl::Run()
 
 			position_setpoint_triplet_poll();
 
-			//Convert Local setpoints to global setpoints
+			// Convert Local setpoints to global setpoints
 			if (_control_mode.flag_control_offboard_enabled) {
-				if (!globallocalconverter_initialized()) {
-					globallocalconverter_init(_local_pos.ref_lat, _local_pos.ref_lon,
-								  _local_pos.ref_alt, _local_pos.ref_timestamp);
+				if (!map_projection_initialized(&_global_local_proj_ref)
+				    || (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
 
-				} else {
-					globallocalconverter_toglobal(_pos_sp_triplet.current.x, _pos_sp_triplet.current.y, _pos_sp_triplet.current.z,
-								      &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon, &_pos_sp_triplet.current.alt);
+					map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
+									_local_pos.ref_timestamp);
 
+					_global_local_alt0 = _local_pos.ref_alt;
 				}
+
+				// local -> global
+				map_projection_reproject(&_global_local_proj_ref,
+							 _pos_sp_triplet.current.x, _pos_sp_triplet.current.y,
+							 &_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon);
+
+				_pos_sp_triplet.current.alt = _global_local_alt0 - _pos_sp_triplet.current.z;
+				_pos_sp_triplet.current.valid = true;
 			}
 
 			// update the reset counters in any case

src/modules/rover_pos_control/RoverPositionControl.hpp

@@ -129,6 +129,9 @@ 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{};
+	float                      _global_local_alt0{NAN};
+
 	/* Pid controller for the speed. Here we assume we can control airspeed but the control variable is actually on
 	 the throttle. For now just assuming a proportional scaler between controlled airspeed and throttle output.*/
 	PID_t _speed_ctrl{};

src/modules/simulator/simulator.h

@@ -261,14 +261,8 @@ private:
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 
 	// hil map_ref data
-	struct map_projection_reference_s _hil_local_proj_ref {};
-
-	bool _hil_local_proj_inited{false};
-
-	double _hil_ref_lat{0};
-	double _hil_ref_lon{0};
-	float _hil_ref_alt{0.0f};
-	uint64_t _hil_ref_timestamp{0};
+	map_projection_reference_s	_global_local_proj_ref{};
+	float						_global_local_alt0{NAN};
 
 	vehicle_status_s _vehicle_status{};
 

src/modules/simulator/simulator_mavlink.cpp

@@ -504,25 +504,21 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
 	}
 
 	/* local position */
-	struct vehicle_local_position_s hil_lpos = {};
+	vehicle_local_position_s hil_lpos{};
 	{
 		hil_lpos.timestamp = timestamp;
 
 		double lat = hil_state.lat * 1e-7;
 		double lon = hil_state.lon * 1e-7;
 
-		if (!_hil_local_proj_inited) {
-			_hil_local_proj_inited = true;
-			map_projection_init(&_hil_local_proj_ref, lat, lon);
-			_hil_ref_timestamp = timestamp;
-			_hil_ref_lat = lat;
-			_hil_ref_lon = lon;
-			_hil_ref_alt = hil_state.alt / 1000.0f;
+		if (!map_projection_initialized(&_global_local_proj_ref)) {
+			map_projection_init(&_global_local_proj_ref, lat, lon);
+			_global_local_alt0 = hil_state.alt / 1000.f;
 		}
 
 		float x;
 		float y;
-		map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
+		map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y);
 		hil_lpos.timestamp = timestamp;
 		hil_lpos.xy_valid = true;
 		hil_lpos.z_valid = true;
@@ -530,7 +526,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
 		hil_lpos.v_z_valid = true;
 		hil_lpos.x = x;
 		hil_lpos.y = y;
-		hil_lpos.z = _hil_ref_alt - hil_state.alt / 1000.0f;
+		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;
 		hil_lpos.vz = hil_state.vz / 100.0f;
@@ -538,10 +534,10 @@ 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_lat = _hil_ref_lat;
-		hil_lpos.ref_lon = _hil_ref_lon;
-		hil_lpos.ref_alt = _hil_ref_alt;
-		hil_lpos.ref_timestamp = _hil_ref_timestamp;
+		hil_lpos.ref_timestamp = _global_local_proj_ref.timestamp;
+		hil_lpos.ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
+		hil_lpos.ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
+		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();
 		hil_lpos.hagl_min = std::numeric_limits<float>::infinity();