a

src/modules/mavlink/mavlink_params.c

@@ -156,6 +156,15 @@ PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);
  */
 PARAM_DEFINE_INT32(MAV_RADIO_TOUT, 5);
 
+/**
+ * mavlink receiver HITL use simulated origin (home) position
+ *
+ * @boolean
+ * @reboot_required true
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_HITL_SHOME, 0);
+
 /**
  * mavlink receiver HITL simulated origin latitude
  *

src/modules/mavlink/mavlink_receiver.cpp

@@ -333,6 +333,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 	 * MAVLINK_MSG_ID_HIL_STATE_QUATERNION contains vehicle pose info from gz sim
 	 * Following that, local, global, attitude and angle velocity ground truth
 	 * will be published. These are needed by PX4 simulated sensors (GPS, Mag, Baro)
+	 * If accep HIL GPS messages, these will be used to provide home position only
 	 */
 	if (_mavlink->get_hil_enabled()) {
 		switch (msg->msgid) {
@@ -2337,59 +2338,18 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 void
 MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 {
+	// Only use to set home position
+	if (_hil_pos_ref.isInitialized()) {
+		return;
+	}
+
 	mavlink_hil_gps_t hil_gps;
 	mavlink_msg_hil_gps_decode(msg, &hil_gps);
 
-	sensor_gps_s gps{};
-
-	device::Device::DeviceId device_id;
-	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK;
-	device_id.devid_s.bus = _mavlink->get_instance_id();
-	device_id.devid_s.address = msg->sysid;
-	device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM;
-
-	gps.device_id = device_id.devid;
-
-	gps.lat = hil_gps.lat;
-	gps.lon = hil_gps.lon;
-	gps.alt = hil_gps.alt;
-	gps.alt_ellipsoid = hil_gps.alt;
-
-	gps.s_variance_m_s = 0.25f;
-	gps.c_variance_rad = 0.5f;
-	gps.fix_type = hil_gps.fix_type;
-
-	gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
-	gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
-
-	gps.hdop = 0; // TODO
-	gps.vdop = 0; // TODO
-
-	gps.noise_per_ms = 0;
-	gps.automatic_gain_control = 0;
-	gps.jamming_indicator = 0;
-	gps.jamming_state = 0;
-	gps.spoofing_state = 0;
-
-	gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s
-	gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s
-	gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s
-	gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s
-	gps.cog_rad = ((hil_gps.cog == 65535) ? (float)NAN : matrix::wrap_2pi(math::radians(
-				hil_gps.cog * 1e-2f))); // cdeg -> rad
-	gps.vel_ned_valid = true;
-
-	gps.timestamp_time_relative = 0;
-	gps.time_utc_usec = hil_gps.time_usec;
-
-	gps.satellites_used = hil_gps.satellites_visible;
-
-	gps.heading = NAN;
-	gps.heading_offset = NAN;
-
-	gps.timestamp = hrt_absolute_time();
-
-	_sensor_gps_pub.publish(gps);
+	_hitl_sim_gps_time_usec = hil_gps.time_usec;
+	_hitl_sim_home_lat = hil_gps.lat;
+	_hitl_sim_home_lon = hil_gps.lon;
+	_hitl_sim_home_alt = hil_gps.alt;
 }
 
 void
@@ -2686,7 +2646,14 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	 */
 
 	if (!_hil_pos_ref.isInitialized()) {
-		_hil_pos_ref.initReference((double)_param_hil_home_lat.get(), (double)_param_hil_home_lon.get(), hrt_absolute_time());
+				if (!_param_hitl_use_sim_home.get() || _hitl_sim_gps_time_usec) {
+					_hil_pos_ref.initReference(_param_hitl_use_sim_home.get() ? _hitl_sim_home_lat : (double)_param_hitl_home_lat.get(),
+							       _param_hitl_use_sim_home.get() ? _hitl_sim_home_lon : (double)_param_hitl_home_lon.get(),
+							       hrt_absolute_time());
+				} else {
+					// no reference set yet
+					return;
+				}
 	}
 
 	vehicle_local_position_s hil_local_position_groundtruth{};
@@ -2719,7 +2686,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		_hil_pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees
 	hil_local_position_groundtruth.ref_lon =
 		_hil_pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees
-	hil_local_position_groundtruth.ref_alt = _param_hil_home_alt.get();
+	hil_local_position_groundtruth.ref_alt = _param_hitl_use_sim_home.get() ? static_cast<float>(_hitl_sim_home_alt) :
+							     _param_hitl_home_alt.get();
 	hil_local_position_groundtruth.ref_timestamp = _hil_pos_ref.getProjectionReferenceTimestamp();
 
 	hil_local_position_groundtruth.timestamp = hrt_absolute_time();
@@ -2736,7 +2704,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 				       hil_global_position_groundtruth.lat,
 				       hil_global_position_groundtruth.lon);
 
-		hil_global_position_groundtruth.alt = _param_hil_home_alt.get() - static_cast<float>(position(2));
+		hil_global_position_groundtruth.alt = _param_hitl_use_sim_home.get() ? static_cast<float>(_hitl_sim_home_alt) :
+									_param_hitl_home_alt.get() - static_cast<float>(position(2));
 		hil_global_position_groundtruth.timestamp = hrt_absolute_time();
 		_gpos_groundtruth_pub.publish(hil_global_position_groundtruth);
 	}

src/modules/mavlink/mavlink_receiver.h

@@ -377,6 +377,11 @@ class MavlinkReceiver : public ModuleParams
 	matrix::Vector3f _hil_euler_prev{};
 	MapProjection _hil_pos_ref{};
 
+	uint64_t _hitl_sim_gps_time_usec{0};
+	double _hitl_sim_home_lat{0.0};
+	double _hitl_sim_home_lon{0.0};
+	double _hitl_sim_home_alt{0.0};
+
 	// Allocated if needed.
 	TunePublisher *_tune_publisher{nullptr};
 
@@ -401,12 +406,13 @@ class MavlinkReceiver : public ModuleParams
 	hrt_abstime _heartbeat_component_uart_bridge{0};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,
-		(ParamFloat<px4::params::BAT_EMERGEN_THR>)  _param_bat_emergen_thr,
-		(ParamFloat<px4::params::BAT_LOW_THR>)      _param_bat_low_thr,
-		(ParamFloat<px4::params::MAV_HITL_LAT>) _param_hil_home_lat,
-		(ParamFloat<px4::params::MAV_HITL_LON>) _param_hil_home_lon,
-		(ParamFloat<px4::params::MAV_HITL_ALT>) _param_hil_home_alt
+		(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
+		(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
+		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
+		(ParamBool<px4::params::MAV_HITL_SHOME>) _param_hitl_use_sim_home,
+		(ParamFloat<px4::params::MAV_HITL_LAT>) _param_hitl_home_lat,
+		(ParamFloat<px4::params::MAV_HITL_LON>) _param_hitl_home_lon,
+		(ParamFloat<px4::params::MAV_HITL_ALT>) _param_hitl_home_alt
 	);
 
 	// Disallow copy construction and move assignment.

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -44,7 +44,6 @@
 
 #include <iostream>
 #include <string>
-#include <cmath>
 
 GZBridge::GZBridge(const char *world, const char *name, const char *model, const char *pose_str) :
 	ModuleParams(nullptr),
@@ -546,7 +545,7 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 			_angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth);
 
 			if (!_pos_ref.isInitialized()) {
-				if (!_param_use_sim_home.get() || !std::isnan(_gz_home_lat) && !std::isnan(_gz_home_lon)) {
+				if (!_param_use_sim_home.get() || _gz_sim_gps_time_usec) {
 					_pos_ref.initReference(_param_use_sim_home.get() ? _gz_home_lat : (double)_param_sim_home_lat.get(),
 							       _param_use_sim_home.get() ? _gz_home_lon : (double)_param_sim_home_lon.get(),
 							       hrt_absolute_time());
@@ -629,6 +628,7 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat)
 	pthread_mutex_lock(&_node_mutex);
 
 	// initialize gz gps position
+	_gz_sim_gps_time_usec = (nav_sat.header().stamp().sec() * 1000000) + (nav_sat.header().stamp().nsec() / 1000);
 	_gz_home_lat = nav_sat.latitude_deg();
 	_gz_home_lon = nav_sat.longitude_deg();
 	_gz_home_alt = nav_sat.altitude();

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -164,11 +164,13 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 
 	float _temperature{288.15};  // 15 degrees
 
-	double _gz_home_lat{std::numeric_limits<double>::quiet_NaN()};
+	uint64_t _gz_sim_gps_time_usec{0};
 
-	double _gz_home_lon{std::numeric_limits<double>::quiet_NaN()};
+	double _gz_home_lat{0.0};
 
-	double _gz_home_alt{std::numeric_limits<double>::quiet_NaN()};
+	double _gz_home_lon{0.0};
+
+	double _gz_home_alt{0.0};
 
 	gz::transport::Node::Publisher _cmd_vel_pub;
 

src/modules/simulation/gz_bridge/parameters.c

@@ -56,7 +56,7 @@ PARAM_DEFINE_INT32(SIM_GZ_RUN_GZSIM, 1);
  * @reboot_required true
  * @group Simulator
  */
-PARAM_DEFINE_FLOAT(SIM_GZ_USE_SIM_HOME, 0);
+PARAM_DEFINE_INT32(SIM_GZ_USE_SIM_HOME, 0);
 
 /**
  * simulator origin latitude