From 4b36cfccfc6769df1e775ed52969ae4aa43afcc3 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Wed, 17 Dec 2025 10:17:56 -0900 Subject: [PATCH] uavcan: gnss: MovingBaselineData subscriber (#26094) * msg: GpsDump: increase queue from 8 to 16 and replace instance with device_id * uavcan: gnss: MovingBaselineData subscriber for RTCM logging for PPK. Add uORB generation tracking to detect data loss on RTCM injection. * add instance TODO --- src/drivers/uavcan/sensors/gnss.cpp | 60 ++++++++++++++++++++++++++++- src/drivers/uavcan/sensors/gnss.hpp | 13 +++++++ src/drivers/uavcan/uavcan_main.hpp | 2 +- src/drivers/uavcan/uavcan_params.c | 12 ++++++ 4 files changed, 85 insertions(+), 2 deletions(-) diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index a3fb2715c3..3ca5d0e863 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -60,6 +60,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node, NodeInfoPublisher *node_ _sub_fix(node), _sub_fix2(node), _sub_gnss_heading(node), + _sub_moving_baseline_data(node), _pub_moving_baseline_data(node), _pub_rtcm_stream(node), _channel_using_fix2(new bool[_max_channels]) @@ -76,6 +77,7 @@ UavcanGnssBridge::~UavcanGnssBridge() delete [] _channel_using_fix2; perf_free(_rtcm_stream_pub_perf); perf_free(_moving_baseline_data_pub_perf); + perf_free(_moving_baseline_data_sub_perf); } int @@ -129,6 +131,15 @@ UavcanGnssBridge::init() _moving_baseline_data_pub_perf = perf_alloc(PC_INTERVAL, "uavcan: gnss: moving baseline data rtcm stream pub"); } + // UAVCAN_SUB_MBD + int32_t uavcan_sub_mbd = 0; + param_get(param_find("UAVCAN_SUB_MBD"), &uavcan_sub_mbd); + + if (uavcan_sub_mbd == 1) { + res = _sub_moving_baseline_data.start(MovingBaselineDataCbBinder(this, &UavcanGnssBridge::moving_baseline_data_sub_cb)); + _moving_baseline_data_sub_perf = perf_alloc(PC_INTERVAL, "uavcan: gnss: moving baseline data rtcm stream sub"); + } + return res; } @@ -338,6 +349,7 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg) { @@ -346,6 +358,41 @@ void UavcanGnssBridge::gnss_relative_sub_cb(const _rel_heading_accuracy = math::radians(msg.reported_heading_acc_deg); } + +void UavcanGnssBridge::moving_baseline_data_sub_cb(const + uavcan::ReceivedDataStructure &msg) +{ + perf_count(_moving_baseline_data_sub_perf); + + size_t total_bytes = msg.data.size(); + size_t written = 0; + + while (written < total_bytes) { + gps_dump_s dump = {}; + dump.timestamp = hrt_absolute_time(); + + DeviceId device_id = {}; + device_id.devid_s.bus_type = DeviceBusType::DeviceBusType_UAVCAN; + device_id.devid_s.bus = 0; + device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_UAVCAN; + + dump.instance = 0; // TODO: How can we determine the instance? Startup order of CANnodes is non-deterministic. + dump.device_id = device_id.devid; + + size_t remaining = total_bytes - written; + size_t write_len = remaining < sizeof(dump.data) ? remaining : sizeof(dump.data); + + memcpy(dump.data, msg.data.begin() + written, write_len); + dump.len = write_len; + dump.len &= 0x7F; // MSB of len is direction - 0 is from the device + + written += write_len; + + _gps_dump_pub.publish(dump); + } +} + template void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure &msg, uint8_t fix_type, @@ -587,7 +634,17 @@ void UavcanGnssBridge::handleInjectDataTopic() _last_rtcm_injection_time = hrt_absolute_time(); } - updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg); + 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); } @@ -655,4 +712,5 @@ void UavcanGnssBridge::print_status() const UavcanSensorBridgeBase::print_status(); perf_print_counter(_rtcm_stream_pub_perf); perf_print_counter(_moving_baseline_data_pub_perf); + perf_print_counter(_moving_baseline_data_sub_perf); } diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index dd712af3f2..72fe9b3453 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include @@ -84,6 +85,8 @@ private: void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure &msg); void gnss_relative_sub_cb(const uavcan::ReceivedDataStructure &msg); + void moving_baseline_data_sub_cb(const uavcan::ReceivedDataStructure &msg); + template void process_fixx(const uavcan::ReceivedDataStructure &msg, @@ -119,6 +122,10 @@ private: void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > RelPosHeadingCbBinder; + typedef uavcan::MethodBinder < UavcanGnssBridge *, + void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > + MovingBaselineDataCbBinder; + uavcan::INode &_node; uavcan::Subscriber _sub_auxiliary; @@ -126,6 +133,9 @@ private: uavcan::Subscriber _sub_fix2; uavcan::Subscriber _sub_gnss_heading; + // Used for MSM7 logging for PPK workflows + uavcan::Subscriber _sub_moving_baseline_data; + uavcan::Publisher _pub_moving_baseline_data; uavcan::Publisher _pub_rtcm_stream; @@ -137,6 +147,8 @@ private: 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 + uORB::Publication _gps_dump_pub{ORB_ID(gps_dump)}; // For PPK + bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data? bool *_channel_using_fix2; ///< Flag for whether each channel is using Fix2 or Fix msg @@ -150,6 +162,7 @@ private: perf_counter_t _rtcm_stream_pub_perf{nullptr}; perf_counter_t _moving_baseline_data_pub_perf{nullptr}; + 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 diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index a9385afc9c..ab49dfe50f 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -200,7 +200,7 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams * 1000000/200 */ - static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * ScheduleIntervalMs; // At + static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * ScheduleIntervalMs; public: typedef UAVCAN_DRIVER::CanInitHelper CanInitHelper; diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index 7347c9ca3f..99a38897e4 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/src/drivers/uavcan/uavcan_params.c @@ -436,3 +436,15 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_RNG, 0); * @group UAVCAN */ PARAM_DEFINE_INT32(UAVCAN_SUB_BTN, 0); + +/** + * subscription MovingBaselineData + * + * Enable UAVCAN MovingBaselineData subscription. + * ardupilot::gnss::MovingBaselineData + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_SUB_MBD, 0);