From 5d8874cd2b4b64e892779be20625411d8d88a22d Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Thu, 16 Jul 2026 13:23:26 -0600 Subject: [PATCH] fix(gps): split RTCM corrections and moving-baseline uORB topics (#27097) * fix(gps): split RTCM corrections and moving-baseline uORB topics The single gps_inject_data topic served two unrelated purposes: external fixed-base RTCM corrections (from MAVLink GPS_RTCM_DATA or UAVCAN RTCMStream) and moving-base-to-rover RTCM 4072. In a dual-GPS-with-moving-base plus fixed-base setup, the two streams collided on the same queue and the FMU UAVCAN bridge mirrored fixed-base RTCM onto the MovingBaselineData CAN message, breaking rover heading or RTK fix (see PX4/PX4-Autopilot#27088). Split by role: - rtcm_corrections (renamed from gps_inject_data): external RTCM flowing into the vehicle; producers are MAVLink, UAVCAN RTCMStream, and GPS drivers in dump mode. - rtcm_moving_baseline (new): moving-base GPS output intended for a rover; single producer per vehicle (MAX_INSTANCES = 1). The GPS driver routes its own RTCM output to the right topic via GPSHelper::isMovingBase(), and gates the two inbound streams per role using new GPSHelper virtuals (PX4-GPSDrivers#212): shouldInjectRTCMCorrections() is true for any configured receiver, so a UART2 moving-base rover still accepts fixed-base corrections over its main link; shouldInjectMovingBaseline() is true only for a UART1/CAN heading rover, since a UART2 rover gets the baseline directly in hardware and a moving base produces rather than consumes it. That submodule PR also renames the ambiguous UBXMode fields to name their UART explicitly (RoverWithMovingBase -> RoverWithMovingBaseUART2, MovingBase -> MovingBaseUART2). Septentrio's publish_rtcm_corrections() always publishes to rtcm_moving_baseline (only the Secondary moving base calls it). Rover-side consumers (gps, septentrio, uavcan bridge) drain both topics independently; each topic gets its own stale-link switchover timer so corrections failover is not suppressed by moving-baseline traffic, or vice versa. FMU UAVCAN bridge: two independent drain loops, one per topic. No more dual-publish of a single uORB message onto both RTCMStream and MovingBaselineData CAN streams. CANnode MovingBaselineDataPub subscribes to rtcm_moving_baseline. The bus_type == UAVCAN check is kept, now purely as a loop guard so a node with both CANNODE_PUB_MBD and CANNODE_SUB_MBD does not rebroadcast a peer's moving-baseline data back onto the bus. CANnode RTCMStream subscriber maps each CAN source node ID to its own rtcm_corrections instance (one PublicationMulti per source, capped at MAX_INSTANCES) so multiple CAN RTCM sources (e.g. dual rovers outputting MSM7 for logging plus a fixed-base feed) land on independent uORB instances instead of interleaving on one, which would otherwise defeat the consumer's per-instance stale-link selection. Depends on PX4-GPSDrivers#212 (submodule bump included). Signed-off-by: Jacob Dahl * fix(gps): use separate RTCM parsers for corrections and moving baseline On a rover injecting both fixed-base corrections and moving-baseline RTCM, feeding both streams through a single parser allowed a fragmented frame from one source to be corrupted by bytes interleaved from the other. Reassemble each stream in its own Rtcm3Parser so frames are recovered independently. Also collapse the two near-identical topics into a single RtcmData.msg published under both rtcm_corrections and rtcm_moving_baseline (the SensorGps pattern), track corrections and moving-baseline injection on separate perf counters so the reported corrections rate is no longer inflated by moving-baseline traffic, and zero-initialize the CAN DeviceId unions before populating their fields. Signed-off-by: Jacob Dahl * fix(septentrio): avoid bugprone sizeof division on RTCM byte buffer moving_baseline.data is a uint8_t array, so sizeof(data)/sizeof(data[0]) divides by 1; clang-tidy's bugprone-sizeof-expression flags this as a suspicious sizeof(T)/sizeof(T). Use sizeof(data) directly - the capacity value is unchanged. Signed-off-by: Jacob Dahl * refactor(gps): use dedicated per-stream RTCM drain functions rtcm_moving_baseline has a single publisher (instance 0), so its consumers are now a plain uORB::Subscription instead of a 4-instance SubscriptionMultiArray, and the per-stream selected-instance and stale-link timer members it no longer needs are removed. With each RTCM stream now a fixed type with a single caller, the templated drain helpers (drain_rtcm_subscriptions, the overloaded drain_rtcm_to_can) bought nothing, so replace them with dedicated functions: drainRtcmCorrections()/drainMovingBaseline() in the GPS driver and the UAVCAN bridge, drain_rtcm_corrections()/ drain_moving_baseline() in Septentrio. The UAVCAN bridge calls PublishRTCMStream/PublishMovingBaselineData directly instead of through Forward lambdas. Rename SeptentrioDriver::publish_rtcm_corrections() to publish_moving_baseline(): it is only reached from the Secondary moving-base decode path and only ever emits moving-baseline RTCM. Corrections-path behavior (instance selection, generation-gap warning, burst cap, self-injection filter) is unchanged. Signed-off-by: Jacob Dahl * refactor(gps): rename RTCM inject gate to receiverReady and simplify chunk helper Rename GPSHelper::shouldInjectRTCMCorrections() to receiverReady(). The virtual gates injection of both RTCM corrections and moving-baseline, and for UBX it simply reports whether the receiver is configured, so the name now describes what it actually gates rather than implying it only concerns corrections. Bumps the GPS-drivers submodule to the matching rename. Drop the vestigial message-type template parameter from publish_rtcm_chunks: both topics share rtcm_data_s, so only the publication type needs templating. * fix(septentrio): log dropped RTCM uORB generations Match the gps driver and warn when the RTCM corrections or moving-baseline subscription skips a uORB generation, so dropped injection data is visible. * docs(docs): Docs only update to the RtcmData msg * chore(gps): pin GPSDrivers to merged main Contains #212 (RTCM/moving-baseline gating virtuals), #213 (X20 CFG-ODO NAK tolerance), and #215 (SPARTN input enable, best-effort VALSET). Signed-off-by: Jacob Dahl * fix(septentrio): reassemble RTCM frames per stream before injecting Both drains wrote raw uORB chunks to the receiver, so a fragmented frame on one stream could get the other stream's bytes spliced in mid-frame and corrupt both. Reassemble each stream in its own parser and only write complete frames, mirroring the gps driver. Injection stats now count frames instead of uORB chunks. Signed-off-by: Jacob Dahl * refactor(mavlink)!: remove GPS_RTCM_DATA output stream GPS_RTCM_DATA is a GCS-to-vehicle correction transport; echoing rtcm_corrections back out over MAVLink had no consumer and the echo was lossy anyway (uint8 len and 180-byte payload truncate 300-byte uORB chunks). Receiving GPS_RTCM_DATA is unchanged. Signed-off-by: Jacob Dahl --------- Signed-off-by: Jacob Dahl Co-authored-by: Hamish Willee --- msg/CMakeLists.txt | 2 +- msg/GpsInjectData.msg | 11 - msg/RtcmData.msg | 28 +++ src/drivers/gnss/septentrio/CMakeLists.txt | 1 + src/drivers/gnss/septentrio/septentrio.cpp | 149 ++++++----- src/drivers/gnss/septentrio/septentrio.h | 38 ++- src/drivers/gps/devices | 2 +- src/drivers/gps/gps.cpp | 237 ++++++++++++------ src/drivers/uavcan/sensors/gnss.cpp | 152 +++++------ src/drivers/uavcan/sensors/gnss.hpp | 11 +- .../Publishers/MovingBaselineData.hpp | 24 +- .../Subscribers/MovingBaselineData.hpp | 20 +- .../uavcannode/Subscribers/RTCMStream.hpp | 87 +++++-- src/modules/mavlink/mavlink_messages.cpp | 4 - src/modules/mavlink/mavlink_receiver.cpp | 28 +-- src/modules/mavlink/mavlink_receiver.h | 6 +- src/modules/mavlink/streams/GPS_RTCM_DATA.hpp | 81 ------ 17 files changed, 502 insertions(+), 379 deletions(-) delete mode 100644 msg/GpsInjectData.msg create mode 100644 msg/RtcmData.msg delete mode 100644 src/modules/mavlink/streams/GPS_RTCM_DATA.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 369ec7538ad..457bf9d4f40 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -117,7 +117,6 @@ set(msg_files GpioOut.msg GpioRequest.msg GpsDump.msg - GpsInjectData.msg Gripper.msg HealthReport.msg HeaterStatus.msg @@ -194,6 +193,7 @@ set(msg_files RoverSteeringSetpoint.msg RoverThrottleSetpoint.msg Rpm.msg + RtcmData.msg RtlStatus.msg RtlTimeEstimate.msg SatelliteInfo.msg diff --git a/msg/GpsInjectData.msg b/msg/GpsInjectData.msg deleted file mode 100644 index 9d83fed7d48..00000000000 --- a/msg/GpsInjectData.msg +++ /dev/null @@ -1,11 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -uint32 device_id # unique device ID for the sensor that does not change between power cycles - -uint16 len # length of data -uint8 flags # LSB: 1=fragmented across multiple uORB publications -uint8[300] data # data chunk to write to GPS device (RTCM message) - -uint8 ORB_QUEUE_LENGTH = 8 - -uint8 MAX_INSTANCES = 2 diff --git a/msg/RtcmData.msg b/msg/RtcmData.msg new file mode 100644 index 00000000000..03c47790cf2 --- /dev/null +++ b/msg/RtcmData.msg @@ -0,0 +1,28 @@ +# RTCM3 data exchanged with GNSS receivers. +# +# Published under two topic names that share this definition (see TOPICS below): +# +# rtcm_corrections - external fixed-base corrections fed into the vehicle (MAVLink +# GPS_RTCM_DATA, UAVCAN RTCMStream, GPS drivers in dump mode). Multiple +# sources are allowed, one uORB instance each; consumers select an instance +# via their stale-link logic. +# +# rtcm_moving_baseline - moving-base GPS output (RTCM 4072 or equivalent) intended for a rover. +# Single publisher per vehicle (on-board moving base, or a CANnode +# forwarding MovingBaselineData); consumers only read instance 0. + +uint64 timestamp # [us] Time since system start + +uint32 device_id # [-] Unique device ID of the publisher that produced this RTCM + +uint16 len # [-] Length of data +uint8 flags # [-] LSB: 1=fragmented +uint8[300] data # RTCM3 payload (fixed-base corrections, or moving-baseline data) + +uint8 ORB_QUEUE_LENGTH = 16 + +# Sized for the fixed-base corrections case (up to four independent sources). The moving-baseline +# topic only uses instance 0 (single publisher per vehicle). +uint8 MAX_INSTANCES = 4 + +# TOPICS rtcm_corrections rtcm_moving_baseline diff --git a/src/drivers/gnss/septentrio/CMakeLists.txt b/src/drivers/gnss/septentrio/CMakeLists.txt index 58d2d5721f5..2b28d9dce43 100644 --- a/src/drivers/gnss/septentrio/CMakeLists.txt +++ b/src/drivers/gnss/septentrio/CMakeLists.txt @@ -49,6 +49,7 @@ px4_add_module( module.yaml DEPENDS failure_injection + gnss ) if(TARGET fuzztest::fuzztest) diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index 356d7e8c846..a681029d6a9 100644 --- a/src/drivers/gnss/septentrio/septentrio.cpp +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include "util.h" @@ -1438,7 +1438,7 @@ int SeptentrioDriver::process_message() } case DecodingStatus::RTCMv3: { SEP_TRACE_PARSING("Processing RTCMv3 message"); - publish_rtcm_corrections(_rtcm_decoder->message(), _rtcm_decoder->received_bytes()); + publish_moving_baseline(_rtcm_decoder->message(), _rtcm_decoder->received_bytes()); break; } } @@ -1626,62 +1626,105 @@ int SeptentrioDriver::set_baudrate(uint32_t baud) } } -void SeptentrioDriver::handle_inject_data_topic() +void SeptentrioDriver::drain_rtcm_corrections() { - // We don't want to call copy again further down if we have already done a copy in the selection process. + // rtcm_corrections may have several sources (MAVLink plus CAN nodes), one uORB instance each. + rtcm_data_s msg; bool already_copied = false; - gps_inject_data_s msg; const hrt_abstime now = hrt_absolute_time(); // If there has not been a valid RTCM message for a while, try to switch to a different RTCM link if (now > _last_rtcm_injection_time + 5_s) { - for (int instance = 0; instance < _gps_inject_data_sub.size(); instance++) { - const bool exists = _gps_inject_data_sub[instance].advertised(); - - if (exists) { - if (_gps_inject_data_sub[instance].copy(&msg)) { - if (now < msg.timestamp + 5_s) { - // Remember that we already did a copy on this instance. - already_copied = true; - _selected_rtcm_instance = instance; - break; - } + for (int instance = 0; instance < _rtcm_corrections_sub.size(); instance++) { + if (_rtcm_corrections_sub[instance].advertised() && _rtcm_corrections_sub[instance].copy(&msg)) { + if (msg.device_id != get_device_id() && now < msg.timestamp + 5_s) { + already_copied = true; + _selected_rtcm_instance = instance; + break; } } } } 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 requires 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++; - // Prevent injection of data from self or from ground if moving base and this is rover. - if ((_instance == Instance::Secondary && msg.device_id != get_device_id()) || (_instance == Instance::Main && msg.device_id == get_device_id()) || _receiver_setup != ReceiverSetup::MovingBase) { - /* 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. - */ - write(msg.data, msg.len); - - ++_current_interval_rtcm_injections; + // Prevent injection of data from self + if (msg.device_id != get_device_id()) { + _rtcm_corrections_parser.addData(msg.data, msg.len); _last_rtcm_injection_time = hrt_absolute_time(); } } - updated = _gps_inject_data_sub[_selected_rtcm_instance].update(&msg); + auto &sub = _rtcm_corrections_sub[_selected_rtcm_instance]; + const unsigned last_generation = sub.get_last_generation(); - } while (updated && num_injections < max_num_injections); + updated = sub.update(&msg); + + if (updated && sub.get_last_generation() != last_generation + 1) { + PX4_WARN("%s lost, generation %u -> %u", sub.get_topic()->o_name, + last_generation, sub.get_last_generation()); + } + } while (updated && num_injections < rtcm_data_s::ORB_QUEUE_LENGTH); +} + +void SeptentrioDriver::drain_moving_baseline() +{ + // rtcm_moving_baseline has a single publisher (instance 0); no stale-link selection needed. + rtcm_data_s msg; + size_t num_injections = 0; + + while (num_injections < rtcm_data_s::ORB_QUEUE_LENGTH) { + const unsigned last_generation = _rtcm_moving_baseline_sub.get_last_generation(); + + if (!_rtcm_moving_baseline_sub.update(&msg)) { + break; + } + + num_injections++; + + if (_rtcm_moving_baseline_sub.get_last_generation() != last_generation + 1) { + PX4_WARN("%s lost, generation %u -> %u", _rtcm_moving_baseline_sub.get_topic()->o_name, + last_generation, _rtcm_moving_baseline_sub.get_last_generation()); + } + + // Prevent injection of data from self + if (msg.device_id != get_device_id()) { + _rtcm_moving_baseline_parser.addData(msg.data, msg.len); + } + } +} + +void SeptentrioDriver::inject_rtcm_frames(gnss::Rtcm3Parser &parser) +{ + size_t frame_len = {}; + const uint8_t *frame_ptr = {}; + + while ((frame_ptr = parser.getNextMessage(&frame_len)) != nullptr) { + write(frame_ptr, frame_len); + parser.consumeMessage(frame_len); + ++_current_interval_rtcm_injections; + } +} + +void SeptentrioDriver::handle_inject_data_topic() +{ + // Fixed-base RTCM corrections (from MAVLink GPS_RTCM_DATA, UAVCAN RTCMStream). + // Both the moving-base (Secondary) and the rover (Main) can benefit from external RTCM. + drain_rtcm_corrections(); + inject_rtcm_frames(_rtcm_corrections_parser); + + // Moving-baseline RTCM is only consumed by the rover (Main instance) in a moving-base setup. + // Single publisher, so no instance selection - just drain it into the receiver. + if (_receiver_setup == ReceiverSetup::MovingBase && _instance == Instance::Main) { + drain_moving_baseline(); + inject_rtcm_frames(_rtcm_moving_baseline_parser); + } } void SeptentrioDriver::publish() @@ -1713,37 +1756,27 @@ bool SeptentrioDriver::first_gps_uorb_message_created() const return _sensor_gps.timestamp != 0; } -void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len) +void SeptentrioDriver::publish_moving_baseline(uint8_t *data, size_t len) { - gps_inject_data_s gps_inject_data{}; + // The only path into this function is the moving-base Secondary decoding RTCM from its + // receiver (see _rtcm_decoder allocation in the constructor), so the output is always + // moving-baseline data intended for the rover. + rtcm_data_s moving_baseline{}; - gps_inject_data.timestamp = hrt_absolute_time(); - gps_inject_data.device_id = get_device_id(); + moving_baseline.timestamp = hrt_absolute_time(); + moving_baseline.device_id = get_device_id(); - size_t capacity = (sizeof(gps_inject_data.data) / sizeof(gps_inject_data.data[0])); - - if (len > capacity) { - gps_inject_data.flags = 1; //LSB: 1=fragmented - - } else { - gps_inject_data.flags = 0; - } + const size_t capacity = sizeof(moving_baseline.data); + moving_baseline.flags = (len > capacity) ? 1 : 0; // LSB: 1=fragmented size_t written = 0; while (written < len) { - - gps_inject_data.len = len - written; - - if (gps_inject_data.len > capacity) { - gps_inject_data.len = capacity; - } - - memcpy(gps_inject_data.data, &data[written], gps_inject_data.len); - - _gps_inject_data_pub.publish(gps_inject_data); - - written = written + gps_inject_data.len; + const size_t chunk = math::min(len - written, capacity); + moving_baseline.len = chunk; + memcpy(moving_baseline.data, &data[written], chunk); + _rtcm_moving_baseline_pub.publish(moving_baseline); + written += chunk; } } diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h index 6370d07565c..54c1a5af831 100644 --- a/src/drivers/gnss/septentrio/septentrio.h +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -50,12 +50,14 @@ #include #include #include +#include #include #include #include #include #include -#include +#include +#include #include #include #include @@ -527,6 +529,25 @@ private: */ void handle_inject_data_topic(); + /** + * @brief Drain the multi-instance rtcm_corrections subscription into its RTCM parser, + * selecting an active instance if the current one goes stale. + */ + void drain_rtcm_corrections(); + + /** + * @brief Drain the single-publisher rtcm_moving_baseline subscription into its RTCM parser. + */ + void drain_moving_baseline(); + + /** + * @brief Write all complete RTCM frames reassembled in a parser to the receiver. + * + * The two inject streams are written frame-atomically so chunks of a fragmented frame on + * one stream can never interleave with the other stream's bytes mid-frame. + */ + void inject_rtcm_frames(gnss::Rtcm3Parser &parser); + /** * @brief Send data to the receiver, such as RTCM injections. * @@ -555,12 +576,12 @@ private: bool first_gps_uorb_message_created() const; /** - * @brief Publish RTCM corrections. + * @brief Publish moving-baseline RTCM produced by the Secondary moving base for the rover. * * @param data: The raw data to publish * @param len: The size of `data` */ - void publish_rtcm_corrections(uint8_t *data, size_t len); + void publish_moving_baseline(uint8_t *data, size_t len); /** * @brief Dump gps communication. @@ -724,8 +745,12 @@ private: DumpMode _dump_communication_mode {DumpMode::Disabled}; ///< GPS communication dump mode device::Serial _uart {}; ///< Serial UART port for communication with the receiver char _port[20] {}; ///< The path of the used serial device - hrt_abstime _last_rtcm_injection_time {0}; ///< Time of last RTCM injection + hrt_abstime _last_rtcm_injection_time {0}; ///< Time of last RTCM corrections injection uint8_t _selected_rtcm_instance {0}; ///< uORB instance that is being used for RTCM corrections + // Separate parser per inject stream: frames are only written to the receiver once complete, + // so fixed-base corrections and moving-baseline bytes cannot interleave mid-frame. + gnss::Rtcm3Parser _rtcm_corrections_parser {}; ///< Frame reassembly for rtcm_corrections + gnss::Rtcm3Parser _rtcm_moving_baseline_parser {}; ///< Frame reassembly for rtcm_moving_baseline uint8_t _spoofing_state {0}; ///< Receiver spoofing state uint8_t _jamming_state {0}; ///< Receiver jamming state bool _time_synced {false}; ///< Receiver time in sync with GPS time @@ -765,9 +790,10 @@ private: uORB::PublicationMulti _sensor_gps_pub {ORB_ID(sensor_gps)}; ///< uORB publication for gps position uORB::PublicationMulti _sensor_gnss_status_pub {ORB_ID(sensor_gnss_status)}; ///< uORB publication for gnss status uORB::Publication _gps_dump_pub {ORB_ID(gps_dump)}; ///< uORB publication for dump GPS data - uORB::Publication _gps_inject_data_pub {ORB_ID(gps_inject_data)}; ///< uORB publication for injected data to the receiver + uORB::Publication _rtcm_moving_baseline_pub {ORB_ID(rtcm_moving_baseline)}; ///< uORB publication for moving-baseline RTCM output uORB::PublicationMulti _satellite_info_pub {ORB_ID(satellite_info)}; ///< uORB publication for satellite info - uORB::SubscriptionMultiArray _gps_inject_data_sub {ORB_ID::gps_inject_data}; ///< uORB subscription about data to inject to the receiver + uORB::SubscriptionMultiArray _rtcm_corrections_sub {ORB_ID::rtcm_corrections}; ///< uORB subscription for external RTCM corrections + uORB::Subscription _rtcm_moving_baseline_sub {ORB_ID(rtcm_moving_baseline)}; ///< uORB subscription for moving-baseline RTCM input (single publisher) failure_injection::Config _failure_config; failure_injection::Stuck _stuck; diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 52bc6ecef4f..1925b375de0 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 52bc6ecef4fb80cc5b7fef12095f65dd3cbe957e +Subproject commit 1925b375de0dc2efd1766cafdcb79338f521bcc6 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index bd00cfaab42..020b6066cd2 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -65,7 +65,7 @@ #include #include #include -#include +#include #include #include @@ -248,26 +248,34 @@ private: failure_injection::Stuck _stuck; float _rate{0.0f}; ///< position update rate - float _rtcm_injection_rate{0.0f}; ///< RTCM message injection rate - unsigned _rtcm_injection_rate_message_count{0}; ///< counter for number of RTCM messages + float _rtcm_injection_rate{0.0f}; ///< RTCM corrections injection rate (Hz) + uint64_t _last_rtcm_corrections_injection_count{0}; ///< corrections-injection perf snapshot for rate calc unsigned _num_bytes_read{0}; ///< counter for number of read bytes from the UART (within update interval) unsigned _rate_reading{0}; ///< reading rate in B/s - hrt_abstime _last_rtcm_injection_time{0}; ///< time of last rtcm injection + hrt_abstime _last_rtcm_injection_time{0}; ///< time of last RTCM corrections injection uint8_t _selected_rtcm_instance{0}; ///< uorb instance that is being used for RTCM corrections const Instance _instance; - uORB::SubscriptionMultiArray _orb_inject_data_sub{ORB_ID::gps_inject_data}; - uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + uORB::SubscriptionMultiArray _rtcm_corrections_sub{ORB_ID::rtcm_corrections}; + uORB::Subscription _rtcm_moving_baseline_sub{ORB_ID(rtcm_moving_baseline)}; + uORB::PublicationMulti _rtcm_corrections_pub{ORB_ID(rtcm_corrections)}; + uORB::Publication _rtcm_moving_baseline_pub{ORB_ID(rtcm_moving_baseline)}; uORB::Publication _dump_communication_pub{ORB_ID(gps_dump)}; gps_dump_s *_dump_to_device{nullptr}; gps_dump_s *_dump_from_device{nullptr}; gps_dump_comm_mode_t _dump_communication_mode{gps_dump_comm_mode_t::Disabled}; - gnss::Rtcm3Parser _rtcm_parser{}; + // Separate RTCM parsers per source. A fragmented fixed-base frame must not be corrupted by + // moving-baseline bytes appended mid-frame (e.g. fixed-base + moving-base + rover setups, where + // the rover injects both streams), so each stream reassembles in its own buffer. + gnss::Rtcm3Parser _rtcm_corrections_parser{}; + gnss::Rtcm3Parser _rtcm_moving_baseline_parser{}; perf_counter_t _uart_tx_buffer_full_perf{perf_alloc(PC_COUNT, MODULE_NAME": tx buf full")}; perf_counter_t _rtcm_buffer_full_perf{perf_alloc(PC_COUNT, MODULE_NAME": rtcm buf full")}; + perf_counter_t _rtcm_corrections_injection_perf{perf_alloc(PC_COUNT, MODULE_NAME": rtcm corrections injected")}; + perf_counter_t _rtcm_moving_baseline_injection_perf{perf_alloc(PC_COUNT, MODULE_NAME": rtcm moving baseline injected")}; static px4::atomic_bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1 /// and thus we wait until the first one publishes at least one message. @@ -313,6 +321,23 @@ private: */ void handleInjectDataTopic(); + /** + * Drain the multi-instance rtcm_corrections subscription into its RTCM parser, selecting an + * active instance if the current one goes stale. + */ + void drainRtcmCorrections(); + + /** + * Drain the single-publisher rtcm_moving_baseline subscription into its RTCM parser. + */ + void drainMovingBaseline(); + + /** + * Inject all complete RTCM frames reassembled in a parser into the receiver, counting each + * injected frame on the given perf counter. + */ + void injectRtcmFrames(gnss::Rtcm3Parser &parser, perf_counter_t injection_perf); + /** * send data to the device, such as an RTCM stream * @param data @@ -460,6 +485,8 @@ GPS::~GPS() perf_free(_uart_tx_buffer_full_perf); perf_free(_rtcm_buffer_full_perf); + perf_free(_rtcm_corrections_injection_perf); + perf_free(_rtcm_moving_baseline_injection_perf); delete _sat_info; delete _dump_to_device; @@ -609,64 +636,42 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) return ret; } -void GPS::handleInjectDataTopic() +void GPS::drainRtcmCorrections() { - if (!_helper->shouldInjectRTCM()) { - return; - } - - // We don't want to call copy again further down if we have already done a - // copy in the selection process. + // rtcm_corrections may have several sources (MAVLink plus CAN nodes), one uORB instance each. + rtcm_data_s msg; bool already_copied = false; - gps_inject_data_s msg; const hrt_abstime now = hrt_absolute_time(); // If there has not been a valid RTCM message for a while, try to switch to a different RTCM link if (now > _last_rtcm_injection_time + 5_s) { - - for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { - const bool exists = _orb_inject_data_sub[instance].advertised(); - - if (exists && _orb_inject_data_sub[instance].copy(&msg)) { + for (int instance = 0; instance < _rtcm_corrections_sub.size(); instance++) { + if (_rtcm_corrections_sub[instance].advertised() && _rtcm_corrections_sub[instance].copy(&msg)) { /* Don't select the own RTCM instance. In case it has a lower * instance number, it will be selected and will be rejected * later in the code, resulting in no RTCM injection at all. */ - if (msg.device_id != get_device_id()) { - // Only use the message if it is up to date - if (now < msg.timestamp + 5_s) { - // Remember that we already did a copy on this instance. - already_copied = true; - _selected_rtcm_instance = instance; - break; - } + if (msg.device_id != get_device_id() && now < msg.timestamp + 5_s) { + already_copied = true; + _selected_rtcm_instance = instance; + break; } } } } 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. - // Drain uORB messages into RTCM parser and inject full messages after draining the queue. - const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH; size_t num_injections = 0; + // Limit maximum number of injections per call so a burst can't starve the driver loop. do { if (updated) { num_injections++; // Prevent injection of data from self if (msg.device_id != get_device_id()) { - // Add data to the RTCM parser buffer for frame reassembly - size_t added = _rtcm_parser.addData(msg.data, msg.len); - - if (added < msg.len) { + if (_rtcm_corrections_parser.addData(msg.data, msg.len) < msg.len) { perf_count(_rtcm_buffer_full_perf); } @@ -674,25 +679,81 @@ void GPS::handleInjectDataTopic() } } - auto &gps_inject_data_sub = _orb_inject_data_sub[_selected_rtcm_instance]; + auto &sub = _rtcm_corrections_sub[_selected_rtcm_instance]; + const unsigned last_generation = sub.get_last_generation(); - const unsigned last_generation = gps_inject_data_sub.get_last_generation(); + updated = sub.update(&msg); - updated = gps_inject_data_sub.update(&msg); + if (updated && sub.get_last_generation() != last_generation + 1) { + PX4_WARN("%s lost, generation %u -> %u", sub.get_topic()->o_name, + last_generation, sub.get_last_generation()); + } + } while (updated && num_injections < rtcm_data_s::ORB_QUEUE_LENGTH); +} - if (updated) { - if (gps_inject_data_sub.get_last_generation() != last_generation + 1) { - PX4_WARN("gps_inject_data lost, generation %u -> %u", last_generation, gps_inject_data_sub.get_last_generation()); - } +void GPS::drainMovingBaseline() +{ + // rtcm_moving_baseline has a single publisher (instance 0), so there is no stale-link instance + // selection - just drain the queue into the parser. + rtcm_data_s msg; + size_t num_injections = 0; + + while (num_injections < rtcm_data_s::ORB_QUEUE_LENGTH) { + const unsigned last_generation = _rtcm_moving_baseline_sub.get_last_generation(); + + if (!_rtcm_moving_baseline_sub.update(&msg)) { + break; } - } while (updated && num_injections < max_num_injections); + num_injections++; - // Now inject all complete RTCM frames from the parser buffer + if (_rtcm_moving_baseline_sub.get_last_generation() != last_generation + 1) { + PX4_WARN("%s lost, generation %u -> %u", _rtcm_moving_baseline_sub.get_topic()->o_name, + last_generation, _rtcm_moving_baseline_sub.get_last_generation()); + } + + // Prevent injection of data from self + if (msg.device_id != get_device_id()) { + if (_rtcm_moving_baseline_parser.addData(msg.data, msg.len) < msg.len) { + perf_count(_rtcm_buffer_full_perf); + } + } + } +} + +void GPS::handleInjectDataTopic() +{ + // receiverReady() is false until the receiver is configured, which keeps us from writing to the + // device mid-configuration. + if (!_helper->receiverReady()) { + return; + } + + // Fixed-base RTCM corrections (MAVLink GPS_RTCM_DATA, UAVCAN RTCMStream). Every configured + // receiver can use these - including a UART2 moving-base rover, whose baseline arrives in + // hardware but which still wants fixed-base corrections over its main link. Reassemble and inject + // them through their own parser before touching the moving-baseline stream. + drainRtcmCorrections(); + injectRtcmFrames(_rtcm_corrections_parser, _rtcm_corrections_injection_perf); + + // Moving-baseline RTCM (RTCM 4072 etc.) from a peer moving-base GPS. Only a heading rover that + // receives the baseline through the flight controller (UART1 / CAN) injects it here; a UART2 + // rover gets it in hardware and a moving base produces it. Single publisher, so no instance + // selection - and a separate parser keeps the two byte streams from interleaving into corrupt + // frames. + if (_helper->shouldInjectMovingBaseline()) { + drainMovingBaseline(); + injectRtcmFrames(_rtcm_moving_baseline_parser, _rtcm_moving_baseline_injection_perf); + } +} + +void GPS::injectRtcmFrames(gnss::Rtcm3Parser &parser, perf_counter_t injection_perf) +{ + // Inject all complete RTCM frames reassembled in this parser. size_t frame_len = {}; const uint8_t *frame_ptr = {}; - while ((frame_ptr = _rtcm_parser.getNextMessage(&frame_len)) != nullptr) { + while ((frame_ptr = parser.getNextMessage(&frame_len)) != nullptr) { // Check TX buffer space before writing if (_interface == GPSHelper::Interface::UART) { ssize_t tx_available = _uart.txSpaceAvailable(); @@ -705,8 +766,8 @@ void GPS::handleInjectDataTopic() } injectData(frame_ptr, frame_len); - _rtcm_parser.consumeMessage(frame_len); - _rtcm_injection_rate_message_count++; + parser.consumeMessage(frame_len); + perf_count(injection_perf); } } @@ -873,16 +934,16 @@ GPS::run() switch (gps_ubx_mode) { case 1: // heading if (_instance == Instance::Main) { - ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBase; + ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBaseUART2; } else { - ubx_mode = GPSDriverUBX::UBXMode::MovingBase; + ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART2; } break; case 2: - ubx_mode = GPSDriverUBX::UBXMode::MovingBase; + ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART2; break; case 3: @@ -900,7 +961,7 @@ GPS::run() break; case 5: // rover with static base on Uart2 - ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUart2; + ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUART2; break; case 6: @@ -1190,7 +1251,7 @@ GPS::run() #if defined(CONFIG_GPS_UBX) - if ((ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBase) + if ((ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBaseUART2) || (ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1)) { /* The MB rover will wait as long as possible to compute a navigation solution, * possibly lowering the navigation rate all the way to 1 Hz while doing so. */ @@ -1228,11 +1289,14 @@ GPS::run() if (now > last_rate_measurement + 5_s) { float dt = (float)((now - last_rate_measurement)) / 1e6f; _rate = last_rate_count / dt; - _rtcm_injection_rate = _rtcm_injection_rate_message_count / dt; + // Report the fixed-base corrections injection rate; moving-baseline injection is + // tracked separately on its own perf counter. + const uint64_t corrections_count = perf_event_count(_rtcm_corrections_injection_perf); + _rtcm_injection_rate = (corrections_count - _last_rtcm_corrections_injection_count) / dt; + _last_rtcm_corrections_injection_count = corrections_count; _rate_reading = _num_bytes_read / dt; last_rate_measurement = now; last_rate_count = 0; - _rtcm_injection_rate_message_count = 0; _num_bytes_read = 0; _helper->storeUpdateRates(); _helper->resetUpdateRates(); @@ -1397,6 +1461,8 @@ GPS::print_status() perf_print_counter(_uart_tx_buffer_full_perf); perf_print_counter(_rtcm_buffer_full_perf); + perf_print_counter(_rtcm_corrections_injection_perf); + perf_print_counter(_rtcm_moving_baseline_injection_perf); if (_instance == Instance::Main && _secondary_instance.load()) { GPS *secondary_instance = _secondary_instance.load(); @@ -1477,38 +1543,45 @@ GPS::publishSatelliteInfo() } } -void -GPS::publishRTCMCorrections(uint8_t *data, size_t len) +// Chunk an RTCM byte stream into a uORB message and publish it. RTCM frames larger than the +// message payload are split across consecutive publications (flags LSB = fragmented). Templated +// on the publication so the same code serves both the corrections and moving-baseline topics. +template +static void publish_rtcm_chunks(PubT &pub, const uint8_t *data, size_t len, hrt_abstime timestamp, + uint32_t device_id) { - gps_inject_data_s gps_inject_data{}; + rtcm_data_s msg{}; + msg.timestamp = timestamp; + msg.device_id = device_id; - gps_inject_data.timestamp = hrt_absolute_time(); - gps_inject_data.device_id = get_device_id(); - - size_t capacity = (sizeof(gps_inject_data.data) / sizeof(gps_inject_data.data[0])); - - if (len > capacity) { - gps_inject_data.flags = 1; //LSB: 1=fragmented - - } else { - gps_inject_data.flags = 0; - } + const size_t capacity = sizeof(msg.data); + msg.flags = (len > capacity) ? 1 : 0; // LSB: 1=fragmented size_t written = 0; while (written < len) { + const size_t chunk = math::min(len - written, capacity); + msg.len = chunk; + memcpy(msg.data, &data[written], chunk); + pub.publish(msg); + written += chunk; + } +} - gps_inject_data.len = len - written; +void +GPS::publishRTCMCorrections(uint8_t *data, size_t len) +{ + const hrt_abstime timestamp = hrt_absolute_time(); + const uint32_t device_id = get_device_id(); - if (gps_inject_data.len > capacity) { - gps_inject_data.len = capacity; - } + // If this GPS is a moving base, its RTCM output is moving-baseline data for a rover (not + // external corrections). Route it to a dedicated topic so downstream consumers can tell it + // apart from fixed-base RTCM. + if (_helper && _helper->isMovingBase()) { + publish_rtcm_chunks(_rtcm_moving_baseline_pub, data, len, timestamp, device_id); - memcpy(gps_inject_data.data, &data[written], gps_inject_data.len); - - _gps_inject_data_pub.publish(gps_inject_data); - - written = written + gps_inject_data.len; + } else { + publish_rtcm_chunks(_rtcm_corrections_pub, data, len, timestamp, device_id); } } diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 9e520a728dc..9af33cf2e39 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -625,16 +625,85 @@ void UavcanGnssBridge::update() handleInjectDataTopic(); } -// Partially taken from src/drivers/gps/gps.cpp -// This listens on the gps_inject_data uORB topic for RTCM data -// sent from a GCS (usually over MAVLINK GPS_RTCM_DATA). -// Forwarding this data to the UAVCAN bus enables DGPS/RTK GPS -// to work. +// Drains rtcm_corrections (fixed-base RTCM from a GCS over MAVLink, or CAN nodes) to +// uavcan::RTCMStream. Several sources may be active (one uORB instance each); switch to a +// different instance if the selected one goes stale. +void UavcanGnssBridge::drainRtcmCorrections() +{ + const hrt_abstime now = hrt_absolute_time(); + + bool already_copied = false; + rtcm_data_s msg; + + if (now > _last_rtcm_injection_time + 5_s) { + for (int instance = 0; instance < _rtcm_corrections_sub.size(); instance++) { + if (_rtcm_corrections_sub[instance].advertised() && _rtcm_corrections_sub[instance].copy(&msg)) { + if (now < msg.timestamp + 5_s) { + already_copied = true; + _selected_rtcm_instance = instance; + break; + } + } + } + } + + bool updated = already_copied; + size_t num_injections = 0; + + do { + if (updated) { + num_injections++; + PublishRTCMStream(msg.data, msg.len); + _rtcm_injection_rate_message_count++; + _last_rtcm_injection_time = hrt_absolute_time(); + } + + auto &sub = _rtcm_corrections_sub[_selected_rtcm_instance]; + const unsigned last_generation = sub.get_last_generation(); + + updated = sub.update(&msg); + + if (updated && sub.get_last_generation() != last_generation + 1) { + PX4_WARN("%s lost, generation %u -> %u", sub.get_topic()->o_name, + last_generation, sub.get_last_generation()); + } + } while (updated && num_injections < rtcm_data_s::ORB_QUEUE_LENGTH); +} + +// Drains rtcm_moving_baseline (moving-base RTCM 4072 from a peer GPS) to +// ardupilot::MovingBaselineData. Single publisher (instance 0), so no stale-link selection. +void UavcanGnssBridge::drainMovingBaseline() +{ + rtcm_data_s msg; + size_t num_injections = 0; + + while (num_injections < rtcm_data_s::ORB_QUEUE_LENGTH) { + const unsigned last_generation = _rtcm_moving_baseline_sub.get_last_generation(); + + if (!_rtcm_moving_baseline_sub.update(&msg)) { + break; + } + + num_injections++; + + if (_rtcm_moving_baseline_sub.get_last_generation() != last_generation + 1) { + PX4_WARN("%s lost, generation %u -> %u", _rtcm_moving_baseline_sub.get_topic()->o_name, + last_generation, _rtcm_moving_baseline_sub.get_last_generation()); + } + + PublishMovingBaselineData(msg.data, msg.len); + } +} + +// The two RTCM streams use separate CAN publishers: fixed-base corrections go to +// uavcan::RTCMStream and moving-baseline RTCM to ardupilot::MovingBaselineData, so corrections +// are never mirrored onto the moving-baseline message. void UavcanGnssBridge::handleInjectDataTopic() { - hrt_abstime now = hrt_absolute_time(); + const hrt_abstime now = hrt_absolute_time(); - // measure RTCM update rate every 5 seconds + // Measure the fixed-base corrections injection rate every 5 seconds (moving-baseline forwarding + // is tracked separately via _moving_baseline_data_pub_perf, so it is excluded here). if (now > _last_rate_measurement + 5_s) { float dt = (now - _last_rate_measurement) / 1e6f; _rtcm_injection_rate = _rtcm_injection_rate_message_count / dt; @@ -642,72 +711,13 @@ void UavcanGnssBridge::handleInjectDataTopic() _rtcm_injection_rate_message_count = 0; } - // 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; - - // If there has not been a valid RTCM message for a while, try to switch to a different RTCM link - if (now > _last_rtcm_injection_time + 5_s) { - - 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_rtcm_stream) { + drainRtcmCorrections(); } - 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 requires 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); - } - - ++_rtcm_injection_rate_message_count; - _last_rtcm_injection_time = hrt_absolute_time(); - } - - auto &gps_inject_data_sub = _orb_inject_data_sub[_selected_rtcm_instance]; - - const unsigned last_generation = gps_inject_data_sub.get_last_generation(); - - updated = gps_inject_data_sub.update(&msg); - - if (updated) { - if (gps_inject_data_sub.get_last_generation() != last_generation + 1) { - PX4_WARN("gps_inject_data lost, generation %u -> %u", last_generation, gps_inject_data_sub.get_last_generation()); - } - } - - } while (updated && num_injections < max_num_injections); + if (_publish_moving_baseline_data) { + drainMovingBaseline(); + } } 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 74962e61da5..f0da5e13352 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include @@ -103,6 +103,8 @@ private: const uint8_t spoofing_state); void handleInjectDataTopic(); + void drainRtcmCorrections(); + void drainMovingBaseline(); bool PublishRTCMStream(const uint8_t *data, size_t data_len); bool PublishMovingBaselineData(const uint8_t *data, size_t data_len); @@ -147,7 +149,8 @@ private: float _last_gnss_auxiliary_hdop{0.0f}; float _last_gnss_auxiliary_vdop{0.0f}; - uORB::SubscriptionMultiArray _orb_inject_data_sub{ORB_ID::gps_inject_data}; + uORB::SubscriptionMultiArray _rtcm_corrections_sub{ORB_ID::rtcm_corrections}; + uORB::Subscription _rtcm_moving_baseline_sub{ORB_ID(rtcm_moving_baseline)}; 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 @@ -180,6 +183,6 @@ private: perf_counter_t _moving_baseline_data_sub_perf{nullptr}; hrt_abstime _last_rate_measurement{0}; - float _rtcm_injection_rate{0.f}; ///< RTCM message injection rate - unsigned _rtcm_injection_rate_message_count{0}; ///< number of RTCM messages since last rate calculation + float _rtcm_injection_rate{0.f}; ///< fixed-base corrections injection rate (Hz); moving-baseline tracked via _moving_baseline_data_pub_perf + unsigned _rtcm_injection_rate_message_count{0}; ///< fixed-base corrections messages since last rate calculation }; diff --git a/src/drivers/uavcannode/Publishers/MovingBaselineData.hpp b/src/drivers/uavcannode/Publishers/MovingBaselineData.hpp index ad90387b429..db1b06e393e 100644 --- a/src/drivers/uavcannode/Publishers/MovingBaselineData.hpp +++ b/src/drivers/uavcannode/Publishers/MovingBaselineData.hpp @@ -39,7 +39,7 @@ #include #include -#include +#include namespace uavcannode { @@ -52,7 +52,7 @@ class MovingBaselineDataPub : public: MovingBaselineDataPub(px4::WorkItem *work_item, uavcan::INode &node) : UavcanPublisherBase(ardupilot::gnss::MovingBaselineData::DefaultDataTypeID), - uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(gps_inject_data)), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(rtcm_moving_baseline)), uavcan::Publisher(node) { this->setPriority(uavcan::TransferPriority::NumericallyMax); @@ -70,26 +70,28 @@ public: void BroadcastAnyUpdates() override { - // gps_inject_data -> ardupilot::gnss::MovingBaselineData - gps_inject_data_s gps_inject_data = {}; + // rtcm_moving_baseline -> ardupilot::gnss::MovingBaselineData + rtcm_data_s moving_baseline = {}; unsigned last_generation = uORB::SubscriptionCallbackWorkItem::get_last_generation(); bool updated = false; // Drain all available messages from the queue and publish to CAN. - while ((updated = uORB::SubscriptionCallbackWorkItem::update(&gps_inject_data))) { + while ((updated = uORB::SubscriptionCallbackWorkItem::update(&moving_baseline))) { unsigned current_generation = uORB::SubscriptionCallbackWorkItem::get_last_generation(); if (current_generation != last_generation + 1) { - PX4_WARN("gps_inject_data lost, generation %u -> %u", last_generation, current_generation); + PX4_WARN("rtcm_moving_baseline lost, generation %u -> %u", last_generation, current_generation); } last_generation = current_generation; - // Prevent republishing rtcm data we received from uavcan + // Don't rebroadcast moving-baseline RTCM that we received over CAN. Without this, a node + // configured with both CANNODE_PUB_MBD and CANNODE_SUB_MBD would echo peer broadcasts back + // onto the bus, creating a CAN rebroadcast loop. union device::Device::DeviceId device_id; - device_id.devid = gps_inject_data.device_id; + device_id.devid = moving_baseline.device_id; if (device_id.devid_s.bus_type == device::Device::DeviceBusType::DeviceBusType_UAVCAN) { continue; @@ -101,15 +103,15 @@ public: size_t written = 0; int result = 0; - while ((result >= 0) && written < gps_inject_data.len) { - size_t chunk_size = gps_inject_data.len - written; + while ((result >= 0) && written < moving_baseline.len) { + size_t chunk_size = moving_baseline.len - written; if (chunk_size > capacity) { chunk_size = capacity; } for (size_t i = 0; i < chunk_size; i++) { - mbd.data.push_back(gps_inject_data.data[written]); + mbd.data.push_back(moving_baseline.data[written]); written += 1; } diff --git a/src/drivers/uavcannode/Subscribers/MovingBaselineData.hpp b/src/drivers/uavcannode/Subscribers/MovingBaselineData.hpp index 80a2af1c428..b6e761c5b09 100644 --- a/src/drivers/uavcannode/Subscribers/MovingBaselineData.hpp +++ b/src/drivers/uavcannode/Subscribers/MovingBaselineData.hpp @@ -39,7 +39,7 @@ #include #include -#include +#include namespace uavcannode { @@ -75,7 +75,7 @@ public: printf("\t%s:%d -> %s\n", ardupilot::gnss::MovingBaselineData::getDataTypeFullName(), ardupilot::gnss::MovingBaselineData::DefaultDataTypeID, - _gps_inject_data_pub.get_topic()->o_name); + _rtcm_moving_baseline_pub.get_topic()->o_name); } private: @@ -83,26 +83,26 @@ private: { // Don't republish a message from ourselves if (msg.getSrcNodeID().get() != getNode().getNodeID().get()) { - gps_inject_data_s gps_inject_data{}; + rtcm_data_s moving_baseline{}; - gps_inject_data.len = msg.data.size(); + moving_baseline.len = msg.data.size(); - memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len); + memcpy(moving_baseline.data, &msg.data[0], moving_baseline.len); - gps_inject_data.timestamp = hrt_absolute_time(); + moving_baseline.timestamp = hrt_absolute_time(); - union device::Device::DeviceId device_id; + union device::Device::DeviceId device_id {}; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN; device_id.devid_s.address = msg.getSrcNodeID().get(); device_id.devid_s.devtype = DRV_GPS_DEVTYPE_UAVCAN; - gps_inject_data.device_id = device_id.devid; + moving_baseline.device_id = device_id.devid; - _gps_inject_data_pub.publish(gps_inject_data); + _rtcm_moving_baseline_pub.publish(moving_baseline); } } - uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + uORB::Publication _rtcm_moving_baseline_pub{ORB_ID(rtcm_moving_baseline)}; }; } // namespace uavcannode diff --git a/src/drivers/uavcannode/Subscribers/RTCMStream.hpp b/src/drivers/uavcannode/Subscribers/RTCMStream.hpp index 83e2c847372..ef041f5dd16 100644 --- a/src/drivers/uavcannode/Subscribers/RTCMStream.hpp +++ b/src/drivers/uavcannode/Subscribers/RTCMStream.hpp @@ -38,8 +38,8 @@ #include #include -#include -#include +#include +#include namespace uavcannode { @@ -75,34 +75,77 @@ public: printf("\t%s:%d -> %s\n", uavcan::equipment::gnss::RTCMStream::getDataTypeFullName(), uavcan::equipment::gnss::RTCMStream::DefaultDataTypeID, - _gps_inject_data_pub.get_topic()->o_name); + _sources[0].pub.get_topic()->o_name); } private: void callback(const uavcan::ReceivedDataStructure &msg) { + const uint8_t source_node_id = msg.getSrcNodeID().get(); + // Don't republish a message from ourselves - if (msg.getSrcNodeID().get() != getNode().getNodeID().get()) { - gps_inject_data_s gps_inject_data{}; - - gps_inject_data.len = msg.data.size(); - - memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len); - - gps_inject_data.timestamp = hrt_absolute_time(); - - union device::Device::DeviceId device_id; - - device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN; - device_id.devid_s.address = msg.getSrcNodeID().get(); - device_id.devid_s.devtype = DRV_GPS_DEVTYPE_UAVCAN; - - gps_inject_data.device_id = device_id.devid; - - _gps_inject_data_pub.publish(gps_inject_data); + if (source_node_id == getNode().getNodeID().get()) { + return; } + + // Route each source node to its own rtcm_corrections instance. Independent CAN + // sources (e.g. a fixed base plus another rover's MSM output) must not interleave + // on a single instance, or the consumer's per-instance stale-link selection breaks. + uORB::PublicationMulti *pub = publicationForNode(source_node_id); + + if (pub == nullptr) { + // More distinct sources than rtcm_corrections instances: drop. + return; + } + + rtcm_data_s corrections{}; + + corrections.len = msg.data.size(); + + memcpy(corrections.data, &msg.data[0], corrections.len); + + corrections.timestamp = hrt_absolute_time(); + + union device::Device::DeviceId device_id {}; + + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN; + device_id.devid_s.address = source_node_id; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_UAVCAN; + + corrections.device_id = device_id.devid; + + pub->publish(corrections); } - uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + // Map a CAN source node ID to a stable rtcm_corrections publication, one uORB + // instance per source node. Returns nullptr once every instance is claimed. + uORB::PublicationMulti *publicationForNode(uint8_t node_id) + { + for (auto &source : _sources) { + if (source.assigned && source.node_id == node_id) { + return &source.pub; + } + } + + for (auto &source : _sources) { + if (!source.assigned) { + source.assigned = true; + source.node_id = node_id; + return &source.pub; + } + } + + return nullptr; + } + + struct SourcePublication { + uORB::PublicationMulti pub{ORB_ID(rtcm_corrections)}; + uint8_t node_id{0}; + bool assigned{false}; + }; + + // One publication per source node, capped at the topic's instance count so we never + // advertise an instance the consumer's SubscriptionMultiArray cannot read. + SourcePublication _sources[rtcm_data_s::MAX_INSTANCES] {}; }; } // namespace uavcannode diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 8eb1f42eea4..2885978a426 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -85,7 +85,6 @@ #endif #include "streams/GPS_GLOBAL_ORIGIN.hpp" #include "streams/GPS_RAW_INT.hpp" -#include "streams/GPS_RTCM_DATA.hpp" #include "streams/GPS_STATUS.hpp" #include "streams/HEARTBEAT.hpp" #include "streams/HIGHRES_IMU.hpp" @@ -512,9 +511,6 @@ static const StreamListItem streams_list[] = { #if defined(EFI_STATUS_HPP) create_stream_list_item(), #endif // EFI_STATUS_HPP -#if defined(GPS_RTCM_DATA_HPP) - create_stream_list_item(), -#endif // GPS_RTCM_DATA_HPP #if defined(UAVIONIX_ADSB_OUT_CFG_HPP) create_stream_list_item(), #endif // UAVIONIX_ADSB_OUT_CFG_HPP diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7720719e864..2436e1e2d39 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -86,7 +86,7 @@ MavlinkReceiver::~MavlinkReceiver() #endif // !CONSTRAINED_FLASH _distance_sensor_pub.unadvertise(); - _gps_inject_data_pub.unadvertise(); + _rtcm_corrections_pub.unadvertise(); _rc_pub.unadvertise(); _manual_control_input_pub.unadvertise(); _ping_pub.unadvertise(); @@ -2909,38 +2909,38 @@ MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg) packet_len, now, message_len); if (message != nullptr) { - publish_gps_inject_data(message, message_len); + publish_rtcm_corrections(message, message_len); // addPacket() can queue at most one deferred message. const uint8_t *deferred_message = _gps_rtcm_message_assembler.takeDeferredMessage(message_len); if (deferred_message != nullptr) { - publish_gps_inject_data(deferred_message, message_len); + publish_rtcm_corrections(deferred_message, message_len); } } } void -MavlinkReceiver::publish_gps_inject_data(const uint8_t *data, size_t len) +MavlinkReceiver::publish_rtcm_corrections(const uint8_t *data, size_t len) { - gps_inject_data_s gps_inject_data_topic{}; - constexpr uint8_t gps_inject_data_flag_fragmented = 1; + rtcm_data_s rtcm_corrections_topic{}; + constexpr uint8_t rtcm_corrections_flag_fragmented = 1; - const size_t capacity = sizeof(gps_inject_data_topic.data); - // gps_inject_data only carries the transport-level fragmented bit. The + const size_t capacity = sizeof(rtcm_corrections_topic.data); + // rtcm_corrections only carries the transport-level fragmented bit. The // MAVLink fragment/sequence bits are consumed by the assembler above. - gps_inject_data_topic.flags = (len > capacity) ? gps_inject_data_flag_fragmented : 0; + rtcm_corrections_topic.flags = (len > capacity) ? rtcm_corrections_flag_fragmented : 0; size_t written = 0; - // gps_inject_data transports RTCM in 300-byte uORB chunks, so a fully + // rtcm_corrections transports RTCM in 300-byte uORB chunks, so a fully // reassembled RTCM frame may still require multiple publications. while (written < len) { const size_t chunk_len = math::min(len - written, capacity); - gps_inject_data_topic.timestamp = hrt_absolute_time(); - gps_inject_data_topic.len = static_cast(chunk_len); - memcpy(gps_inject_data_topic.data, &data[written], chunk_len); - _gps_inject_data_pub.publish(gps_inject_data_topic); + rtcm_corrections_topic.timestamp = hrt_absolute_time(); + rtcm_corrections_topic.len = static_cast(chunk_len); + memcpy(rtcm_corrections_topic.data, &data[written], chunk_len); + _rtcm_corrections_pub.publish(rtcm_corrections_topic); written += chunk_len; } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index d42d7cdca51..16756dc28a9 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -80,7 +80,7 @@ #include #include #include -#include +#include #include #include #include @@ -282,7 +282,7 @@ private: void update_rx_stats(const mavlink_message_t &message); void publish_hil_battery(); - void publish_gps_inject_data(const uint8_t *data, size_t len); + void publish_rtcm_corrections(const uint8_t *data, size_t len); px4::atomic_bool _should_exit{false}; pthread_t _thread {}; @@ -396,7 +396,7 @@ private: // ORB publications (multi) uORB::PublicationMulti _distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)}; - uORB::PublicationMulti _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + uORB::PublicationMulti _rtcm_corrections_pub{ORB_ID(rtcm_corrections)}; uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; uORB::PublicationMulti _manual_control_input_pub{ORB_ID(manual_control_input)}; uORB::PublicationMulti _ping_pub{ORB_ID(ping)}; diff --git a/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp b/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp deleted file mode 100644 index 8730d2e8810..00000000000 --- a/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp +++ /dev/null @@ -1,81 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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. - * - ****************************************************************************/ - -#ifndef GPS_RTCM_DATA_HPP -#define GPS_RTCM_DATA_HPP - -#include - -class MavlinkStreamGPSRTCMData : public MavlinkStream -{ -public: - static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSRTCMData(mavlink); } - - static constexpr const char *get_name_static() { return "GPS_RTCM_DATA"; } - static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GPS_RTCM_DATA; } - - const char *get_name() const override { return get_name_static(); } - uint16_t get_id() override { return get_id_static(); } - - unsigned get_size() override - { - return _gps_inject_data_sub.advertised() ? (MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; - } - -private: - explicit MavlinkStreamGPSRTCMData(Mavlink *mavlink) : MavlinkStream(mavlink) {} - - uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data), 0}; - - bool send() override - { - gps_inject_data_s gps_inject_data; - bool sent = false; - - while ((_mavlink->get_free_tx_buf() >= get_size()) && _gps_inject_data_sub.update(&gps_inject_data)) { - mavlink_gps_rtcm_data_t msg{}; - - msg.len = gps_inject_data.len; - msg.flags = gps_inject_data.flags; - memcpy(msg.data, gps_inject_data.data, sizeof(msg.data)); - - mavlink_msg_gps_rtcm_data_send_struct(_mavlink->get_channel(), &msg); - - sent = true; - } - - return sent; - } -}; - -#endif // GPS_RTCM_DATA_HPP