uavcan: GNSS optionally publish RTCMStream or MovingBaselineData

src/drivers/uavcan/sensors/gnss.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-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
@@ -46,6 +46,7 @@
 #include <drivers/drv_hrt.h>
 #include <systemlib/err.h>
 #include <mathlib/mathlib.h>
+#include <lib/parameters/param.h>
 
 using namespace time_literals;
 
@@ -57,9 +58,9 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
 	_sub_auxiliary(node),
 	_sub_fix(node),
 	_sub_fix2(node),
-	_pub_rtcm(node),
+	_pub_moving_baseline_data(node),
-	_channel_using_fix2(new bool[_max_channels]),
+	_pub_rtcm_stream(node),
-	_rtcm_perf(perf_alloc(PC_INTERVAL, "uavcan: gnss: rtcm pub"))
+	_channel_using_fix2(new bool[_max_channels])
 {
 	for (uint8_t i = 0; i < _max_channels; i++) {
 		_channel_using_fix2[i] = false;
@@ -71,7 +72,8 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
 UavcanGnssBridge::~UavcanGnssBridge()
 {
 	delete [] _channel_using_fix2;
-	perf_free(_rtcm_perf);
+	perf_free(_rtcm_stream_pub_perf);
+	perf_free(_moving_baseline_data_pub_perf);
 }
 
 int
@@ -98,7 +100,26 @@ UavcanGnssBridge::init()
 		return res;
 	}
 
-	_pub_rtcm.setPriority(uavcan::TransferPriority::NumericallyMax);
+
+	// UAVCAN_PUB_RTCM
+	int32_t uavcan_pub_rtcm = 0;
+	param_get(param_find("UAVCAN_PUB_RTCM"), &uavcan_pub_rtcm);
+
+	if (uavcan_pub_rtcm == 1) {
+		_publish_rtcm_stream = true;
+		_pub_rtcm_stream.setPriority(uavcan::TransferPriority::NumericallyMax);
+		_rtcm_stream_pub_perf = perf_alloc(PC_INTERVAL, "uavcan: gnss: rtcm stream pub");
+	}
+
+	// UAVCAN_PUB_MBD
+	int32_t uavcan_pub_mbd = 0;
+	param_get(param_find("UAVCAN_PUB_MBD"), &uavcan_pub_mbd);
+
+	if (uavcan_pub_mbd == 1) {
+		_publish_moving_baseline_data = true;
+		_pub_moving_baseline_data.setPriority(uavcan::TransferPriority::NumericallyMax);
+		_moving_baseline_data_pub_perf = perf_alloc(PC_INTERVAL, "uavcan: gnss: moving baseline data rtcm stream pub");
+	}
 
 	return res;
 }
@@ -446,41 +467,37 @@ void UavcanGnssBridge::update()
 // 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;
+	static constexpr size_t MAX_NUM_INJECTIONS = 6;
+
 	size_t num_injections = 0;
+	gps_inject_data_s gps_inject_data;
 
-	do {
+	while ((num_injections <= MAX_NUM_INJECTIONS) && _gps_inject_data_sub.update(&gps_inject_data)) {
-		num_injections++;
+		// Write the message to the gps device. Note that the message could be fragmented.
-		updated = _orb_inject_data_sub.updated();
+		// But as we don't write anywhere else to the device during operation, we don't
-
+		// need to assemble the message first.
-		if (updated) {
+		if (_publish_rtcm_stream) {
-			gps_inject_data_s msg;
+			PublishRTCMStream(gps_inject_data.data, gps_inject_data.len);
-
-			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);
+
+		if (_publish_moving_baseline_data) {
+			PublishMovingBaselineData(gps_inject_data.data, gps_inject_data.len);
+		}
+
+		num_injections++;
+	}
 }
 
-bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_len)
+bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len)
 {
-	using ardupilot::gnss::MovingBaselineData;
+	using uavcan::equipment::gnss::RTCMStream;
 
-	perf_count(_rtcm_perf);
+	RTCMStream msg;
-
+	msg.protocol_id = RTCMStream::PROTOCOL_ID_RTCM3;
-	MovingBaselineData msg;
 
 	const size_t capacity = msg.data.capacity();
 	size_t written = 0;
@@ -498,7 +515,36 @@ bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_l
 			written += 1;
 		}
 
-		result = _pub_rtcm.broadcast(msg) >= 0;
+		result = _pub_rtcm_stream.broadcast(msg) >= 0;
+		perf_count(_rtcm_stream_pub_perf);
+		msg.data = {};
+	}
+
+	return result;
+}
+
+bool UavcanGnssBridge::PublishMovingBaselineData(const uint8_t *data, size_t data_len)
+{
+	ardupilot::gnss::MovingBaselineData msg;
+
+	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_moving_baseline_data.broadcast(msg) >= 0;
+		perf_count(_moving_baseline_data_pub_perf);
 		msg.data.clear();
 	}
 
@@ -508,5 +554,6 @@ bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_l
 void UavcanGnssBridge::print_status() const
 {
 	UavcanSensorBridgeBase::print_status();
-	perf_print_counter(_rtcm_perf);
+	perf_print_counter(_rtcm_stream_pub_perf);
+	perf_print_counter(_moving_baseline_data_pub_perf);
 }

src/drivers/uavcan/sensors/gnss.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-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
@@ -54,6 +54,7 @@
 #include <uavcan/equipment/gnss/Fix.hpp>
 #include <uavcan/equipment/gnss/Fix2.hpp>
 #include <ardupilot/gnss/MovingBaselineData.hpp>
+#include <uavcan/equipment/gnss/RTCMStream.hpp>
 
 #include <lib/perf/perf_counter.h>
 
@@ -61,8 +62,6 @@
 
 class UavcanGnssBridge : public UavcanSensorBridgeBase
 {
-	static constexpr unsigned ORB_TO_UAVCAN_FREQUENCY_HZ = 10;
-
 public:
 	static const char *const NAME;
 
@@ -92,7 +91,8 @@ private:
 			  const float heading_accuracy);
 
 	void handleInjectDataTopic();
-	bool injectData(const uint8_t *data, size_t data_len);
+	bool PublishRTCMStream(const uint8_t *data, size_t data_len);
+	bool PublishMovingBaselineData(const uint8_t *data, size_t data_len);
 
 	typedef uavcan::MethodBinder < UavcanGnssBridge *,
 		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &) >
@@ -115,17 +115,23 @@ private:
 	uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxiliaryCbBinder> _sub_auxiliary;
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
-	uavcan::Publisher<ardupilot::gnss::MovingBaselineData> _pub_rtcm;
+
+	uavcan::Publisher<ardupilot::gnss::MovingBaselineData> _pub_moving_baseline_data;
+	uavcan::Publisher<uavcan::equipment::gnss::RTCMStream> _pub_rtcm_stream;
 
 	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::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data)};
 
 	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;
+	bool _publish_rtcm_stream{false};
+	bool _publish_moving_baseline_data{false};
+
+	perf_counter_t _rtcm_stream_pub_perf{nullptr};
+	perf_counter_t _moving_baseline_data_pub_perf{nullptr};
 };

src/drivers/uavcan/uavcan_params.c

@@ -194,6 +194,30 @@ PARAM_DEFINE_INT32(UAVCAN_LGT_NAV, 3);
  */
 PARAM_DEFINE_INT32(UAVCAN_LGT_LAND, 0);
 
+/**
+ * publish RTCM stream
+ *
+ * Enable UAVCAN RTCM stream publication
+ *  uavcan::equipment::gnss::RTCMStream
+ *
+ * @boolean
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_PUB_RTCM, 0);
+
+/**
+ * publish moving baseline data RTCM stream
+ *
+ * Enable UAVCAN RTCM stream publication
+ *  ardupilot::gnss::MovingBaselineData
+ *
+ * @boolean
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_PUB_MBD, 0);
+
 /**
  * subscription airspeed
  *