From 0c671d9bc7605d3f2cf234921b70010c5535bfaf Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 9 Feb 2026 00:57:34 -0900 Subject: [PATCH 1/5] msg: sensor_gps quality metrics --- msg/SensorGps.msg | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/msg/SensorGps.msg b/msg/SensorGps.msg index 2e94d174f344..1708f68092c2 100644 --- a/msg/SensorGps.msg +++ b/msg/SensorGps.msg @@ -73,6 +73,22 @@ uint32 SYSTEM_ERROR_CPU_OVERLOAD = 32 uint32 SYSTEM_ERROR_OUTPUT_CONGESTION = 64 uint32 system_error # General errors with the connected GPS receiver +uint16 diff_age # Differential correction age [seconds]. 0xFFFF = unavailable. + +uint8 ANTENNA_STATUS_UNKNOWN = 0 +uint8 ANTENNA_STATUS_INIT = 1 +uint8 ANTENNA_STATUS_OK = 2 +uint8 ANTENNA_STATUS_SHORT = 3 +uint8 ANTENNA_STATUS_OPEN = 4 +uint8 antenna_status # Antenna supervisor state + +uint8 ANTENNA_POWER_UNKNOWN = 0 +uint8 ANTENNA_POWER_OFF = 1 +uint8 ANTENNA_POWER_ON = 2 +uint8 antenna_power # Antenna power status + +uint8 fix_quality # Fix quality/confidence indicator [0-100]. 0 = no confidence, 100 = full confidence. + float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI]) float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI]) float32 heading_accuracy # heading accuracy (rad, [0, 2PI]) From 4240d0ee535dfc4fc478c53b2fba98d34a85dc5e Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 9 Feb 2026 00:58:02 -0900 Subject: [PATCH 2/5] gps: ubx driver for quality --- src/drivers/gps/devices | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index a213f386d3fd..99427567b66a 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit a213f386d3fdae0cf497bb80b41bcc9c0c9e1614 +Subproject commit 99427567b66a4ddd01384ed24097014a02acc70f From 1ee938863ac090ba1dab7ef23bda3a5e694f657d Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 9 Feb 2026 00:58:27 -0900 Subject: [PATCH 3/5] uavcan: subscriber for gnss.quality --- src/drivers/uavcan/libdronecan/dsdl | 2 +- src/drivers/uavcan/sensors/gnss.cpp | 30 +++++++++++++++++++++++++++++ src/drivers/uavcan/sensors/gnss.hpp | 10 ++++++++++ 3 files changed, 41 insertions(+), 1 deletion(-) diff --git a/src/drivers/uavcan/libdronecan/dsdl b/src/drivers/uavcan/libdronecan/dsdl index 993be80a62ec..6035657d000b 160000 --- a/src/drivers/uavcan/libdronecan/dsdl +++ b/src/drivers/uavcan/libdronecan/dsdl @@ -1 +1 @@ -Subproject commit 993be80a62ec957c01fb41115b83663959a49f46 +Subproject commit 6035657d000b4d7f414243d3a9b15f71ae634631 diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index fb02f1d27481..609306f7d3d6 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -57,6 +57,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node, NodeInfoPublisher *node_ UavcanSensorBridgeBase("uavcan_gnss", ORB_ID(sensor_gps), node_info_publisher), _node(node), _sub_auxiliary(node), + _sub_quality(node), _sub_fix(node), _sub_fix2(node), _sub_gnss_heading(node), @@ -90,6 +91,13 @@ UavcanGnssBridge::init() return res; } + res = _sub_quality.start(QualityCbBinder(this, &UavcanGnssBridge::gnss_quality_sub_cb)); + + if (res < 0) { + PX4_WARN("GNSS quality sub failed %i", res); + return res; + } + res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); if (res < 0) { @@ -152,6 +160,13 @@ UavcanGnssBridge::gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure &msg) +{ + _last_gnss_quality_timestamp = hrt_absolute_time(); + _last_quality = msg; +} + void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { @@ -557,6 +572,21 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure sensor_gps.selected_rtcm_instance = _selected_rtcm_instance; sensor_gps.rtcm_injection_rate = _rtcm_injection_rate; + // Apply cached Quality message fields (if received within last 2s) + if (hrt_elapsed_time(&_last_gnss_quality_timestamp) < 2_s) { + sensor_gps.noise_per_ms = _last_quality.noise; + sensor_gps.automatic_gain_control = _last_quality.agc; + sensor_gps.jamming_state = _last_quality.jamming_state; + sensor_gps.jamming_indicator = _last_quality.jamming_indicator; + sensor_gps.spoofing_state = _last_quality.spoofing_state; + sensor_gps.authentication_state = _last_quality.auth_state; + sensor_gps.diff_age = _last_quality.diff_age; + sensor_gps.antenna_status = _last_quality.antenna_status; + sensor_gps.antenna_power = _last_quality.antenna_power; + sensor_gps.fix_quality = _last_quality.fix_quality; + sensor_gps.system_error = _last_quality.system_errors; + } + publish(msg.getSrcNodeID().get(), &sensor_gps); } diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index 72fe9b345341..cac5b5215b59 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -82,6 +83,7 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase * GNSS fix message will be reported via this callback. */ void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure &msg); + void gnss_quality_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_relative_sub_cb(const uavcan::ReceivedDataStructure &msg); @@ -106,6 +108,10 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > AuxiliaryCbBinder; + typedef uavcan::MethodBinder < UavcanGnssBridge *, + void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > + QualityCbBinder; + typedef uavcan::MethodBinder < UavcanGnssBridge *, void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > FixCbBinder; @@ -129,6 +135,7 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase uavcan::INode &_node; uavcan::Subscriber _sub_auxiliary; + uavcan::Subscriber _sub_quality; uavcan::Subscriber _sub_fix; uavcan::Subscriber _sub_fix2; uavcan::Subscriber _sub_gnss_heading; @@ -143,6 +150,9 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase float _last_gnss_auxiliary_hdop{0.0f}; float _last_gnss_auxiliary_vdop{0.0f}; + uint64_t _last_gnss_quality_timestamp{0}; + uavcan::equipment::gnss::Quality _last_quality{}; + uORB::SubscriptionMultiArray _orb_inject_data_sub{ORB_ID::gps_inject_data}; hrt_abstime _last_rtcm_injection_time{0}; ///< time of last rtcm injection uint8_t _selected_rtcm_instance{0}; ///< uorb instance that is being used for RTCM corrections From 9dbf46bb3def3a2be84a3552cca91f2a15b12d68 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 9 Feb 2026 00:59:00 -0900 Subject: [PATCH 4/5] uavcannode: publisher for gnss.quality --- .../uavcannode/Publishers/GnssQuality.hpp | 99 +++++++++++++++++++ src/drivers/uavcannode/UavcanNode.cpp | 2 + 2 files changed, 101 insertions(+) create mode 100644 src/drivers/uavcannode/Publishers/GnssQuality.hpp diff --git a/src/drivers/uavcannode/Publishers/GnssQuality.hpp b/src/drivers/uavcannode/Publishers/GnssQuality.hpp new file mode 100644 index 000000000000..d9f530b04e48 --- /dev/null +++ b/src/drivers/uavcannode/Publishers/GnssQuality.hpp @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "UavcanPublisherBase.hpp" + +#include + +#include +#include + +namespace uavcannode +{ + +class GnssQuality : + public UavcanPublisherBase, + public uORB::SubscriptionCallbackWorkItem, + private uavcan::Publisher +{ +public: + GnssQuality(px4::WorkItem *work_item, uavcan::INode &node) : + UavcanPublisherBase(uavcan::equipment::gnss::Quality::DefaultDataTypeID), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_gps)), + uavcan::Publisher(node) + { + this->setPriority(uavcan::TransferPriority::Default); + } + + void PrintInfo() override + { + if (uORB::SubscriptionCallbackWorkItem::advertised()) { + printf("\t%s -> %s:%d\n", + uORB::SubscriptionCallbackWorkItem::get_topic()->o_name, + uavcan::equipment::gnss::Quality::getDataTypeFullName(), + id()); + } + } + + void BroadcastAnyUpdates() override + { + using uavcan::equipment::gnss::Quality; + + // sensor_gps -> uavcan::equipment::gnss::Quality + sensor_gps_s gps; + + if (uORB::SubscriptionCallbackWorkItem::update(&gps)) { + uavcan::equipment::gnss::Quality quality{}; + + quality.noise = gps.noise_per_ms; + quality.agc = gps.automatic_gain_control; + quality.jamming_state = gps.jamming_state; + quality.jamming_indicator = gps.jamming_indicator; + quality.spoofing_state = gps.spoofing_state; + quality.auth_state = gps.authentication_state; + quality.diff_age = gps.diff_age; + quality.antenna_status = gps.antenna_status; + quality.antenna_power = gps.antenna_power; + quality.fix_quality = gps.fix_quality; + quality.system_errors = gps.system_error; + + uavcan::Publisher::broadcast(quality); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); + } + } +}; +} // namespace uavcannode diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 0940791c90ec..2e38b1c25973 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -59,6 +59,7 @@ #if defined(CONFIG_UAVCANNODE_GNSS_FIX) #include "Publishers/GnssFix2.hpp" #include "Publishers/GnssAuxiliary.hpp" +#include "Publishers/GnssQuality.hpp" #endif // CONFIG_UAVCANNODE_GNSS_FIX #if defined(CONFIG_UAVCANNODE_INDICATED_AIR_SPEED) @@ -385,6 +386,7 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events #if defined(CONFIG_UAVCANNODE_GNSS_FIX) _publisher_list.add(new GnssFix2(this, _node)); _publisher_list.add(new GnssAuxiliary(this, _node)); + _publisher_list.add(new GnssQuality(this, _node)); #endif // CONFIG_UAVCANNODE_GNSS_FIX #if defined(CONFIG_UAVCANNODE_MAGNETIC_FIELD_STRENGTH) From 5b16dcfc086fb26362308da4286f5327fe1c2b11 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 13 Apr 2026 18:07:54 -0800 Subject: [PATCH 5/5] uavcan: align gnss.quality with sensor_gnss_status / GNSS_INTEGRITY MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Drop fix_quality (u8, 0-100) from SensorGps.msg and route the four abstract quality metrics (corrections, receiver summary, gnss signal, post-processing) through sensor_gnss_status (u8, 0-10, 255 = N/A) — matching MAVLink GNSS_INTEGRITY (id 441). - Bridge publishes sensor_gnss_status on incoming Quality messages. - uavcannode GnssQuality publisher reads the four quality fields from sensor_gnss_status while continuing to trigger on sensor_gps. - Submodule bumps for DSDL and PX4-GPSDrivers. --- msg/SensorGps.msg | 2 -- src/drivers/gps/devices | 2 +- src/drivers/uavcan/libdronecan/dsdl | 2 +- src/drivers/uavcan/sensors/gnss.cpp | 11 +++++++++- src/drivers/uavcan/sensors/gnss.hpp | 3 +++ .../uavcannode/Publishers/GnssQuality.hpp | 22 ++++++++++++++++--- 6 files changed, 34 insertions(+), 8 deletions(-) diff --git a/msg/SensorGps.msg b/msg/SensorGps.msg index 1708f68092c2..44cddba42e29 100644 --- a/msg/SensorGps.msg +++ b/msg/SensorGps.msg @@ -87,8 +87,6 @@ uint8 ANTENNA_POWER_OFF = 1 uint8 ANTENNA_POWER_ON = 2 uint8 antenna_power # Antenna power status -uint8 fix_quality # Fix quality/confidence indicator [0-100]. 0 = no confidence, 100 = full confidence. - float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI]) float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI]) float32 heading_accuracy # heading accuracy (rad, [0, 2PI]) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 99427567b66a..51103d82ac26 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 99427567b66a4ddd01384ed24097014a02acc70f +Subproject commit 51103d82ac2697a78bc028c9fe957f2d23f9b66a diff --git a/src/drivers/uavcan/libdronecan/dsdl b/src/drivers/uavcan/libdronecan/dsdl index 6035657d000b..9ef86224366d 160000 --- a/src/drivers/uavcan/libdronecan/dsdl +++ b/src/drivers/uavcan/libdronecan/dsdl @@ -1 +1 @@ -Subproject commit 6035657d000b4d7f414243d3a9b15f71ae634631 +Subproject commit 9ef86224366d63de32502bc1ed1ecb9b76f2f596 diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 609306f7d3d6..bdd812fa9d40 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -583,8 +583,17 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure sensor_gps.diff_age = _last_quality.diff_age; sensor_gps.antenna_status = _last_quality.antenna_status; sensor_gps.antenna_power = _last_quality.antenna_power; - sensor_gps.fix_quality = _last_quality.fix_quality; sensor_gps.system_error = _last_quality.system_errors; + + sensor_gnss_status_s sensor_gnss_status{}; + sensor_gnss_status.timestamp = hrt_absolute_time(); + sensor_gnss_status.device_id = sensor_gps.device_id; + sensor_gnss_status.quality_available = true; + sensor_gnss_status.quality_corrections = _last_quality.corrections_quality; + sensor_gnss_status.quality_receiver = _last_quality.system_status_summary; + sensor_gnss_status.quality_gnss_signals = _last_quality.gnss_signal_quality; + sensor_gnss_status.quality_post_processing = _last_quality.post_processing_quality; + _sensor_gnss_status_pub.publish(sensor_gnss_status); } publish(msg.getSrcNodeID().get(), &sensor_gps); diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index cac5b5215b59..118823ccbc64 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -153,6 +154,8 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase uint64_t _last_gnss_quality_timestamp{0}; uavcan::equipment::gnss::Quality _last_quality{}; + uORB::PublicationMulti _sensor_gnss_status_pub{ORB_ID(sensor_gnss_status)}; + uORB::SubscriptionMultiArray _orb_inject_data_sub{ORB_ID::gps_inject_data}; hrt_abstime _last_rtcm_injection_time{0}; ///< time of last rtcm injection uint8_t _selected_rtcm_instance{0}; ///< uorb instance that is being used for RTCM corrections diff --git a/src/drivers/uavcannode/Publishers/GnssQuality.hpp b/src/drivers/uavcannode/Publishers/GnssQuality.hpp index d9f530b04e48..8ce9f78f8494 100644 --- a/src/drivers/uavcannode/Publishers/GnssQuality.hpp +++ b/src/drivers/uavcannode/Publishers/GnssQuality.hpp @@ -37,8 +37,10 @@ #include +#include #include #include +#include namespace uavcannode { @@ -71,7 +73,6 @@ class GnssQuality : { using uavcan::equipment::gnss::Quality; - // sensor_gps -> uavcan::equipment::gnss::Quality sensor_gps_s gps; if (uORB::SubscriptionCallbackWorkItem::update(&gps)) { @@ -86,14 +87,29 @@ class GnssQuality : quality.diff_age = gps.diff_age; quality.antenna_status = gps.antenna_status; quality.antenna_power = gps.antenna_power; - quality.fix_quality = gps.fix_quality; quality.system_errors = gps.system_error; + quality.corrections_quality = 255; + quality.system_status_summary = 255; + quality.gnss_signal_quality = 255; + quality.post_processing_quality = 255; + + sensor_gnss_status_s status; + + if (_sensor_gnss_status_sub.copy(&status) && status.quality_available) { + quality.corrections_quality = status.quality_corrections; + quality.system_status_summary = status.quality_receiver; + quality.gnss_signal_quality = status.quality_gnss_signals; + quality.post_processing_quality = status.quality_post_processing; + } + uavcan::Publisher::broadcast(quality); - // ensure callback is registered uORB::SubscriptionCallbackWorkItem::registerCallback(); } } + +private: + uORB::Subscription _sensor_gnss_status_sub{ORB_ID(sensor_gnss_status)}; }; } // namespace uavcannode