diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index 83ca20ec1f..e6def29678 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -766,7 +766,38 @@ void AP_GPS_DroneCAN::inject_data(const uint8_t *data, uint16_t len) // don't want to send duplicates if (_detected_module == 0 || _detected_modules[_detected_module].ap_dronecan != _detected_modules[0].ap_dronecan) { - _detected_modules[_detected_module].ap_dronecan->send_RTCMStream(data, len); + 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); + + const uint32_t now = AP_HAL::native_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 outlen = _rtcm_stream.buf->available(); + if (outlen > 128) { + outlen = 128; + } + msg.protocol_id = UAVCAN_EQUIPMENT_GNSS_RTCMSTREAM_PROTOCOL_ID_RTCM3; + for (uint8_t i=0; iread_byte(&b)) { + return; + } + msg.data.data[i] = b; + } + msg.data.len = outlen; + _detected_modules[_detected_module].ap_dronecan->rtcm_stream.broadcast(msg); + } } diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.h b/libraries/AP_GPS/AP_GPS_DroneCAN.h index 9e2968ecd2..fb1e20ab8e 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.h +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.h @@ -138,5 +138,11 @@ private: bool handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, int32_t &value); bool handle_param_get_set_response_float(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, float &value); void handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success); + + // GNSS RTCM injection + struct { + uint32_t last_send_ms; + ByteBuffer *buf; + } _rtcm_stream; }; #endif