uavcan: gnss: MovingBaselineData subscriber (#26094)

* msg: GpsDump: increase queue from 8 to 16 and replace instance with device_id

* uavcan: gnss: MovingBaselineData subscriber for RTCM logging for PPK. Add uORB generation tracking to detect data loss on RTCM injection.

* add instance TODO

src/drivers/uavcan/sensors/gnss.cpp

@@ -60,6 +60,7 @@ UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node, NodeInfoPublisher *node_
 	_sub_fix(node),
 	_sub_fix2(node),
 	_sub_gnss_heading(node),
+	_sub_moving_baseline_data(node),
 	_pub_moving_baseline_data(node),
 	_pub_rtcm_stream(node),
 	_channel_using_fix2(new bool[_max_channels])
@@ -76,6 +77,7 @@ UavcanGnssBridge::~UavcanGnssBridge()
 	delete [] _channel_using_fix2;
 	perf_free(_rtcm_stream_pub_perf);
 	perf_free(_moving_baseline_data_pub_perf);
+	perf_free(_moving_baseline_data_sub_perf);
 }
 
 int
@@ -129,6 +131,15 @@ UavcanGnssBridge::init()
 		_moving_baseline_data_pub_perf = perf_alloc(PC_INTERVAL, "uavcan: gnss: moving baseline data rtcm stream pub");
 	}
 
+	// UAVCAN_SUB_MBD
+	int32_t uavcan_sub_mbd = 0;
+	param_get(param_find("UAVCAN_SUB_MBD"), &uavcan_sub_mbd);
+
+	if (uavcan_sub_mbd == 1) {
+		res = _sub_moving_baseline_data.start(MovingBaselineDataCbBinder(this, &UavcanGnssBridge::moving_baseline_data_sub_cb));
+		_moving_baseline_data_sub_perf = perf_alloc(PC_INTERVAL, "uavcan: gnss: moving baseline data rtcm stream sub");
+	}
+
 	return res;
 }
 
@@ -338,6 +349,7 @@ UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 	process_fixx(msg, fix_type, pos_cov, vel_cov, valid_covariances, valid_covariances, heading, heading_offset,
 		     heading_accuracy, noise_per_ms, jamming_indicator, jamming_state, spoofing_state);
 }
+
 void UavcanGnssBridge::gnss_relative_sub_cb(const
 		uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &msg)
 {
@@ -346,6 +358,41 @@ void UavcanGnssBridge::gnss_relative_sub_cb(const
 	_rel_heading_accuracy = math::radians(msg.reported_heading_acc_deg);
 
 }
+
+void UavcanGnssBridge::moving_baseline_data_sub_cb(const
+		uavcan::ReceivedDataStructure<ardupilot::gnss::MovingBaselineData> &msg)
+{
+	perf_count(_moving_baseline_data_sub_perf);
+
+	size_t total_bytes = msg.data.size();
+	size_t written = 0;
+
+	while (written < total_bytes) {
+		gps_dump_s dump = {};
+		dump.timestamp = hrt_absolute_time();
+
+		DeviceId device_id = {};
+		device_id.devid_s.bus_type = DeviceBusType::DeviceBusType_UAVCAN;
+		device_id.devid_s.bus = 0;
+		device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF;
+		device_id.devid_s.devtype = DRV_GPS_DEVTYPE_UAVCAN;
+
+		dump.instance = 0; // TODO: How can we determine the instance? Startup order of CANnodes is non-deterministic.
+		dump.device_id = device_id.devid;
+
+		size_t remaining = total_bytes - written;
+		size_t write_len = remaining < sizeof(dump.data) ? remaining : sizeof(dump.data);
+
+		memcpy(dump.data, msg.data.begin() + written, write_len);
+		dump.len = write_len;
+		dump.len &= 0x7F; // MSB of len is direction - 0 is from the device
+
+		written += write_len;
+
+		_gps_dump_pub.publish(dump);
+	}
+}
+
 template <typename FixType>
 void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
 				    uint8_t fix_type,
@@ -587,7 +634,17 @@ void UavcanGnssBridge::handleInjectDataTopic()
 			_last_rtcm_injection_time = hrt_absolute_time();
 		}
 
-		updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg);
+		auto &gps_inject_data_sub = _orb_inject_data_sub[_selected_rtcm_instance];
+
+		const unsigned last_generation = gps_inject_data_sub.get_last_generation();
+
+		updated = gps_inject_data_sub.update(&msg);
+
+		if (updated) {
+			if (gps_inject_data_sub.get_last_generation() != last_generation + 1) {
+				PX4_WARN("gps_inject_data lost, generation %u -> %u", last_generation, gps_inject_data_sub.get_last_generation());
+			}
+		}
 
 	} while (updated && num_injections < max_num_injections);
 }
@@ -655,4 +712,5 @@ void UavcanGnssBridge::print_status() const
 	UavcanSensorBridgeBase::print_status();
 	perf_print_counter(_rtcm_stream_pub_perf);
 	perf_print_counter(_moving_baseline_data_pub_perf);
+	perf_print_counter(_moving_baseline_data_sub_perf);
 }

src/drivers/uavcan/sensors/gnss.hpp

@@ -49,6 +49,7 @@
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/gps_inject_data.h>
+#include <uORB/topics/gps_dump.h>
 
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/gnss/Auxiliary.hpp>
@@ -84,6 +85,8 @@ private:
 	void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
 	void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg);
 	void gnss_relative_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &msg);
+	void moving_baseline_data_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::gnss::MovingBaselineData> &msg);
+
 
 	template <typename FixType>
 	void process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
@@ -119,6 +122,10 @@ private:
 		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<ardupilot::gnss::RelPosHeading> &) >
 		RelPosHeadingCbBinder;
 
+	typedef uavcan::MethodBinder < UavcanGnssBridge *,
+		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<ardupilot::gnss::MovingBaselineData> &) >
+		MovingBaselineDataCbBinder;
+
 	uavcan::INode &_node;
 
 	uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxiliaryCbBinder> _sub_auxiliary;
@@ -126,6 +133,9 @@ private:
 	uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
 	uavcan::Subscriber<ardupilot::gnss::RelPosHeading, RelPosHeadingCbBinder> _sub_gnss_heading;
 
+	// Used for MSM7 logging for PPK workflows
+	uavcan::Subscriber<ardupilot::gnss::MovingBaselineData, MovingBaselineDataCbBinder> _sub_moving_baseline_data;
+
 	uavcan::Publisher<ardupilot::gnss::MovingBaselineData> _pub_moving_baseline_data;
 	uavcan::Publisher<uavcan::equipment::gnss::RTCMStream> _pub_rtcm_stream;
 
@@ -137,6 +147,8 @@ private:
 	hrt_abstime		_last_rtcm_injection_time{0};	///< time of last rtcm injection
 	uint8_t			_selected_rtcm_instance{0};	///< uorb instance that is being used for RTCM corrections
 
+	uORB::Publication<gps_dump_s> _gps_dump_pub{ORB_ID(gps_dump)}; // For PPK
+
 	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
@@ -150,6 +162,7 @@ private:
 
 	perf_counter_t _rtcm_stream_pub_perf{nullptr};
 	perf_counter_t _moving_baseline_data_pub_perf{nullptr};
+	perf_counter_t _moving_baseline_data_sub_perf{nullptr};
 
 	hrt_abstime _last_rate_measurement{0};
 	float _rtcm_injection_rate{0.f}; ///< RTCM message injection rate

src/drivers/uavcan/uavcan_main.hpp

@@ -200,7 +200,7 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams
 	 *  1000000/200
 	 */
 
-	static constexpr unsigned RxQueueLenPerIface	= FramePerMSecond * ScheduleIntervalMs; // At
+	static constexpr unsigned RxQueueLenPerIface	= FramePerMSecond * ScheduleIntervalMs;
 
 public:
 	typedef UAVCAN_DRIVER::CanInitHelper<RxQueueLenPerIface> CanInitHelper;

src/drivers/uavcan/uavcan_params.c

@@ -436,3 +436,15 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_RNG, 0);
  * @group UAVCAN
  */
 PARAM_DEFINE_INT32(UAVCAN_SUB_BTN, 0);
+
+/**
+ * subscription MovingBaselineData
+ *
+ * Enable UAVCAN MovingBaselineData subscription.
+ *  ardupilot::gnss::MovingBaselineData
+ *
+ * @boolean
+ * @reboot_required true
+ * @group UAVCAN
+ */
+PARAM_DEFINE_INT32(UAVCAN_SUB_MBD, 0);