fix(mavlink): select GPS_RAW_INT/GPS2_RAW instance via SENS_GPS_PRIME

src/lib/gnss/SensorGpsSelector.hpp

@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2026 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <lib/drivers/device/Device.hpp>
+#include <lib/parameters/param.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gps.h>
+
+// Resolves SENS_GPS_PRIME to the sensor_gps uORB instance of the primary receiver:
+// 0 and 1 select the uORB instance directly, 2-127 select a DroneCAN receiver by
+// node ID (matched via device_id). Used by the GPS_RAW_INT/GPS2_RAW MAVLink streams
+// so the reported receiver does not depend on uORB instance ordering, which is a
+// boot-order race for CAN receivers, and shared with the vehicle_gps_position
+// selection.
+class SensorGpsSelector
+{
+public:
+	// SENS_GPS_PRIME values 2-127 designate a DroneCAN receiver by node ID
+	static bool is_node_id(int32_t gps_prime) { return (gps_prime >= 2) && (gps_prime <= 127); }
+
+	// true if the SENS_GPS_PRIME node ID designates the receiver with this device_id
+	static bool node_id_matches(int32_t gps_prime, uint32_t device_id)
+	{
+		if (is_node_id(gps_prime)) {
+			device::Device::DeviceId id{};
+			id.devid = device_id;
+
+			return (id.devid_s.bus_type == device::Device::DeviceBusType_UAVCAN)
+			       && (id.devid_s.address == gps_prime);
+		}
+
+		return false;
+	}
+
+	SensorGpsSelector()
+	{
+		read_param();
+	}
+
+	// uORB instance of the primary receiver (0 or 1)
+	uint8_t primary_instance()
+	{
+		if (_parameter_update_sub.updated()) {
+			parameter_update_s parameter_update;
+			_parameter_update_sub.copy(&parameter_update);
+			read_param();
+		}
+
+		if ((_gps_prime == 0) || (_gps_prime == 1)) {
+			return _gps_prime;
+		}
+
+		if (is_node_id(_gps_prime)) {
+			// keep looking until the receiver has published
+			if (_node_id_instance < 0) {
+				_node_id_instance = find_instance_by_node_id(_gps_prime);
+			}
+
+			if (_node_id_instance >= 0) {
+				return _node_id_instance;
+			}
+		}
+
+		// -1 (auto) or not resolvable
+		return 0;
+	}
+
+private:
+	void read_param()
+	{
+		_gps_prime = 0;
+		_node_id_instance = -1;
+
+		if (_param_sens_gps_prime != PARAM_INVALID) {
+			param_get(_param_sens_gps_prime, &_gps_prime);
+		}
+	}
+
+	static int8_t find_instance_by_node_id(int32_t node_id)
+	{
+		for (uint8_t i = 0; i < 2; i++) {
+			uORB::Subscription sensor_gps_sub{ORB_ID(sensor_gps), i};
+			sensor_gps_s gps;
+
+			if (sensor_gps_sub.copy(&gps) && node_id_matches(node_id, gps.device_id)) {
+				return i;
+			}
+		}
+
+		return -1;
+	}
+
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	const param_t _param_sens_gps_prime{param_find("SENS_GPS_PRIME")};
+	int32_t _gps_prime{0};
+	int8_t _node_id_instance{-1};
+};

src/modules/mavlink/streams/GPS2_RAW.hpp

@@ -34,6 +34,7 @@
 #ifndef GPS2_RAW_HPP
 #define GPS2_RAW_HPP
 
+#include <lib/gnss/SensorGpsSelector.hpp>
 #include <uORB/topics/sensor_gps.h>
 
 using namespace time_literals;
@@ -58,16 +59,24 @@ class MavlinkStreamGPS2Raw : public MavlinkStream
 	explicit MavlinkStreamGPS2Raw(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
 	uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 1};
+	SensorGpsSelector _gps_selector{};
 	hrt_abstime _last_send_ts {};
 	static constexpr hrt_abstime kNoGpsSendInterval {1_s};
 
 	bool send() override
 	{
+		const uint8_t secondary = 1 - _gps_selector.primary_instance();
+
+		if (secondary != _sensor_gps_sub.get_instance()) {
+			_sensor_gps_sub.ChangeInstance(secondary);
+		}
+
 		sensor_gps_s gps;
 		mavlink_gps2_raw_t msg{};
 		hrt_abstime now{};
 
-		if (_sensor_gps_sub.update(&gps)) {
+		// only report the secondary receiver, never another instance's data
+		if ((_sensor_gps_sub.get_instance() == secondary) && _sensor_gps_sub.update(&gps)) {
 			if (gps.time_utc_usec <= 0) {
 				msg.time_usec = gps.timestamp;
 

src/modules/mavlink/streams/GPS_RAW_INT.hpp

@@ -34,6 +34,7 @@
 #ifndef GPS_RAW_INT_HPP
 #define GPS_RAW_INT_HPP
 
+#include <lib/gnss/SensorGpsSelector.hpp>
 #include <uORB/topics/sensor_gps.h>
 
 using namespace time_literals;
@@ -58,16 +59,24 @@ class MavlinkStreamGPSRawInt : public MavlinkStream
 	explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
 	uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 0};
+	SensorGpsSelector _gps_selector{};
 	hrt_abstime _last_send_ts {};
 	static constexpr hrt_abstime kNoGpsSendInterval {1_s};
 
 	bool send() override
 	{
+		const uint8_t primary = _gps_selector.primary_instance();
+
+		if (primary != _sensor_gps_sub.get_instance()) {
+			_sensor_gps_sub.ChangeInstance(primary);
+		}
+
 		sensor_gps_s gps;
 		mavlink_gps_raw_int_t msg{};
 		hrt_abstime now{};
 
-		if (_sensor_gps_sub.update(&gps)) {
+		// only report the primary receiver, never another instance's data
+		if ((_sensor_gps_sub.get_instance() == primary) && _sensor_gps_sub.update(&gps)) {
 			if (gps.time_utc_usec <= 0) {
 				msg.time_usec = gps.timestamp;
 

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

@@ -35,8 +35,8 @@
 
 #include <px4_platform_common/log.h>
 #include <lib/geo/geo.h>
+#include <lib/gnss/SensorGpsSelector.hpp>
 #include <lib/mathlib/mathlib.h>
-#include <lib/drivers/device/Device.hpp>
 
 namespace sensors
 {
@@ -173,14 +173,8 @@ void VehicleGPSPosition::Run()
 			_gps_blending.setAntennaOffset(antenna_offset, i);
 			_gps_blending.setGpsData(gps_data, i);
 
-			if (math::isInRange(static_cast<int>(gps_prime), 2, 127)) {
-				device::Device::DeviceId device_id{};
-				device_id.devid = gps_data.device_id;
-
-				if (device_id.devid_s.bus_type == device::Device::DeviceBusType_UAVCAN
-				    && device_id.devid_s.address == static_cast<uint8_t>(gps_prime)) {
-					_gps_blending.setPrimaryInstance(i);
-				}
+			if (SensorGpsSelector::node_id_matches(gps_prime, gps_data.device_id)) {
+				_gps_blending.setPrimaryInstance(i);
 			}
 
 			if (!_sensor_gps_sub[i].registered()) {

src/modules/sensors/vehicle_gps_position/module.yaml

@@ -46,7 +46,11 @@ parameters:
 
                     To select a DroneCAN GPS, set this to the node ID.
 
-                    This parameter has no effect if blending is active.
+                    The primary receiver is reported in the GPS_RAW_INT MAVLink
+                    message and the secondary in GPS2_RAW.
+
+                    This parameter has no effect on the EKF GPS selection if
+                    blending is active.
             type: int32
             default: 0
             min: -1