From e0a2e0b2234d371bbe02002468ba9cbe768413cf Mon Sep 17 00:00:00 2001 From: Alex Klimaj Date: Tue, 20 Jun 2023 09:24:06 -0600 Subject: [PATCH] GPS RTCM selection fixes (#21666) * uavcan rtcm set max num injections * Subscribe to all instances of gps_inject_data and mirror uavcan rtcm pub mirror gps driver * Minor logic refactor in gps and uavcan gps inject data --- src/drivers/gps/gps.cpp | 18 ++++---- src/drivers/uavcan/sensors/gnss.cpp | 70 +++++++++++++++++++++-------- src/drivers/uavcan/sensors/gnss.hpp | 5 ++- 3 files changed, 64 insertions(+), 29 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 008f4d3977..4ed29a8c6e 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -203,7 +204,7 @@ private: const Instance _instance; - uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; + uORB::SubscriptionMultiArray _orb_inject_data_sub{ORB_ID::gps_inject_data}; uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; uORB::Publication _dump_communication_pub{ORB_ID(gps_dump)}; gps_dump_s *_dump_to_device{nullptr}; @@ -530,13 +531,15 @@ void GPS::handleInjectDataTopic() // If there has not been a valid RTCM message for a while, try to switch to a different RTCM link if ((hrt_absolute_time() - _last_rtcm_injection_time) > 5_s) { - for (uint8_t i = 0; i < gps_inject_data_s::MAX_INSTANCES; i++) { - if (_orb_inject_data_sub.ChangeInstance(i)) { - if (_orb_inject_data_sub.copy(&msg)) { + for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { + const bool exists = _orb_inject_data_sub[instance].advertised(); + + if (exists) { + if (_orb_inject_data_sub[instance].copy(&msg)) { if ((hrt_absolute_time() - msg.timestamp) < 5_s) { // Remember that we already did a copy on this instance. already_copied = true; - _selected_rtcm_instance = i; + _selected_rtcm_instance = instance; break; } } @@ -544,9 +547,6 @@ void GPS::handleInjectDataTopic() } } - // Reset instance in case we didn't actually want to switch - _orb_inject_data_sub.ChangeInstance(_selected_rtcm_instance); - bool updated = already_copied; // Limit maximum number of GPS injections to 8 since usually @@ -574,7 +574,7 @@ void GPS::handleInjectDataTopic() } } - updated = _orb_inject_data_sub.update(&msg); + updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg); } while (updated && num_injections < max_num_injections); } diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 2eb59d3b26..90fa23c46f 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -467,29 +467,61 @@ void UavcanGnssBridge::update() // to work. void UavcanGnssBridge::handleInjectDataTopic() { - // Limit maximum number of GPS injections to 6 since usually - // GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo). - // Looking at 6 packets thus guarantees, that at least a full injection - // data set is evaluated. - static constexpr size_t MAX_NUM_INJECTIONS = 6; + // We don't want to call copy again further down if we have already done a + // copy in the selection process. + bool already_copied = false; + gps_inject_data_s msg; - size_t num_injections = 0; - gps_inject_data_s gps_inject_data; + // If there has not been a valid RTCM message for a while, try to switch to a different RTCM link + if ((hrt_absolute_time() - _last_rtcm_injection_time) > 5_s) { - while ((num_injections <= MAX_NUM_INJECTIONS) && _gps_inject_data_sub.update(&gps_inject_data)) { - // Write the message to the gps device. Note that the message could be fragmented. - // But as we don't write anywhere else to the device during operation, we don't - // need to assemble the message first. - if (_publish_rtcm_stream) { - PublishRTCMStream(gps_inject_data.data, gps_inject_data.len); + for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { + const bool exists = _orb_inject_data_sub[instance].advertised(); + + if (exists) { + if (_orb_inject_data_sub[instance].copy(&msg)) { + if ((hrt_absolute_time() - msg.timestamp) < 5_s) { + // Remember that we already did a copy on this instance. + already_copied = true; + _selected_rtcm_instance = instance; + break; + } + } + } } - - if (_publish_moving_baseline_data) { - PublishMovingBaselineData(gps_inject_data.data, gps_inject_data.len); - } - - num_injections++; } + + bool updated = already_copied; + + // Limit maximum number of GPS injections to 8 since usually + // GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo). + // Looking at 8 packets thus guarantees, that at least a full injection + // data set is evaluated. + // Moving Base reuires a higher rate, so we allow up to 8 packets. + const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH; + size_t num_injections = 0; + + do { + if (updated) { + num_injections++; + + // Write the message to the gps device. Note that the message could be fragmented. + // But as we don't write anywhere else to the device during operation, we don't + // need to assemble the message first. + if (_publish_rtcm_stream) { + PublishRTCMStream(msg.data, msg.len); + } + + if (_publish_moving_baseline_data) { + PublishMovingBaselineData(msg.data, msg.len); + } + + _last_rtcm_injection_time = hrt_absolute_time(); + } + + updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg); + + } while (updated && num_injections < max_num_injections); } bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len) diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index b60c238f2f..5b65f57bb6 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -45,6 +45,7 @@ #pragma once #include +#include #include #include #include @@ -123,7 +124,9 @@ private: float _last_gnss_auxiliary_hdop{0.0f}; float _last_gnss_auxiliary_vdop{0.0f}; - uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data)}; + 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 bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data?