From 8c73abbe0d3483b93f2f8e4f379c7d5937f5d84b Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 10 Jul 2026 16:18:09 -0600 Subject: [PATCH] fix(mavlink): select GPS_RAW_INT/GPS2_RAW instance via SENS_GPS_PRIME sensor_gps uORB instance numbering is boot-order dependent for CAN receivers, so GPS_RAW_INT/GPS2_RAW showed a random receiver in dual-GPS setups (e.g. moving base + rover RTK). Resolve the primary from SENS_GPS_PRIME instead, matching the vehicle_gps_position selection including DroneCAN node IDs (2-127). The node ID matching lives in lib/gnss/SensorGpsSelector.hpp and is shared with vehicle_gps_position. Each stream only reports its selected receiver and falls back to NO_GPS keepalives if it has no data. Signed-off-by: Jacob Dahl --- src/lib/gnss/SensorGpsSelector.hpp | 130 ++++++++++++++++++ src/modules/mavlink/streams/GPS2_RAW.hpp | 11 +- src/modules/mavlink/streams/GPS_RAW_INT.hpp | 11 +- .../VehicleGPSPosition.cpp | 12 +- .../sensors/vehicle_gps_position/module.yaml | 6 +- 5 files changed, 158 insertions(+), 12 deletions(-) create mode 100644 src/lib/gnss/SensorGpsSelector.hpp diff --git a/src/lib/gnss/SensorGpsSelector.hpp b/src/lib/gnss/SensorGpsSelector.hpp new file mode 100644 index 000000000000..1926445a0cd1 --- /dev/null +++ b/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 +#include +#include +#include +#include + +// 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(¶meter_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}; +}; diff --git a/src/modules/mavlink/streams/GPS2_RAW.hpp b/src/modules/mavlink/streams/GPS2_RAW.hpp index 9ae6f37792b2..21d7fedec3de 100644 --- a/src/modules/mavlink/streams/GPS2_RAW.hpp +++ b/src/modules/mavlink/streams/GPS2_RAW.hpp @@ -34,6 +34,7 @@ #ifndef GPS2_RAW_HPP #define GPS2_RAW_HPP +#include #include 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; diff --git a/src/modules/mavlink/streams/GPS_RAW_INT.hpp b/src/modules/mavlink/streams/GPS_RAW_INT.hpp index 6f68fe1c83cc..74e7948466fc 100644 --- a/src/modules/mavlink/streams/GPS_RAW_INT.hpp +++ b/src/modules/mavlink/streams/GPS_RAW_INT.hpp @@ -34,6 +34,7 @@ #ifndef GPS_RAW_INT_HPP #define GPS_RAW_INT_HPP +#include #include 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; diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 306451984777..24f65917dfe1 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -35,8 +35,8 @@ #include #include +#include #include -#include 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(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(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()) { diff --git a/src/modules/sensors/vehicle_gps_position/module.yaml b/src/modules/sensors/vehicle_gps_position/module.yaml index ffd9faf1321a..8453ec71a1c8 100644 --- a/src/modules/sensors/vehicle_gps_position/module.yaml +++ b/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