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 <alexmikhalevalex@gmail.com>

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<uavcan::eq
 		return;
 	}
 
+	uint8_t fix_type = msg.status;
+
 	const bool valid_pos_cov = !msg.position_covariance.empty();
 	const bool valid_vel_cov = !msg.velocity_covariance.empty();
 
@@ -132,12 +139,14 @@ UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
 	float vel_cov[9];
 	msg.velocity_covariance.unpackSquareMatrix(vel_cov);
 
-	process_fixx(msg, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov);
+	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov);
 }
 
 void
 UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &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<uavcan::e
 		_channel_using_fix2[ch] = true;
 	}
 
+	uint8_t fix_type = msg.status;
+
+	switch (msg.mode) {
+	case Fix2::MODE_DGPS:
+		fix_type = 4; // RTCM code differential
+		break;
+
+	case Fix2::MODE_RTK:
+		switch (msg.sub_mode) {
+		case Fix2::SUB_MODE_RTK_FLOAT:
+			fix_type = 5; // RTK float
+			break;
+
+		case Fix2::SUB_MODE_RTK_FIXED:
+			fix_type = 6; // RTK fixed
+			break;
+		}
+
+		break;
+	}
+
 	float pos_cov[9] {};
 	float vel_cov[9] {};
 	bool valid_covariances = true;
@@ -249,11 +279,12 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 		}
 	}
 
-	process_fixx(msg, pos_cov, vel_cov, valid_covariances, valid_covariances);
+	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances);
 }
 
 template <typename FixType>
 void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &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<FixType>
 		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<FixType>
 
 	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);
+}

src/drivers/uavcan/sensors/gnss.hpp

@@ -47,11 +47,15 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/gps_inject_data.h>
 
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/gnss/Auxiliary.hpp>
 #include <uavcan/equipment/gnss/Fix.hpp>
 #include <uavcan/equipment/gnss/Fix2.hpp>
+#include <uavcan/equipment/gnss/RTCMStream.hpp>
+
+#include <lib/perf/perf_counter.h>
 
 #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 <typename FixType>
 	void process_fixx(const uavcan::ReceivedDataStructure<FixType> &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<uavcan::equipment::gnss::Auxiliary> &) >
 		AuxiliaryCbBinder;
@@ -103,11 +113,13 @@ 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<uavcan::equipment::gnss::RTCMStream> _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<sensor_gps_s>	_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;
 };

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.

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