From 48dd85ad618cf2fb79b36dbfc320ac85c7afc44c Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Fri, 13 Mar 2026 01:19:24 +0000 Subject: [PATCH] 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 | 11 +++++++ src/modules/mavlink/mavlink_main.cpp | 10 ++++++ src/modules/mavlink/mavlink_receiver.cpp | 42 ++++++++++++++++++------ src/modules/mavlink/mavlink_receiver.h | 4 ++- 4 files changed, 56 insertions(+), 11 deletions(-) diff --git a/docs/en/sim_airsim/index.md b/docs/en/sim_airsim/index.md index ce396a6640..4bc576ce7e 100644 --- a/docs/en/sim_airsim/index.md +++ b/docs/en/sim_airsim/index.md @@ -14,6 +14,17 @@ It provides physically and visually realistic simulations of Pixhawk/PX4 using e +## 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). diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d1f4686f29..33b2c1b7c8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ad5cd15069..2e9c4c22e4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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{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 diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a21a9f313c..f5c4d5d4ca 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -346,7 +346,9 @@ private: uORB::PublicationMulti _ping_pub{ORB_ID(ping)}; uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status)}; uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; - uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + static constexpr int MAX_HIL_GPS = 3; + uORB::PublicationMulti *_sensor_gps_pubs[MAX_HIL_GPS] {}; + uint8_t _hil_gps_ids[MAX_HIL_GPS] {}; uORB::PublicationMulti _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)}; // ORB publications (queue length > 1)