Extend SENS_GPS_PRIME usage for UAVCAN GNSS devices (#26126)

* UAVCAN: extent SENS_GPS_PRIME usage to UAVCAN GNSS devices

* use convenience function

Co-authored-by: Matthias Grob <maetugr@gmail.com>

* Update src/drivers/uavcan/sensors/gnss.cpp

Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com>

* Apply suggestion from @MaEtUgR

Co-authored-by: Matthias Grob <maetugr@gmail.com>

* Fix type casting in GPS prime range check

* reverted parameter default

* UAVCAN: fix and improve device_id logic (#26135)

* UAVCAN: extent SENS_GPS_PRIME usage to UAVCAN GNSS devices

* use convenience function

Co-authored-by: Matthias Grob <maetugr@gmail.com>

* Update src/drivers/uavcan/sensors/gnss.cpp

Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com>

* Apply suggestion from @MaEtUgR

Co-authored-by: Matthias Grob <maetugr@gmail.com>

* Fix type casting in GPS prime range check

* UAVCAN: fix and improve device_id logic

* Added bus information to more UAVCAN drivers

* Fix device_id registration in UavcanBarometerBridge

---------

Co-authored-by: Matthias Grob <maetugr@gmail.com>
Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com>
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>

---------

Co-authored-by: Matthias Grob <maetugr@gmail.com>
Co-authored-by: Øyvind Taksdal Stubhaug <o_github@oystub.com>
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp

@@ -36,6 +36,7 @@
 #include <px4_platform_common/log.h>
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
+#include <lib/drivers/device/Device.hpp>
 
 namespace sensors
 {
@@ -95,7 +96,12 @@ void VehicleGPSPosition::ParametersUpdate(bool force)
 		_gps_blending.setBlendingUseHPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC);
 		_gps_blending.setBlendingUseVPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC);
 		_gps_blending.setBlendingTimeConstant(_param_sens_gps_tau.get());
-		_gps_blending.setPrimaryInstance(_param_sens_gps_prime.get());
+
+		const int gps_prime = _param_sens_gps_prime.get();
+
+		if (math::isInRange(gps_prime, -1, 1)) {
+			_gps_blending.setPrimaryInstance(gps_prime);
+		}
 	}
 }
 
@@ -113,6 +119,7 @@ void VehicleGPSPosition::Run()
 	// Check all GPS instance
 	bool any_gps_updated = false;
 	bool gps_updated = false;
+	const int32_t gps_prime = _param_sens_gps_prime.get();
 
 	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
 		gps_updated = _sensor_gps_sub[i].updated();
@@ -125,6 +132,16 @@ void VehicleGPSPosition::Run()
 			_sensor_gps_sub[i].copy(&gps_data);
 			_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 (!_sensor_gps_sub[i].registered()) {
 				_sensor_gps_sub[i].registerCallback();
 			}

src/modules/sensors/vehicle_gps_position/params.c

@@ -70,13 +70,16 @@ PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f);
  * send data to the EKF even if a secondary instance is already available.
  * The secondary instance is then only used if the primary one times out.
  *
- * To have an equal priority of all the instances, set this parameter to -1 and
- * the best receiver will be used.
+ * Accepted values:
+ * -1 : Auto (equal priority for all instances)
+ * 0 : Main serial GPS instance
+ * 1 : Secondary serial GPS instance
+ * 2-127 : UAVCAN module node ID
  *
  * This parameter has no effect if blending is active.
  *
  * @group Sensors
  * @min -1
- * @max 1
+ * @max 127
  */
 PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0);