diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3ecb1ee597..28b9e32d01 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -116,7 +116,6 @@ set(msg_files GpioOut.msg GpioRequest.msg GpsDump.msg - GpsInjectData.msg Gripper.msg HealthReport.msg HeaterStatus.msg @@ -192,6 +191,8 @@ set(msg_files RoverSteeringSetpoint.msg RoverThrottleSetpoint.msg Rpm.msg + RtcmCorrections.msg + RtcmMovingBaseline.msg RtlStatus.msg RtlTimeEstimate.msg SatelliteInfo.msg diff --git a/msg/GpsInjectData.msg b/msg/GpsInjectData.msg deleted file mode 100644 index 516d5cb5d9..0000000000 --- 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 -uint8[300] data # data to write to GPS device (RTCM message) - -uint8 ORB_QUEUE_LENGTH = 8 - -uint8 MAX_INSTANCES = 2 diff --git a/msg/RtcmCorrections.msg b/msg/RtcmCorrections.msg new file mode 100644 index 0000000000..3885f1d86b --- /dev/null +++ b/msg/RtcmCorrections.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +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 correction data (from a fixed base, or another GPS) + +uint8 ORB_QUEUE_LENGTH = 8 + +uint8 MAX_INSTANCES = 4 diff --git a/msg/RtcmMovingBaseline.msg b/msg/RtcmMovingBaseline.msg new file mode 100644 index 0000000000..42500c3c6a --- /dev/null +++ b/msg/RtcmMovingBaseline.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 device_id # unique device ID of the moving-base GPS that produced this RTCM + +uint16 len # length of data +uint8 flags # LSB: 1=fragmented +uint8[300] data # RTCM 4072 (or equivalent) moving-baseline data for the rover GPS + +uint8 ORB_QUEUE_LENGTH = 16 + +uint8 MAX_INSTANCES = 2 diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp index bffc940e29..e15c64ae21 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" @@ -1621,27 +1621,22 @@ int SeptentrioDriver::set_baudrate(uint32_t baud) } } -void SeptentrioDriver::handle_inject_data_topic() +template +void SeptentrioDriver::drain_rtcm_subscriptions(uORB::SubscriptionMultiArray &subs, uint8_t &selected_instance) { - // 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; + T 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 < subs.size(); instance++) { + if (subs[instance].advertised() && subs[instance].copy(&msg)) { + if (msg.device_id != get_device_id() && now < msg.timestamp + 5_s) { + already_copied = true; + selected_instance = instance; + break; } } } @@ -1649,36 +1644,37 @@ void SeptentrioDriver::handle_inject_data_topic() 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; + const size_t max_num_injections = T::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. - */ + // Prevent injection of data from self + if (msg.device_id != get_device_id()) { write(msg.data, msg.len); - ++_current_interval_rtcm_injections; _last_rtcm_injection_time = hrt_absolute_time(); } } - updated = _gps_inject_data_sub[_selected_rtcm_instance].update(&msg); - + updated = subs[selected_instance].update(&msg); } while (updated && num_injections < max_num_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_subscriptions(_rtcm_corrections_sub, _selected_rtcm_instance); + + // Moving-baseline RTCM is only consumed by the rover (Main instance) in a moving-base setup. + if (_receiver_setup == ReceiverSetup::MovingBase && _instance == Instance::Main) { + drain_rtcm_subscriptions(_rtcm_moving_baseline_sub, _selected_moving_baseline_instance); + } +} + void SeptentrioDriver::publish() { _sensor_gps.device_id = get_device_id(); @@ -1702,35 +1698,25 @@ bool SeptentrioDriver::first_gps_uorb_message_created() const void SeptentrioDriver::publish_rtcm_corrections(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_moving_baseline_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) / sizeof(moving_baseline.data[0]); + 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 78761432a9..29100cf618 100644 --- a/src/drivers/gnss/septentrio/septentrio.h +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -55,7 +55,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -525,6 +526,12 @@ private: */ void handle_inject_data_topic(); + /** + * @brief Drain an RTCM source (rtcm_corrections or rtcm_moving_baseline) and write to the receiver. + */ + template + void drain_rtcm_subscriptions(uORB::SubscriptionMultiArray &subs, uint8_t &selected_instance); + /** * @brief Send data to the receiver, such as RTCM injections. * @@ -716,6 +723,7 @@ private: char _port[20] {}; ///< The path of the used serial device 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 + uint8_t _selected_moving_baseline_instance {0}; ///< uORB instance used for moving-baseline RTCM input 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 @@ -755,9 +763,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::SubscriptionMultiArray _rtcm_moving_baseline_sub {ORB_ID::rtcm_moving_baseline}; ///< uORB subscription for moving-baseline RTCM input // Data about update frequencies of various bits of information like RTCM message injection frequency, received data rate... hrt_abstime _current_interval_start_time {0}; ///< Start time of the current update measurement interval in us diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 5ed844c374..9bd25c78ea 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 5ed844c374646bfa2b41461c909a3b4df1a17233 +Subproject commit 9bd25c78ea7b36a740af3a90d0f8e174226b18aa diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5f8138180c..816546654e 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -64,7 +64,8 @@ #include #include #include -#include +#include +#include #include #include @@ -215,11 +216,14 @@ private: unsigned _rate_reading{0}; ///< reading rate in B/s 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 + uint8_t _selected_moving_baseline_instance{0}; ///< uorb instance used for moving-baseline RTCM input 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::SubscriptionMultiArray _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}; @@ -274,6 +278,13 @@ private: */ void handleInjectDataTopic(); + /** + * Drain an RTCM source (rtcm_corrections or rtcm_moving_baseline) into the RTCM parser. + * Selects an active instance if the current one goes stale. + */ + template + void drainRtcmSubscriptions(uORB::SubscriptionMultiArray &subs, uint8_t &selected_instance); + /** * send data to the device, such as an RTCM stream * @param data @@ -554,38 +565,26 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) return ret; } -void GPS::handleInjectDataTopic() +template +void GPS::drainRtcmSubscriptions(uORB::SubscriptionMultiArray &subs, uint8_t &selected_instance) { - 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. bool already_copied = false; - gps_inject_data_s msg; + T 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 < subs.size(); instance++) { + if (subs[instance].advertised() && subs[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_instance = instance; + break; } } } @@ -593,13 +592,8 @@ void GPS::handleInjectDataTopic() 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; + // Limit maximum number of injections per call so a burst can't starve the driver loop. + const size_t max_num_injections = T::ORB_QUEUE_LENGTH; size_t num_injections = 0; do { @@ -608,7 +602,6 @@ void GPS::handleInjectDataTopic() // 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) { @@ -619,19 +612,29 @@ void GPS::handleInjectDataTopic() } } - auto &gps_inject_data_sub = _orb_inject_data_sub[_selected_rtcm_instance]; + auto &sub = subs[selected_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) { - 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()); - } + 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 < max_num_injections); +} + +void GPS::handleInjectDataTopic() +{ + if (!_helper->shouldInjectRTCM()) { + return; + } + + // Fixed-base RTCM corrections (MAVLink GPS_RTCM_DATA, UAVCAN RTCMStream). + drainRtcmSubscriptions(_rtcm_corrections_sub, _selected_rtcm_instance); + + // Moving-baseline RTCM from a peer GPS (UBX RTCM 4072 etc). + drainRtcmSubscriptions(_rtcm_moving_baseline_sub, _selected_moving_baseline_instance); // Now inject all complete RTCM frames from the parser buffer size_t frame_len = {}; @@ -1382,35 +1385,45 @@ GPS::publishSatelliteInfo() void GPS::publishRTCMCorrections(uint8_t *data, size_t len) { - gps_inject_data_s gps_inject_data{}; + // 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. + const bool moving_base = _helper && _helper->isMovingBase(); - gps_inject_data.timestamp = hrt_absolute_time(); - gps_inject_data.device_id = get_device_id(); + rtcm_corrections_s corrections{}; + rtcm_moving_baseline_s moving_baseline{}; - size_t capacity = (sizeof(gps_inject_data.data) / sizeof(gps_inject_data.data[0])); + const hrt_abstime now = hrt_absolute_time(); + const uint32_t device_id = get_device_id(); - if (len > capacity) { - gps_inject_data.flags = 1; //LSB: 1=fragmented + corrections.timestamp = now; + corrections.device_id = device_id; + moving_baseline.timestamp = now; + moving_baseline.device_id = device_id; - } else { - gps_inject_data.flags = 0; - } + const size_t capacity = sizeof(corrections.data) / sizeof(corrections.data[0]); + const uint8_t flags = (len > capacity) ? 1 : 0; // LSB: 1=fragmented + corrections.flags = flags; + moving_baseline.flags = flags; size_t written = 0; while (written < len) { - gps_inject_data.len = len - written; + const size_t chunk = math::min(len - written, capacity); - if (gps_inject_data.len > capacity) { - gps_inject_data.len = capacity; + if (moving_base) { + moving_baseline.len = chunk; + memcpy(moving_baseline.data, &data[written], chunk); + _rtcm_moving_baseline_pub.publish(moving_baseline); + + } else { + corrections.len = chunk; + memcpy(corrections.data, &data[written], chunk); + _rtcm_corrections_pub.publish(corrections); } - 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; + written += chunk; } } diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 36b57786f6..7fdc5d849d 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -564,14 +564,65 @@ 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 a uORB RTCM source and forwards each message to `forward(data, len)`. +// Handles stale-link switchover across instances via `last_injection_time`. +template +static void drain_rtcm_to_can(uORB::SubscriptionMultiArray &subs, + uint8_t &selected_instance, + hrt_abstime &last_injection_time, + unsigned &rate_message_count, + Forward forward) +{ + const hrt_abstime now = hrt_absolute_time(); + + bool already_copied = false; + T msg; + + if (now > last_injection_time + 5_s) { + for (int instance = 0; instance < subs.size(); instance++) { + if (subs[instance].advertised() && subs[instance].copy(&msg)) { + if ((hrt_absolute_time() - msg.timestamp) < 5_s) { + already_copied = true; + selected_instance = instance; + break; + } + } + } + } + + bool updated = already_copied; + + const size_t max_num_injections = T::ORB_QUEUE_LENGTH; + size_t num_injections = 0; + + do { + if (updated) { + num_injections++; + forward(msg.data, msg.len); + ++rate_message_count; + last_injection_time = hrt_absolute_time(); + } + + auto &sub = subs[selected_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 < max_num_injections); +} + +// This drains rtcm_corrections (fixed-base RTCM from GCS/CAN) to uavcan::RTCMStream, +// and rtcm_moving_baseline (moving-base RTCM 4072 from a peer GPS) to +// ardupilot::MovingBaselineData. Each topic maps to its own CAN publisher — the +// two streams are kept separate so fixed-base corrections are not mirrored onto +// the moving-baseline CAN message. void UavcanGnssBridge::handleInjectDataTopic() { - hrt_abstime now = hrt_absolute_time(); + const hrt_abstime now = hrt_absolute_time(); // measure RTCM update rate every 5 seconds if (now > _last_rate_measurement + 5_s) { @@ -581,72 +632,17 @@ 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) { + drain_rtcm_to_can(_rtcm_corrections_sub, _selected_rtcm_instance, + _last_rtcm_injection_time, _rtcm_injection_rate_message_count, + [this](const uint8_t *data, size_t len) { PublishRTCMStream(data, len); }); } - 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) { + drain_rtcm_to_can(_rtcm_moving_baseline_sub, _selected_moving_baseline_instance, + _last_moving_baseline_injection_time, _rtcm_injection_rate_message_count, + [this](const uint8_t *data, size_t len) { PublishMovingBaselineData(data, len); }); + } } 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 72fe9b3453..bd436e09aa 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -48,7 +48,8 @@ #include #include #include -#include +#include +#include #include #include @@ -143,9 +144,12 @@ 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::SubscriptionMultiArray _rtcm_moving_baseline_sub{ORB_ID::rtcm_moving_baseline}; hrt_abstime _last_rtcm_injection_time{0}; ///< time of last rtcm injection + hrt_abstime _last_moving_baseline_injection_time{0}; ///< time of last moving-baseline injection uint8_t _selected_rtcm_instance{0}; ///< uorb instance that is being used for RTCM corrections + uint8_t _selected_moving_baseline_instance{0}; ///< uorb instance used for moving-baseline input uORB::Publication _gps_dump_pub{ORB_ID(gps_dump)}; // For PPK diff --git a/src/drivers/uavcannode/Publishers/MovingBaselineData.hpp b/src/drivers/uavcannode/Publishers/MovingBaselineData.hpp index ad90387b42..f9c775d6f2 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,46 +70,38 @@ public: void BroadcastAnyUpdates() override { - // gps_inject_data -> ardupilot::gnss::MovingBaselineData - gps_inject_data_s gps_inject_data = {}; + // rtcm_moving_baseline -> ardupilot::gnss::MovingBaselineData + rtcm_moving_baseline_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 - union device::Device::DeviceId device_id; - device_id.devid = gps_inject_data.device_id; - - if (device_id.devid_s.bus_type == device::Device::DeviceBusType::DeviceBusType_UAVCAN) { - continue; - } - ardupilot::gnss::MovingBaselineData mbd = {}; const size_t capacity = mbd.data.capacity(); 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 80a2af1c42..7e2eeeaf7d 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,13 +83,13 @@ private: { // Don't republish a message from ourselves if (msg.getSrcNodeID().get() != getNode().getNodeID().get()) { - gps_inject_data_s gps_inject_data{}; + rtcm_moving_baseline_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; @@ -97,12 +97,12 @@ private: 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 83e2c84737..ffa786735f 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,7 +75,7 @@ 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); + _rtcm_corrections_pub.get_topic()->o_name); } private: @@ -83,13 +83,13 @@ private: { // Don't republish a message from ourselves if (msg.getSrcNodeID().get() != getNode().getNodeID().get()) { - gps_inject_data_s gps_inject_data{}; + rtcm_corrections_s corrections{}; - gps_inject_data.len = msg.data.size(); + corrections.len = msg.data.size(); - memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len); + memcpy(corrections.data, &msg.data[0], corrections.len); - gps_inject_data.timestamp = hrt_absolute_time(); + corrections.timestamp = hrt_absolute_time(); union device::Device::DeviceId device_id; @@ -97,12 +97,12 @@ private: 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; + corrections.device_id = device_id.devid; - _gps_inject_data_pub.publish(gps_inject_data); + _rtcm_corrections_pub.publish(corrections); } } - uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; + uORB::PublicationMulti _rtcm_corrections_pub{ORB_ID(rtcm_corrections)}; }; } // namespace uavcannode diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 60507e533f..fb8bada4e4 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -81,7 +81,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(); @@ -2736,18 +2736,18 @@ MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg) mavlink_gps_rtcm_data_t gps_rtcm_data_msg; mavlink_msg_gps_rtcm_data_decode(msg, &gps_rtcm_data_msg); - gps_inject_data_s gps_inject_data_topic{}; + rtcm_corrections_s rtcm_corrections_topic{}; - gps_inject_data_topic.timestamp = hrt_absolute_time(); + rtcm_corrections_topic.timestamp = hrt_absolute_time(); - gps_inject_data_topic.len = math::min((int)sizeof(gps_rtcm_data_msg.data), - (int)sizeof(uint8_t) * gps_rtcm_data_msg.len); - gps_inject_data_topic.flags = gps_rtcm_data_msg.flags; - memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data, - math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_topic.len)); + rtcm_corrections_topic.len = math::min((int)sizeof(gps_rtcm_data_msg.data), + (int)sizeof(uint8_t) * gps_rtcm_data_msg.len); + rtcm_corrections_topic.flags = gps_rtcm_data_msg.flags; + memcpy(rtcm_corrections_topic.data, gps_rtcm_data_msg.data, + math::min((int)sizeof(rtcm_corrections_topic.data), (int)sizeof(uint8_t) * rtcm_corrections_topic.len)); - gps_inject_data_topic.timestamp = hrt_absolute_time(); - _gps_inject_data_pub.publish(gps_inject_data_topic); + rtcm_corrections_topic.timestamp = hrt_absolute_time(); + _rtcm_corrections_pub.publish(rtcm_corrections_topic); } void diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index e2adf224db..1e88f3b1b2 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -79,7 +79,7 @@ #include #include #include -#include +#include #include #include #include @@ -360,7 +360,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 index 8730d2e881..7cf556e7fa 100644 --- a/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp +++ b/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp @@ -34,7 +34,7 @@ #ifndef GPS_RTCM_DATA_HPP #define GPS_RTCM_DATA_HPP -#include +#include class MavlinkStreamGPSRTCMData : public MavlinkStream { @@ -49,25 +49,25 @@ public: unsigned get_size() override { - return _gps_inject_data_sub.advertised() ? (MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + return _rtcm_corrections_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}; + uORB::Subscription _rtcm_corrections_sub{ORB_ID(rtcm_corrections), 0}; bool send() override { - gps_inject_data_s gps_inject_data; + rtcm_corrections_s corrections; bool sent = false; - while ((_mavlink->get_free_tx_buf() >= get_size()) && _gps_inject_data_sub.update(&gps_inject_data)) { + while ((_mavlink->get_free_tx_buf() >= get_size()) && _rtcm_corrections_sub.update(&corrections)) { 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)); + msg.len = corrections.len; + msg.flags = corrections.flags; + memcpy(msg.data, corrections.data, sizeof(msg.data)); mavlink_msg_gps_rtcm_data_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index 1b59139374..a0b2f800f3 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -337,10 +337,6 @@ menu "Zenoh publishers/subscribers" bool "gps_dump" default n - config ZENOH_PUBSUB_GPS_INJECT_DATA - bool "gps_inject_data" - default n - config ZENOH_PUBSUB_GRIPPER bool "gripper" default n @@ -665,6 +661,14 @@ menu "Zenoh publishers/subscribers" bool "rpm" default n + config ZENOH_PUBSUB_RTCM_CORRECTIONS + bool "rtcm_corrections" + default n + + config ZENOH_PUBSUB_RTCM_MOVING_BASELINE + bool "rtcm_moving_baseline" + default n + config ZENOH_PUBSUB_RTL_STATUS bool "rtl_status" default n @@ -1034,7 +1038,6 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_GPIO_OUT select ZENOH_PUBSUB_GPIO_REQUEST select ZENOH_PUBSUB_GPS_DUMP - select ZENOH_PUBSUB_GPS_INJECT_DATA select ZENOH_PUBSUB_GRIPPER select ZENOH_PUBSUB_HEALTH_REPORT select ZENOH_PUBSUB_HEATER_STATUS @@ -1116,6 +1119,8 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_ROVER_STEERING_SETPOINT select ZENOH_PUBSUB_ROVER_THROTTLE_SETPOINT select ZENOH_PUBSUB_RPM + select ZENOH_PUBSUB_RTCM_CORRECTIONS + select ZENOH_PUBSUB_RTCM_MOVING_BASELINE select ZENOH_PUBSUB_RTL_STATUS select ZENOH_PUBSUB_RTL_TIME_ESTIMATE select ZENOH_PUBSUB_SATELLITE_INFO