diff --git a/src/lib/gnss/SensorGpsSelector.hpp b/src/lib/gnss/SensorGpsSelector.hpp new file mode 100644 index 00000000000..1926445a0cd --- /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 9ae6f37792b..21d7fedec3d 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 @@ private: 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 6f68fe1c83c..74e7948466f 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 @@ private: 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 30645198477..24f65917dfe 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 ffd9faf1321..8453ec71a1c 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