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);
+}