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