From c2af7ba9613b464c08d74dff6df957cf03e2edb8 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Tue, 13 Oct 2020 01:28:25 -0600 Subject: [PATCH] uavcan: gnss: Implement RTCM data for RTK (#15921) Forwards messages on the gps_inject_data uORB topic as uavcan::equipment::gnss::RTCMStream messages on the UAVCAN bus. This enables differential/RTK GPS over UAVCAN to work. Tested with CubePilot Cube Orange, Here+ base and Here3 rover. Signed-off-by: Alex Mikhalev --- src/drivers/uavcan/sensors/gnss.cpp | 117 ++++++++++++++++++- src/drivers/uavcan/sensors/gnss.hpp | 14 +++ src/drivers/uavcan/sensors/sensor_bridge.hpp | 2 + src/drivers/uavcan/uavcan_main.cpp | 4 + 4 files changed, 133 insertions(+), 4 deletions(-) diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 7380cb8948..4d51d0b2bd 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -57,8 +57,10 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _sub_auxiliary(node), _sub_fix(node), _sub_fix2(node), + _pub_rtcm(node), _report_pub(nullptr), - _channel_using_fix2(new bool[_max_channels]) + _channel_using_fix2(new bool[_max_channels]), + _rtcm_perf(perf_alloc(PC_INTERVAL, "uavcan: gnss: rtcm pub")) { for (uint8_t i = 0; i < _max_channels; i++) { _channel_using_fix2[i] = false; @@ -68,6 +70,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : UavcanGnssBridge::~UavcanGnssBridge() { delete [] _channel_using_fix2; + perf_free(_rtcm_perf); } int @@ -100,6 +103,8 @@ UavcanGnssBridge::init() return res; } + _pub_rtcm.setPriority(uavcan::TransferPriority::OneHigherThanLowest); + return res; } @@ -123,6 +128,8 @@ UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { + using uavcan::equipment::gnss::Fix2; + const int8_t ch = get_channel_index_for_node(msg.getSrcNodeID().get()); if (ch > -1 && !_channel_using_fix2[ch]) { @@ -145,6 +154,27 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure &msg, + uint8_t fix_type, const float (&pos_cov)[9], const float (&vel_cov)[9], const bool valid_pos_cov, const bool valid_vel_cov) { @@ -325,7 +356,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.c_variance_rad = -1.0F; } - report.fix_type = msg.status; + report.fix_type = fix_type; report.vel_n_m_s = msg.ned_velocity[0]; report.vel_e_m_s = msg.ned_velocity[1]; @@ -396,3 +427,81 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure publish(msg.getSrcNodeID().get(), &report); } + +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. +void UavcanGnssBridge::handleInjectDataTopic() +{ + bool updated = false; + + // Limit maximum number of GPS injections to 6 since usually + // GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo). + // Looking at 6 packets thus guarantees, that at least a full injection + // data set is evaluated. + const size_t max_num_injections = 6; + size_t num_injections = 0; + + do { + num_injections++; + updated = _orb_inject_data_sub.updated(); + + if (updated) { + gps_inject_data_s msg; + + if (_orb_inject_data_sub.copy(&msg)) { + + /* 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. + */ + injectData(msg.data, msg.len); + } + } + } while (updated && num_injections < max_num_injections); +} + +bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_len) +{ + using uavcan::equipment::gnss::RTCMStream; + + perf_count(_rtcm_perf); + + RTCMStream msg; + msg.protocol_id = RTCMStream::PROTOCOL_ID_RTCM3; + + const size_t capacity = msg.data.capacity(); + size_t written = 0; + bool result = true; + + while (result && written < data_len) { + size_t chunk_size = data_len - written; + + if (chunk_size > capacity) { + chunk_size = capacity; + } + + for (size_t i = 0; i < chunk_size; ++i) { + msg.data.push_back(data[written]); + written += 1; + } + + result = _pub_rtcm.broadcast(msg) >= 0; + msg.data = {}; + } + + return result; +} + +void UavcanGnssBridge::print_status() const +{ + UavcanCDevSensorBridgeBase::print_status(); + perf_print_counter(_rtcm_perf); +} diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index a159f09260..de4e11fad2 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -47,11 +47,15 @@ #include #include #include +#include #include #include #include #include +#include + +#include #include "sensor_bridge.hpp" @@ -68,6 +72,8 @@ public: const char *get_name() const override { return NAME; } int init() override; + void update() override; + void print_status() const override; private: /** @@ -79,9 +85,13 @@ private: template void process_fixx(const uavcan::ReceivedDataStructure &msg, + uint8_t fix_type, const float (&pos_cov)[9], const float (&vel_cov)[9], const bool valid_pos_cov, const bool valid_vel_cov); + void handleInjectDataTopic(); + bool injectData(const uint8_t *data, size_t data_len); + typedef uavcan::MethodBinder < UavcanGnssBridge *, void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > AuxiliaryCbBinder; @@ -103,11 +113,13 @@ private: uavcan::Subscriber _sub_auxiliary; uavcan::Subscriber _sub_fix; uavcan::Subscriber _sub_fix2; + uavcan::Publisher _pub_rtcm; uint64_t _last_gnss_auxiliary_timestamp{0}; float _last_gnss_auxiliary_hdop{0.0f}; float _last_gnss_auxiliary_vdop{0.0f}; + uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; uORB::PublicationMulti _gps_pub{ORB_ID(sensor_gps)}; int _receiver_node_id{-1}; @@ -118,4 +130,6 @@ private: 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 + + perf_counter_t _rtcm_perf; }; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.hpp b/src/drivers/uavcan/sensors/sensor_bridge.hpp index fe51a02238..2e806564b4 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.hpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.hpp @@ -75,6 +75,8 @@ public: */ virtual void print_status() const = 0; + virtual void update() {}; + /** * Sensor bridge factory. * Creates all known sensor bridges and puts them in the linked list. diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 97fff6e873..374c9e1d62 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -749,6 +749,10 @@ UavcanNode::Run() perf_begin(_cycle_perf); perf_count(_interval_perf); + for (auto &br : _sensor_bridges) { + br->update(); + } + node_spin_once(); // expected to be non-blocking // Check arming state