uavcan module cleanup

- move most orb to uORB::Publication and uORB::Subscription
 - update legacy message handling (warn to PX4_INFO, PX4_WARN, PX4_ERR)
 - add perf counters
 - sensors/mag support newer `uavcan::equipment::ahrs::MagneticFieldStrength2` message
 - sensors/gps support `uavcan::equipment::gnss::Auxiliary` for hdop and vdop
 - sensors delete obsolete ioctl and read methods
 - use PublicationMulti for actuator_outputs and esc_reports (to coexist with other output modules)
 - add GNSS parameter metadata (parameters_injected.xml)

src/drivers/uavcan/sensors/baro.cpp

@@ -44,8 +44,7 @@ const char *const UavcanBarometerBridge::NAME = "baro";
 UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
 	UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
 	_sub_air_pressure_data(node),
-	_sub_air_temperature_data(node),
-	_reports(2, sizeof(sensor_baro_s))
+	_sub_air_temperature_data(node)
 { }
 
 int UavcanBarometerBridge::init()
@@ -73,49 +72,15 @@ int UavcanBarometerBridge::init()
 	return 0;
 }
 
-ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen)
-{
-	unsigned count = buflen / sizeof(sensor_baro_s);
-	sensor_baro_s *baro_buf = reinterpret_cast<sensor_baro_s *>(buffer);
-	int ret = 0;
-
-	/* buffer must be large enough */
-	if (count < 1) {
-		return -ENOSPC;
-	}
-
-	while (count--) {
-		if (_reports.get(baro_buf)) {
-			ret += sizeof(*baro_buf);
-			baro_buf++;
-		}
-	}
-
-	/* if there was no data, warn the caller */
-	return ret ? ret : -EAGAIN;
-}
-
-int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-	case SENSORIOCSPOLLRATE: {
-			// not supported yet, pretend that everything is ok
-			return OK;
-		}
-
-	default: {
-			return CDev::ioctl(filp, cmd, arg);
-		}
-	}
-}
-
-void UavcanBarometerBridge::air_temperature_sub_cb(const
+void
+UavcanBarometerBridge::air_temperature_sub_cb(const
 		uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
 {
 	last_temperature_kelvin = msg.static_temperature;
 }
 
-void UavcanBarometerBridge::air_pressure_sub_cb(const
+void
+UavcanBarometerBridge::air_pressure_sub_cb(const
 		uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg)
 {
 	sensor_baro_s report{};
@@ -135,8 +100,5 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
 	/* TODO get device ID for sensor */
 	report.device_id = 0;
 
-	// add to the ring buffer
-	_reports.force(&report);
-
 	publish(msg.getSrcNodeID().get(), &report);
 }

src/drivers/uavcan/sensors/baro.hpp

@@ -39,13 +39,10 @@
 
 #include "sensor_bridge.hpp"
 #include <drivers/drv_baro.h>
-#include <drivers/device/ringbuffer.h>
 
 #include <uavcan/equipment/air_data/StaticPressure.hpp>
 #include <uavcan/equipment/air_data/StaticTemperature.hpp>
 
-class RingBuffer;
-
 class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
 {
 public:
@@ -58,8 +55,6 @@ public:
 	int init() override;
 
 private:
-	ssize_t read(struct file *filp, char *buffer, size_t buflen);
-	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
 
 	void air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg);
 	void air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg);
@@ -77,8 +72,6 @@ private:
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data;
 	uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
 
-	ringbuffer::RingBuffer _reports;
-
-	float last_temperature_kelvin = 0.0f;
+	float last_temperature_kelvin{0.0f};
 
 };

src/drivers/uavcan/sensors/gnss.cpp

@@ -47,24 +47,22 @@
 #include <systemlib/err.h>
 #include <mathlib/mathlib.h>
 
+using namespace time_literals;
+
 const char *const UavcanGnssBridge::NAME = "gnss";
 
 UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
 	_node(node),
+	_sub_auxiliary(node),
 	_sub_fix(node),
 	_sub_fix2(node),
 	_pub_fix2(node),
-	_orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb)),
-	_report_pub(nullptr)
+	_orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb))
 {
 }
 
-UavcanGnssBridge::~UavcanGnssBridge()
-{
-	(void) orb_unsubscribe(_orb_sub_gnss);
-}
-
-int UavcanGnssBridge::init()
+int
+UavcanGnssBridge::init()
 {
 	int res = _pub_fix2.init(uavcan::TransferPriority::MiddleLower);
 
@@ -73,6 +71,13 @@ int UavcanGnssBridge::init()
 		return res;
 	}
 
+	res = _sub_auxiliary.start(AuxiliaryCbBinder(this, &UavcanGnssBridge::gnss_auxiliary_sub_cb));
+
+	if (res < 0) {
+		PX4_WARN("GNSS auxiliary sub failed %i", res);
+		return res;
+	}
+
 	res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
 
 	if (res < 0) {
@@ -87,18 +92,19 @@ int UavcanGnssBridge::init()
 		return res;
 	}
 
-	_orb_to_uavcan_pub_timer.startPeriodic(
-		uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ));
+	_orb_to_uavcan_pub_timer.startPeriodic(uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ));
 
 	return res;
 }
 
-unsigned UavcanGnssBridge::get_num_redundant_channels() const
+unsigned
+UavcanGnssBridge::get_num_redundant_channels() const
 {
 	return (_receiver_node_id < 0) ? 0 : 1;
 }
 
-void UavcanGnssBridge::print_status() const
+void
+UavcanGnssBridge::print_status() const
 {
 	printf("RX errors: %d, using old Fix: %d, receiver node id: ",
 	       _sub_fix.getFailureCount(), int(_old_fix_subscriber_active));
@@ -111,7 +117,17 @@ void UavcanGnssBridge::print_status() const
 	}
 }
 
-void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
+void
+UavcanGnssBridge::gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &msg)
+{
+	// store latest hdop and vdop for use in process_fixx();
+	_last_gnss_auxiliary_timestamp = hrt_absolute_time();
+	_last_gnss_auxiliary_hdop = msg.hdop;
+	_last_gnss_auxiliary_vdop = msg.vdop;
+}
+
+void
+UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
 {
 	const bool valid_pos_cov = !msg.position_covariance.empty();
 	const bool valid_vel_cov = !msg.velocity_covariance.empty();
@@ -125,10 +141,11 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 	process_fixx(msg, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov);
 }
 
-void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg)
+void
+UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg)
 {
 	if (_old_fix_subscriber_active) {
-		PX4_WARN("GNSS Fix2 message detected, disabling support for the old Fix message");
+		PX4_INFO("GNSS Fix2 message detected, disabling support for the old Fix message");
 		_sub_fix.stop();
 		_old_fix_subscriber_active = false;
 		_receiver_node_id = -1;
@@ -139,8 +156,8 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
 	bool valid_covariances = true;
 
 	switch (msg.covariance.size()) {
-	// Scalar matrix
 	case 1: {
+			// Scalar matrix
 			const auto x = msg.covariance[0];
 
 			pos_cov[0] = x;
@@ -150,11 +167,11 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
 			vel_cov[0] = x;
 			vel_cov[4] = x;
 			vel_cov[8] = x;
-			break;
 		}
+		break;
 
-	// Diagonal matrix (the most common case)
 	case 6: {
+			// Diagonal matrix (the most common case)
 			pos_cov[0] = msg.covariance[0];
 			pos_cov[4] = msg.covariance[1];
 			pos_cov[8] = msg.covariance[2];
@@ -162,19 +179,21 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
 			vel_cov[0] = msg.covariance[3];
 			vel_cov[4] = msg.covariance[4];
 			vel_cov[8] = msg.covariance[5];
-			break;
-		}
 
-	// Upper triangular matrix.
-	// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
-	// Sub-matrix indexes (empty squares contain velocity-position covariance data):
-	// 0  1  2
-	// 1  6  7
-	// 2  7 11
-	//         15 16 17
-	//         16 18 19
-	//         17 19 20
+		}
+		break;
+
+
 	case 21: {
+			// Upper triangular matrix.
+			// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
+			// Sub-matrix indexes (empty squares contain velocity-position covariance data):
+			// 0  1  2
+			// 1  6  7
+			// 2  7 11
+			//         15 16 17
+			//         16 18 19
+			//         17 19 20
 			pos_cov[0] = msg.covariance[0];
 			pos_cov[1] = msg.covariance[1];
 			pos_cov[2] = msg.covariance[2];
@@ -197,17 +216,16 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
 		}
 
 	/* FALLTHROUGH */
-
-	// Full matrix 6x6.
-	// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
-	// Sub-matrix indexes (empty squares contain velocity-position covariance data):
-	//  0  1  2
-	//  6  7  8
-	// 12 13 14
-	//          21 22 23
-	//          27 28 29
-	//          33 34 35
 	case 36: {
+			// Full matrix 6x6.
+			// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
+			// Sub-matrix indexes (empty squares contain velocity-position covariance data):
+			//  0  1  2
+			//  6  7  8
+			// 12 13 14
+			//          21 22 23
+			//          27 28 29
+			//          33 34 35
 			pos_cov[0] = msg.covariance[0];
 			pos_cov[1] = msg.covariance[1];
 			pos_cov[2] = msg.covariance[2];
@@ -230,9 +248,8 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
 		}
 
 	/* FALLTHROUGH */
-
-	// Either empty or invalid sized, interpret as zero matrix
 	default: {
+			// Either empty or invalid sized, interpret as zero matrix
 			valid_covariances = false;
 			break;	// Nothing to do
 		}
@@ -243,15 +260,13 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
 
 template <typename FixType>
 void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
-				    const float (&pos_cov)[9],
-				    const float (&vel_cov)[9],
-				    const bool valid_pos_cov,
-				    const bool valid_vel_cov)
+				    const float (&pos_cov)[9], const float (&vel_cov)[9],
+				    const bool valid_pos_cov, const bool valid_vel_cov)
 {
 	// This bridge does not support redundant GNSS receivers yet.
 	if (_receiver_node_id < 0) {
 		_receiver_node_id = msg.getSrcNodeID().get();
-		PX4_WARN("GNSS receiver node ID: %d", _receiver_node_id);
+		PX4_INFO("GNSS receiver node ID: %d", _receiver_node_id);
 
 	} else {
 		if (_receiver_node_id != msg.getSrcNodeID().get()) {
@@ -259,7 +274,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 		}
 	}
 
-	auto report = ::vehicle_gps_position_s();
+	vehicle_gps_position_s report{};
 
 	/*
 	 * FIXME HACK
@@ -329,107 +344,78 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 
 	report.timestamp_time_relative = 0;
 
-	const std::uint64_t gnss_ts_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();
+	const uint64_t gnss_ts_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();
 
 	switch (msg.gnss_time_standard) {
-	case FixType::GNSS_TIME_STANDARD_UTC: {
-			report.time_utc_usec = gnss_ts_usec;
-			break;
+	case FixType::GNSS_TIME_STANDARD_UTC:
+		report.time_utc_usec = gnss_ts_usec;
+		break;
+
+	case FixType::GNSS_TIME_STANDARD_GPS:
+		if (msg.num_leap_seconds > 0) {
+			report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds + 9;
 		}
 
-	case FixType::GNSS_TIME_STANDARD_GPS: {
-			if (msg.num_leap_seconds > 0) {
-				report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds + 9;
-			}
+		break;
 
-			break;
+	case FixType::GNSS_TIME_STANDARD_TAI:
+		if (msg.num_leap_seconds > 0) {
+			report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds - 10;
 		}
 
-	case FixType::GNSS_TIME_STANDARD_TAI: {
-			if (msg.num_leap_seconds > 0) {
-				report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds - 10;
-			}
+		break;
 
-			break;
-		}
-
-	default: {
-			break;
-		}
+	default:
+		break;
 	}
 
-//TODO px4_clock_settime does nothing on the Snapdragon platform
-#ifndef __PX4_QURT
-
 	// If we haven't already done so, set the system clock using GPS data
 	if (valid_pos_cov && !_system_clock_set) {
-		timespec ts;
-		memset(&ts, 0, sizeof(ts));
+		timespec ts{};
+
 		// get the whole microseconds
 		ts.tv_sec = report.time_utc_usec / 1000000ULL;
+
 		// get the remainder microseconds and convert to nanoseconds
 		ts.tv_nsec = (report.time_utc_usec % 1000000ULL) * 1000;
+
 		px4_clock_settime(CLOCK_REALTIME, &ts);
+
 		_system_clock_set = true;
 	}
 
-#endif
-
 	report.satellites_used = msg.sats_used;
 
-	// Using PDOP for HDOP and VDOP
-	// Relevant discussion: https://github.com/PX4/Firmware/issues/5153
-	report.hdop = msg.pdop;
-	report.vdop = msg.pdop;
+	if (hrt_elapsed_time(&_last_gnss_auxiliary_timestamp) < 2_s) {
+		report.hdop = _last_gnss_auxiliary_hdop;
+		report.vdop = _last_gnss_auxiliary_vdop;
+
+	} else {
+		// Using PDOP for HDOP and VDOP
+		// Relevant discussion: https://github.com/PX4/Firmware/issues/5153
+		report.hdop = msg.pdop;
+		report.vdop = msg.pdop;
+	}
 
 	report.heading = NAN;
 	report.heading_offset = NAN;
 
 	// Publish to a multi-topic
-	int32_t gps_orb_instance;
-	orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_pub, &report, &gps_orb_instance,
-			 ORB_PRIO_HIGH);
+	_gps_pub.publish(report);
 
 	// Doing less time critical stuff here
 	if (_orb_to_uavcan_pub_timer.isRunning()) {
 		_orb_to_uavcan_pub_timer.stop();
-		PX4_WARN("GNSS ORB->UAVCAN bridge stopped, because there are other GNSS publishers");
+		PX4_INFO("GNSS ORB->UAVCAN bridge stopped, because there are other GNSS publishers");
 	}
 }
 
-void UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &)
+void
+UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &)
 {
-	if (_orb_sub_gnss < 0) {
-		// ORB subscription stops working if this is relocated into init()
-		_orb_sub_gnss = orb_subscribe(ORB_ID(vehicle_gps_position));
+	vehicle_gps_position_s orb_msg{};
 
-		if (_orb_sub_gnss < 0) {
-			PX4_WARN("GNSS ORB sub errno %d", errno);
-			return;
-		}
-
-		PX4_WARN("GNSS ORB fd %d", _orb_sub_gnss);
-	}
-
-	{
-		bool updated = false;
-		const int res = orb_check(_orb_sub_gnss, &updated);
-
-		if (res < 0) {
-			PX4_WARN("GNSS ORB check err %d %d", res, errno);
-			return;
-		}
-
-		if (!updated) {
-			return;
-		}
-	}
-
-	auto orb_msg = ::vehicle_gps_position_s();
-	const int res = orb_copy(ORB_ID(vehicle_gps_position), _orb_sub_gnss, &orb_msg);
-
-	if (res < 0) {
-		PX4_WARN("GNSS ORB read errno %d", errno);
+	if (!_orb_sub_gnss.update(&orb_msg)) {
 		return;
 	}
 
@@ -462,5 +448,5 @@ void UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &)
 	msg.pdop = (orb_msg.hdop > orb_msg.vdop) ? orb_msg.hdop : orb_msg.vdop;  // this is a hack :(
 
 	// Publishing now
-	(void) _pub_fix2.broadcast(msg);
+	_pub_fix2.broadcast(msg);
 }

src/drivers/uavcan/sensors/gnss.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-2019 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
@@ -44,10 +44,12 @@
 
 #pragma once
 
-#include <uORB/uORB.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/vehicle_gps_position.h>
 
 #include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/gnss/Auxiliary.hpp>
 #include <uavcan/equipment/gnss/Fix.hpp>
 #include <uavcan/equipment/gnss/Fix2.hpp>
 
@@ -61,7 +63,7 @@ public:
 	static const char *const NAME;
 
 	UavcanGnssBridge(uavcan::INode &node);
-	~UavcanGnssBridge();
+	~UavcanGnssBridge() = default;
 
 	const char *get_name() const override { return NAME; }
 
@@ -75,18 +77,21 @@ private:
 	/**
 	 * GNSS fix message will be reported via this callback.
 	 */
+	void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &msg);
 	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);
 
 	template <typename FixType>
 	void process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
-			  const float (&pos_cov)[9],
-			  const float (&vel_cov)[9],
-			  const bool valid_pos_cov,
-			  const bool valid_vel_cov);
+			  const float (&pos_cov)[9], const float (&vel_cov)[9],
+			  const bool valid_pos_cov, const bool valid_vel_cov);
 
 	void broadcast_from_orb(const uavcan::TimerEvent &);
 
+	typedef uavcan::MethodBinder < UavcanGnssBridge *,
+		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &) >
+		AuxiliaryCbBinder;
+
 	typedef uavcan::MethodBinder < UavcanGnssBridge *,
 		void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &) >
 		FixCbBinder;
@@ -100,17 +105,24 @@ private:
 		TimerCbBinder;
 
 	uavcan::INode &_node;
+
+	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::Fix2> _pub_fix2;
+
 	uavcan::TimerEventForwarder<TimerCbBinder> _orb_to_uavcan_pub_timer;
-	int _receiver_node_id = -1;
 
-	bool _old_fix_subscriber_active = true;
+	uint64_t	_last_gnss_auxiliary_timestamp{0};
+	float		_last_gnss_auxiliary_hdop{0.0f};
+	float		_last_gnss_auxiliary_vdop{0.0f};
 
-	orb_advert_t _report_pub;                ///< uORB pub for gnss position
+	uORB::PublicationMulti<vehicle_gps_position_s>	_gps_pub{ORB_ID(vehicle_gps_position), ORB_PRIO_DEFAULT};
+	uORB::Subscription				_orb_sub_gnss{ORB_ID(vehicle_gps_position)};
 
-	int _orb_sub_gnss = -1;                  ///< uORB sub for gnss position, used for bridging uORB --> UAVCAN
+	int	_receiver_node_id{-1};
+	bool	_old_fix_subscriber_active{true};
+	bool	_system_clock_set{false};			///< Have we set the system clock at least once from GNSS data?
 
-	bool _system_clock_set = false; ///< Have we set the system clock at least once from GNSS data?
 };

src/drivers/uavcan/sensors/mag.cpp

@@ -44,7 +44,8 @@ const char *const UavcanMagnetometerBridge::NAME = "mag";
 
 UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
 	UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
-	_sub_mag(node)
+	_sub_mag(node),
+	_sub_mag2(node)
 {
 	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;     // <-- Why?
 
@@ -53,7 +54,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
 	_scale.z_scale = 1.0F;
 }
 
-int UavcanMagnetometerBridge::init()
+int
+UavcanMagnetometerBridge::init()
 {
 	int res = device::CDev::init();
 
@@ -64,40 +66,22 @@ int UavcanMagnetometerBridge::init()
 	res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
 
 	if (res < 0) {
-		DEVICE_LOG("failed to start uavcan sub: %d", res);
+		PX4_ERR("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
+	res = _sub_mag2.start(Mag2CbBinder(this, &UavcanMagnetometerBridge::mag2_sub_cb));
+
+	if (res < 0) {
+		PX4_ERR("failed to start uavcan sub2: %d", res);
 		return res;
 	}
 
 	return 0;
 }
 
-ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen)
-{
-	static uint64_t last_read = 0;
-	struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
-
-	/* buffer must be large enough */
-	unsigned count = buflen / sizeof(struct mag_report);
-
-	if (count < 1) {
-		return -ENOSPC;
-	}
-
-	if (last_read < _report.timestamp) {
-		/* copy report */
-		lock();
-		*mag_buf = _report;
-		last_read = _report.timestamp;
-		unlock();
-		return sizeof(struct mag_report);
-
-	} else {
-		/* no new data available, warn caller */
-		return -EAGAIN;
-	}
-}
-
-int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
+int
+UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 
@@ -127,9 +111,33 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 	}
 }
 
-void UavcanMagnetometerBridge::mag_sub_cb(const
-		uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
-		&msg)
+void
+UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
+				     &msg)
+{
+	lock();
+	/*
+	 * FIXME HACK
+	 * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
+	 * It stopped working when the time sync feature has been introduced, because it caused libuavcan
+	 * to use an independent time source (based on hardware TIM5) instead of HRT.
+	 * The proper solution is to be developed.
+	 */
+	_report.timestamp = hrt_absolute_time();
+	_report.device_id = _device_id.devid;
+	_report.is_external = true;
+
+	_report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale;
+	_report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale;
+	_report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale;
+	unlock();
+
+	publish(msg.getSrcNodeID().get(), &_report);
+}
+
+void
+UavcanMagnetometerBridge::mag2_sub_cb(const
+				      uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength2> &msg)
 {
 	lock();
 	/*

src/drivers/uavcan/sensors/mag.hpp

@@ -38,9 +38,11 @@
 #pragma once
 
 #include "sensor_bridge.hpp"
+
 #include <drivers/drv_mag.h>
 
 #include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
+#include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
 
 class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
 {
@@ -54,17 +56,25 @@ public:
 	int init() override;
 
 private:
-	ssize_t	read(struct file *filp, char *buffer, size_t buflen);
+
 	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
 
 	void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &msg);
+	void mag2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength2> &msg);
 
 	typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
 		void (UavcanMagnetometerBridge::*)
 		(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &) >
 		MagCbBinder;
 
+	typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
+		void (UavcanMagnetometerBridge::*)
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength2> &) >
+		Mag2CbBinder;
+
 	uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength, MagCbBinder> _sub_mag;
-	struct mag_calibration_s _scale = {};
-	mag_report _report =  {};
+	uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength2, Mag2CbBinder> _sub_mag2;
+
+	mag_calibration_s _scale{};
+	sensor_mag_s _report{};
 };

src/drivers/uavcan/sensors/sensor_bridge.cpp

@@ -70,10 +70,9 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
 	delete [] _channels;
 }
 
-void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
+void
+UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 {
-	assert(report != nullptr);
-
 	Channel *channel = nullptr;
 
 	// Checking if such channel already exists
@@ -141,7 +140,8 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 	(void)orb_publish(_orb_topic, channel->orb_advert, report);
 }
 
-unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
+unsigned
+UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
 {
 	unsigned out = 0;
 
@@ -154,7 +154,8 @@ unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
 	return out;
 }
 
-void UavcanCDevSensorBridgeBase::print_status() const
+void
+UavcanCDevSensorBridgeBase::print_status() const
 {
 	printf("devname: %s\n", _class_devname);
 

src/drivers/uavcan/sensors/sensor_bridge.hpp

@@ -51,7 +51,7 @@ class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBr
 public:
 	static constexpr unsigned MAX_NAME_LEN = 20;
 
-	virtual ~IUavcanSensorBridge() { }
+	virtual ~IUavcanSensorBridge() = default;
 
 	/**
 	 * Returns ASCII name of the bridge.
@@ -101,7 +101,7 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD
 	bool _out_of_channels = false;
 
 protected:
-	static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
+	static constexpr unsigned DEFAULT_MAX_CHANNELS = 5;
 
 	UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
 				   const orb_id_t orb_topic_sensor,