From b2288f5fab9a699747deca86fdd10afbaec9e27a Mon Sep 17 00:00:00 2001
From: haitomatic <hai.to@unikie.com>
Date: Wed, 5 Jun 2024 12:56:52 +0000
Subject: [PATCH] mavlink: re-enable sensor_gps pub for SITL from HIL_GPS
 mavlink msg

---
 src/modules/mavlink/mavlink_params.c     |  2 +-
 src/modules/mavlink/mavlink_receiver.cpp | 64 ++++++++++++++++++++++--
 ssrc_config/config_hitl_eth_gzsim.txt    |  1 +
 3 files changed, 61 insertions(+), 6 deletions(-)

diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c
index 8faa0a7b4372..1cc5fa5d162f 100644
--- a/src/modules/mavlink/mavlink_params.c
+++ b/src/modules/mavlink/mavlink_params.c
@@ -163,7 +163,7 @@ PARAM_DEFINE_INT32(MAV_RADIO_TOUT, 5);
  * @reboot_required true
  * @group MAVLink
  */
-PARAM_DEFINE_INT32(MAV_HITL_SHOME, 1);
+PARAM_DEFINE_INT32(MAV_HITL_SHOME, 0);
 
 /**
  * mavlink receiver HITL simulated origin latitude
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index f7991de7e102..ca212392c926 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -2339,17 +2339,71 @@ void
 MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 {
 	// Only use to set home position
-	if (_hil_pos_ref.isInitialized()) {
+	if (_mavlink->get_hil_enabled() && _hil_pos_ref.isInitialized()) {
 		return;
 	}
 
 	mavlink_hil_gps_t hil_gps;
 	mavlink_msg_hil_gps_decode(msg, &hil_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;
+	if (_mavlink->get_hil_enabled()) {
+		_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;
+
+	} else {
+		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);
+	}
 }
 
 void
diff --git a/ssrc_config/config_hitl_eth_gzsim.txt b/ssrc_config/config_hitl_eth_gzsim.txt
index fb447d4a1a5a..622b9e1252b9 100644
--- a/ssrc_config/config_hitl_eth_gzsim.txt
+++ b/ssrc_config/config_hitl_eth_gzsim.txt
@@ -7,6 +7,7 @@
 
 # Set HITL related flag
 param set SYS_HITL 1
+param set MAV_HITL_SHOME 1
 param set SENS_EN_GPSSIM 1
 param set SENS_EN_BAROSIM 1
 param set SENS_EN_MAGSIM 1