fix(mavlink): handle multi-GPS HIL_GPS id extension and prevent HIL message forwarding

Co-authored-by: mrpollo <317648+mrpollo@users.noreply.github.com>

docs/en/sim_airsim/index.md

@@ -14,6 +14,17 @@ It provides physically and visually realistic simulations of Pixhawk/PX4 using e
 
 <!-- datestamp:video:youtube:20170216:AirSim Demo -->
 
+## MAVLink Compatibility Note
+
+When using AirSim with PX4 for HITL simulation, you may encounter a `Message length 37 doesn't match expected length 36` error.
+This is caused by a MAVLink version mismatch: the `HIL_GPS` message was extended with an `id` field (to support multiple GPS inputs) in a newer version of the MAVLink spec.
+When this extension field is non-zero (e.g. when a simulator sends a second GPS instance with `id=1`), the message length becomes 37 bytes instead of 36 bytes.
+Older MAVLink implementations that are unaware of this extension may reject the message.
+
+PX4 now properly handles both the old 36-byte and new 37-byte `HIL_GPS` messages (where 37 bytes occurs when the `id` extension field is non-zero and the `yaw` extension field is zero), and no longer forwards HIL sensor messages between MAVLink instances to avoid triggering this error in connected clients.
+
+If you are still experiencing this error, ensure your AirSim version uses a MAVLink library that supports the `HIL_GPS` `id` extension field.
+
 ## PX4 Setup
 
 [PX4 Setup for AirSim](https://microsoft.github.io/AirSim/px4_setup/) describes how to use PX4 with AirSim using both [SITL](https://microsoft.github.io/AirSim/px4_sitl/) and [HITL](https://microsoft.github.io/AirSim/px4_setup/#setting-up-px4-hardware-in-loop).

src/modules/mavlink/mavlink_main.cpp

@@ -513,6 +513,16 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 		return;
 	}
 
+	// Don't forward HIL sensor messages to avoid MAVLink version compatibility issues.
+	// HIL messages use MAVLink 2 extension fields (e.g. HIL_GPS id field) that older
+	// clients may not handle correctly, causing message length mismatch errors.
+	if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS ||
+	    msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR ||
+	    msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION ||
+	    msg->msgid == MAVLINK_MSG_ID_HIL_OPTICAL_FLOW) {
+		return;
+	}
+
 	LockGuard lg{mavlink_module_mutex};
 
 	for (Mavlink *inst : mavlink_module_instances) {

src/modules/mavlink/mavlink_receiver.cpp

@@ -87,7 +87,10 @@ MavlinkReceiver::~MavlinkReceiver()
 	_ping_pub.unadvertise();
 	_radio_status_pub.unadvertise();
 	_sensor_baro_pub.unadvertise();
-	_sensor_gps_pub.unadvertise();
+	for (auto &pub : _sensor_gps_pubs) {
+		delete pub;
+		pub = nullptr;
+	}
 	_sensor_optical_flow_pub.unadvertise();
 }
 
@@ -2439,14 +2442,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 
 	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.latitude_deg = hil_gps.lat * 1e-7;
 	gps.longitude_deg = hil_gps.lon * 1e-7;
 	gps.altitude_msl_m = hil_gps.alt * 1e-3;
@@ -2486,7 +2481,34 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 
 	gps.timestamp = hrt_absolute_time();
 
-	_sensor_gps_pub.publish(gps);
+	// Compute device_id from the source system.
+	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;
+
+	// New publishers will be created based on the HIL_GPS ID's being different or not.
+	// This supports simulators that send multiple GPS instances using the id extension field.
+	for (int i = 0; i < MAX_HIL_GPS; i++) {
+		if (_sensor_gps_pubs[i] && _hil_gps_ids[i] == hil_gps.id) {
+			_sensor_gps_pubs[i]->publish(gps);
+			break;
+		}
+
+		if (_sensor_gps_pubs[i] == nullptr) {
+			_sensor_gps_pubs[i] = new uORB::PublicationMulti<sensor_gps_s>{ORB_ID(sensor_gps)};
+			_hil_gps_ids[i] = hil_gps.id;
+
+			_sensor_gps_pubs[i]->publish(gps);
+			break;
+		}
+
+		if (i == MAX_HIL_GPS - 1) {
+			PX4_WARN("HIL_GPS: max GPS instances (%d) reached, discarding GPS id %d", MAX_HIL_GPS, hil_gps.id);
+		}
+	}
 }
 
 void

src/modules/mavlink/mavlink_receiver.h

@@ -346,7 +346,9 @@ private:
 	uORB::PublicationMulti<ping_s>				_ping_pub{ORB_ID(ping)};
 	uORB::PublicationMulti<radio_status_s>			_radio_status_pub{ORB_ID(radio_status)};
 	uORB::PublicationMulti<sensor_baro_s>			_sensor_baro_pub{ORB_ID(sensor_baro)};
-	uORB::PublicationMulti<sensor_gps_s>			_sensor_gps_pub{ORB_ID(sensor_gps)};
+	static constexpr int MAX_HIL_GPS = 3;
+	uORB::PublicationMulti<sensor_gps_s>			*_sensor_gps_pubs[MAX_HIL_GPS] {};
+	uint8_t							_hil_gps_ids[MAX_HIL_GPS] {};
 	uORB::PublicationMulti<sensor_optical_flow_s>           _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
 	// ORB publications (queue length > 1)