drivers/uavcannode: Add RTCMStream subscriber and fix RTCMStream Publisher (#20056)

* Add cannode RTCMStream subscriber
* Fix uavcan RTCMStream publisher
* Break out CANNODE_SUB_RTCM and CANNODE_SUB_MBD

boards/ark/can-rtk-gps/init/rc.board_defaults

@@ -4,7 +4,8 @@
 #------------------------------------------------------------------------------
 
 param set-default CBRK_IO_SAFETY 0
-param set-default CANNODE_GPS_RTCM 1
+param set-default CANNODE_SUB_MBD 1
+param set-default CANNODE_SUB_RTCM 1
 param set-default MBE_ENABLE 1
 
 safety_button start

src/drivers/uavcan/sensors/gnss.cpp

@@ -494,10 +494,9 @@ void UavcanGnssBridge::handleInjectDataTopic()
 
 bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len)
 {
-	using uavcan::equipment::gnss::RTCMStream;
+	uavcan::equipment::gnss::RTCMStream msg;
 
-	RTCMStream msg;
-	msg.protocol_id = RTCMStream::PROTOCOL_ID_RTCM3;
+	msg.protocol_id = uavcan::equipment::gnss::RTCMStream::PROTOCOL_ID_RTCM3;
 
 	const size_t capacity = msg.data.capacity();
 	size_t written = 0;
@@ -517,7 +516,7 @@ bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t
 
 		result = _pub_rtcm_stream.broadcast(msg) >= 0;
 		perf_count(_rtcm_stream_pub_perf);
-		msg.data = {};
+		msg.data.clear();
 	}
 
 	return result;

src/drivers/uavcannode/Subscribers/RTCMStream.hpp

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "UavcanSubscriberBase.hpp"
+
+#include <uavcan/equipment/gnss/RTCMStream.hpp>
+
+#include <lib/drivers/device/Device.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/gps_inject_data.h>
+
+namespace uavcannode
+{
+
+class RTCMStream;
+
+typedef uavcan::MethodBinder<RTCMStream *,
+	void (RTCMStream::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::RTCMStream>&)>
+	RTCMStreamBinder;
+
+class RTCMStream :
+	public UavcanSubscriberBase,
+	private uavcan::Subscriber<uavcan::equipment::gnss::RTCMStream, RTCMStreamBinder>
+{
+public:
+	RTCMStream(uavcan::INode &node) :
+		UavcanSubscriberBase(uavcan::equipment::gnss::RTCMStream::DefaultDataTypeID),
+		uavcan::Subscriber<uavcan::equipment::gnss::RTCMStream, RTCMStreamBinder>(node)
+	{}
+
+	bool init()
+	{
+		if (start(RTCMStreamBinder(this, &RTCMStream::callback)) < 0) {
+			PX4_ERR("uavcan::equipment::gnss::RTCMStream subscription failed");
+			return false;
+		}
+
+		return true;
+	}
+
+	void PrintInfo() const override
+	{
+		printf("\t%s:%d -> %s\n",
+		       uavcan::equipment::gnss::RTCMStream::getDataTypeFullName(),
+		       uavcan::equipment::gnss::RTCMStream::DefaultDataTypeID,
+		       _gps_inject_data_pub.get_topic()->o_name);
+	}
+
+private:
+	void callback(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::RTCMStream> &msg)
+	{
+		// Don't republish a message from ourselves
+		if (msg.getSrcNodeID().get() != getNode().getNodeID().get()) {
+			gps_inject_data_s gps_inject_data{};
+
+			gps_inject_data.len = msg.data.size();
+
+			memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len);
+
+			gps_inject_data.timestamp = hrt_absolute_time();
+
+			union device::Device::DeviceId device_id;
+
+			device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN;
+			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;
+
+			_gps_inject_data_pub.publish(gps_inject_data);
+		}
+	}
+
+	uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
+};
+} // namespace uavcannode

src/drivers/uavcannode/UavcanNode.cpp

@@ -55,6 +55,7 @@
 #include "Subscribers/BeepCommand.hpp"
 #include "Subscribers/LightsCommand.hpp"
 #include "Subscribers/MovingBaselineData.hpp"
+#include "Subscribers/RTCMStream.hpp"
 
 using namespace time_literals;
 
@@ -301,12 +302,11 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 	_publisher_list.add(new RawAirData(this, _node));
 	_publisher_list.add(new RelPosHeadingPub(this, _node));
 
-	int32_t enable_movingbaselinedata = 0;
-	param_get(param_find("CANNODE_GPS_RTCM"), &enable_movingbaselinedata);
+	int32_t cannode_pub_mbd = 0;
+	param_get(param_find("CANNODE_PUB_MBD"), &cannode_pub_mbd);
 
-	if (enable_movingbaselinedata != 0) {
+	if (cannode_pub_mbd == 1) {
 		_publisher_list.add(new MovingBaselineDataPub(this, _node));
-
 	}
 
 	_publisher_list.add(new SafetyButton(this, _node));
@@ -316,10 +316,20 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 	_subscriber_list.add(new BeepCommand(_node));
 	_subscriber_list.add(new LightsCommand(_node));
 
-	if (enable_movingbaselinedata != 0) {
+	int32_t cannode_sub_mdb = 0;
+	param_get(param_find("CANNODE_SUB_MDB"), &cannode_sub_mdb);
+
+	if (cannode_sub_mdb == 1) {
 		_subscriber_list.add(new MovingBaselineData(_node));
 	}
 
+	int32_t cannode_sub_rtcm = 0;
+	param_get(param_find("CANNODE_SUB_RTCM"), &cannode_sub_rtcm);
+
+	if (cannode_sub_rtcm == 1) {
+		_subscriber_list.add(new RTCMStream(_node));
+	}
+
 	for (auto &subscriber : _subscriber_list) {
 		subscriber->init();
 	}

src/drivers/uavcannode/uavcannode_params.c

@@ -61,10 +61,29 @@ PARAM_DEFINE_INT32(CANNODE_BITRATE, 1000000);
 PARAM_DEFINE_INT32(CANNODE_TERM, 0);
 
 /**
- * Enable RTCM pub/sub
+ * Enable MovingBaselineData subscription
  *
  * @boolean
  * @max 1
+ * @reboot_required true
  * @group UAVCAN
  */
-PARAM_DEFINE_INT32(CANNODE_GPS_RTCM, 0);
+PARAM_DEFINE_INT32(CANNODE_SUB_MBD, 0);
+
+/**
+ * Enable RTCM subscription
+ *
+ * @boolean
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(CANNODE_SUB_RTCM, 0);
+
+/**
+ * Enable MovingBaselineData publication
+ *
+ * @boolean
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(CANNODE_PUB_MBD, 0);