Add position ground truth. (#5819)

Have confirmed this works with gazebo sitl.

msg/vehicle_global_position.msg

@@ -18,3 +18,5 @@ float32 terrain_alt		# Terrain altitude WGS84, (metres)
 bool terrain_alt_valid		# Terrain altitude estimate is valid
 bool dead_reckoning		# True if this position is estimated through dead-reckoning
 float32 pressure_alt		# Pressure altitude AMSL, (metres)
+
+# TOPICS vehicle_global_position vehicle_global_position_groundtruth

msg/vehicle_local_position.msg

@@ -32,4 +32,6 @@ float32 dist_bottom_rate		# Rate of change of distance from bottom surface to gr
 uint64 surface_bottom_timestamp		# Time when new bottom surface found since system start, (microseconds)
 bool dist_bottom_valid			# true if distance to bottom surface is valid
 float32 eph				# Standard deviation of horizontal position error, (metres)
-float32 epv				# Standard deviation of vertical position error, (metres)
+float32 epv				# Standard deviation of vertical position error, (metres)
+
+# TOPICS vehicle_local_position vehicle_local_position_groundtruth

src/modules/logger/logger.cpp

@@ -510,6 +510,8 @@ void Logger::add_default_topics()
 {
 #ifdef CONFIG_ARCH_BOARD_SITL
 	add_topic("vehicle_attitude_groundtruth", 10);
+	add_topic("vehicle_global_position_groundtruth", 100);
+	add_topic("vehicle_local_position_groundtruth", 100);
 #endif
 
 	// Note: try to avoid setting the interval where possible, as it increases RAM usage

src/modules/simulator/simulator.h

@@ -43,6 +43,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/battery_status.h>
 #include <drivers/drv_accel.h>
@@ -58,6 +59,7 @@
 #include <uORB/topics/distance_sensor.h>
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
+#include <geo/geo.h>
 namespace simulator
 {
 
@@ -247,10 +249,17 @@ private:
 		,
 		_rc_channels_pub(nullptr),
 		_attitude_pub(nullptr),
+		_gpos_pub(nullptr),
 		_actuator_outputs_sub{},
 		_vehicle_attitude_sub(-1),
 		_manual_sub(-1),
 		_vehicle_status_sub(-1),
+		_hil_local_proj_ref(),
+		_hil_local_proj_inited(false),
+		_hil_ref_lat(0),
+		_hil_ref_lon(0),
+		_hil_ref_alt(0),
+		_hil_ref_timestamp(0),
 		_rc_input{},
 		_actuators{},
 		_attitude{},
@@ -317,6 +326,8 @@ private:
 	// uORB publisher handlers
 	orb_advert_t _rc_channels_pub;
 	orb_advert_t _attitude_pub;
+	orb_advert_t _gpos_pub;
+	orb_advert_t _lpos_pub;
 
 	// uORB subscription handlers
 	int _actuator_outputs_sub[ORB_MULTI_MAX_INSTANCES];
@@ -324,6 +335,14 @@ private:
 	int _manual_sub;
 	int _vehicle_status_sub;
 
+	// hil map_ref data
+	struct map_projection_reference_s _hil_local_proj_ref;
+	bool _hil_local_proj_inited;
+	double _hil_ref_lat;
+	double _hil_ref_lon;
+	float _hil_ref_alt;
+	uint64_t _hil_ref_timestamp;
+
 	// uORB data containers
 	struct rc_input_values _rc_input;
 	struct actuator_outputs_s _actuators[ORB_MULTI_MAX_INSTANCES];

src/modules/simulator/simulator_mavlink.cpp

@@ -397,6 +397,75 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 			int hilstate_multi;
 			orb_publish_auto(ORB_ID(vehicle_attitude_groundtruth), &_attitude_pub, &hil_attitude, &hilstate_multi, ORB_PRIO_HIGH);
 		}
+
+		/* global position */
+		struct vehicle_global_position_s hil_gpos = {};
+		{
+			hil_gpos.timestamp = timestamp;
+
+			hil_gpos.time_utc_usec = timestamp;
+			hil_gpos.lat = hil_state.lat;
+			hil_gpos.lon = hil_state.lon;
+			hil_gpos.alt = hil_state.alt;
+
+			hil_gpos.vel_n = hil_state.vx;
+			hil_gpos.vel_e = hil_state.vy;
+			hil_gpos.vel_d = hil_state.vz;
+
+			// always publish ground truth attitude message
+			int hil_gpos_multi;
+			orb_publish_auto(ORB_ID(vehicle_global_position_groundtruth), &_gpos_pub, &hil_gpos, &hil_gpos_multi,
+					 ORB_PRIO_HIGH);
+		}
+
+		/* local position */
+		struct 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;;
+			}
+
+			float x;
+			float y;
+			map_projection_project(&_hil_local_proj_ref, 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;
+			hil_lpos.y = y;
+			hil_lpos.z = _hil_ref_alt - 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;
+			matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
+			hil_lpos.yaw = 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;
+
+			// always publish ground truth attitude message
+			int hil_lpos_multi;
+			orb_publish_auto(ORB_ID(vehicle_local_position_groundtruth), &_lpos_pub, &hil_lpos, &hil_lpos_multi,
+					 ORB_PRIO_HIGH);
+		}
+
+
+
 		break;
 	}