AP_UAVCAN: support RTCMStream for RTCM injection

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -39,6 +39,7 @@
 #include <ardupilot/indication/SafetyState.hpp>
 #include <ardupilot/indication/Button.hpp>
 #include <ardupilot/equipment/trafficmonitor/TrafficReport.hpp>
+#include <uavcan/equipment/gnss/RTCMStream.hpp>
 
 #include <AP_Baro/AP_Baro_UAVCAN.h>
 #include <AP_RangeFinder/AP_RangeFinder_UAVCAN.h>
@@ -106,6 +107,7 @@ static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[MAX_NUMBER
 static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
 static uavcan::Publisher<uavcan::equipment::indication::BeepCommand>* buzzer[MAX_NUMBER_OF_CAN_DRIVERS];
 static uavcan::Publisher<ardupilot::indication::SafetyState>* safety_state[MAX_NUMBER_OF_CAN_DRIVERS];
+static uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>* rtcm_stream[MAX_NUMBER_OF_CAN_DRIVERS];
 
 // subscribers
 
@@ -257,6 +259,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     safety_state[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
     safety_state[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
 
+    rtcm_stream[driver_index] = new uavcan::Publisher<uavcan::equipment::gnss::RTCMStream>(*_node);
+    rtcm_stream[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
+    rtcm_stream[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+    
     safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
     if (safety_button_listener[driver_index]) {
         safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button));
@@ -337,6 +343,7 @@ void AP_UAVCAN::loop(void)
 
         led_out_send();
         buzzer_send();
+        rtcm_stream_send();
         safety_state_send();
         AP::uavcan_server().verify_nodes(this);
     }
@@ -546,6 +553,36 @@ void AP_UAVCAN::set_buzzer_tone(float frequency, float duration_s)
     _buzzer.pending_mask = 0xFF;
 }
 
+void AP_UAVCAN::rtcm_stream_send()
+{
+    WITH_SEMAPHORE(_rtcm_stream.sem);
+    if (_rtcm_stream.buf == nullptr ||
+        _rtcm_stream.buf->available() == 0) {
+        // nothing to send
+        return;
+    }
+    uint32_t now = AP_HAL::millis();
+    if (now - _rtcm_stream.last_send_ms < 20) {
+        // don't send more than 50 per second
+        return;
+    }
+    _rtcm_stream.last_send_ms = now;
+    uavcan::equipment::gnss::RTCMStream msg;
+    uint32_t len = _rtcm_stream.buf->available();
+    if (len > 128) {
+        len = 128;
+    }
+    msg.protocol_id = uavcan::equipment::gnss::RTCMStream::PROTOCOL_ID_RTCM3;
+    for (uint8_t i=0; i<len; i++) {
+        uint8_t b;
+        if (!_rtcm_stream.buf->read_byte(&b)) {
+            return;
+        }
+        msg.data.push_back(b);
+    }
+    rtcm_stream[_driver_index]->broadcast(msg);
+}
+
 // SafetyState send
 void AP_UAVCAN::safety_state_send()
 {
@@ -570,6 +607,23 @@ void AP_UAVCAN::safety_state_send()
     safety_state[_driver_index]->broadcast(msg);
 }
 
+/*
+ send RTCMStream packet on all active UAVCAN drivers
+*/
+void AP_UAVCAN::send_RTCMStream(const uint8_t *data, uint32_t len)
+{
+    WITH_SEMAPHORE(_rtcm_stream.sem);
+    if (_rtcm_stream.buf == nullptr) {
+        // give enough space for a full round from a NTRIP server with all
+        // constellations
+        _rtcm_stream.buf = new ByteBuffer(2400);
+    }
+    if (_rtcm_stream.buf == nullptr) {
+        return;
+    }
+    _rtcm_stream.buf->write(data, len);
+}
+
 /*
   handle Button message
  */

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -88,6 +88,9 @@ public:
     // buzzer
     void set_buzzer_tone(float frequency, float duration_s);
 
+    // send RTCMStream packets
+    void send_RTCMStream(const uint8_t *data, uint32_t len);
+
     template <typename DataType_>
     class RegistryBinder {
     protected:
@@ -158,7 +161,10 @@ private:
 
     // SafetyState
     void safety_state_send();
-    
+
+    // send GNSS injection
+    void rtcm_stream_send();
+
     uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
 
     // UAVCAN parameters
@@ -207,6 +213,13 @@ private:
         uint8_t pending_mask; // mask of interfaces to send to
     } _buzzer;
 
+    // GNSS RTCM injection
+    struct {
+        HAL_Semaphore sem;
+        uint32_t last_send_ms;
+        ByteBuffer *buf;
+    } _rtcm_stream;
+    
     // safety status send state
     uint32_t _last_safety_state_ms;