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/actuators/esc.cpp

@@ -52,14 +52,6 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_orb_timer(node)
 {
 	_uavcan_pub_raw_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
-
-	if (_perfcnt_invalid_input == nullptr) {
-		errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input");
-	}
-
-	if (_perfcnt_scaling_error == nullptr) {
-		errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error");
-	}
 }
 
 UavcanEscController::~UavcanEscController()
@@ -68,13 +60,14 @@ UavcanEscController::~UavcanEscController()
 	perf_free(_perfcnt_scaling_error);
 }
 
-int UavcanEscController::init()
+int
+UavcanEscController::init()
 {
 	// ESC status subscription
 	int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
 
 	if (res < 0) {
-		warnx("ESC status sub failed %i", res);
+		PX4_ERR("ESC status sub failed %i", res);
 		return res;
 	}
 
@@ -85,11 +78,13 @@ int UavcanEscController::init()
 	return res;
 }
 
-void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
+void
+UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 {
 	if ((outputs == nullptr) ||
 	    (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
 	    (num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
+
 		perf_count(_perfcnt_invalid_input);
 		return;
 	}
@@ -111,9 +106,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	 */
 	uavcan::equipment::esc::RawCommand msg;
 
-	actuator_outputs_s actuator_outputs = {};
+	actuator_outputs_s actuator_outputs{};
 	actuator_outputs.noutputs = num_outputs;
-	actuator_outputs.timestamp = hrt_absolute_time();
 
 	static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
 	const float cmd_min = _run_at_idle_throttle_when_armed ? 1.0F : 0.0F;
@@ -167,21 +161,15 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	 * Publish the command message to the bus
 	 * Note that for a quadrotor it takes one CAN frame
 	 */
-	(void)_uavcan_pub_raw_cmd.broadcast(msg);
+	_uavcan_pub_raw_cmd.broadcast(msg);
 
 	// Publish actuator outputs
-	if (_actuator_outputs_pub != nullptr) {
-		orb_publish(ORB_ID(actuator_outputs), _actuator_outputs_pub, &actuator_outputs);
-
-	} else {
-		int instance;
-		_actuator_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &actuator_outputs,
-					&instance, ORB_PRIO_DEFAULT);
-	}
-
+	actuator_outputs.timestamp = hrt_absolute_time();
+	_actuator_outputs_pub.publish(actuator_outputs);
 }
 
-void UavcanEscController::arm_all_escs(bool arm)
+void
+UavcanEscController::arm_all_escs(bool arm)
 {
 	if (arm) {
 		_armed_mask = -1;
@@ -191,7 +179,8 @@ void UavcanEscController::arm_all_escs(bool arm)
 	}
 }
 
-void UavcanEscController::arm_single_esc(int num, bool arm)
+void
+UavcanEscController::arm_single_esc(int num, bool arm)
 {
 	if (arm) {
 		_armed_mask = MOTOR_BIT(num);
@@ -201,7 +190,8 @@ void UavcanEscController::arm_single_esc(int num, bool arm)
 	}
 }
 
-void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
+void
+UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
 {
 	if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
 		auto &ref = _esc_status.esc[msg.esc_index];
@@ -217,7 +207,8 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
 	}
 }
 
-void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
+void
+UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
 {
 	_esc_status.timestamp = hrt_absolute_time();
 	_esc_status.esc_count = _rotor_count;
@@ -225,18 +216,14 @@ void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
 	_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
 	_esc_status.esc_online_flags = UavcanEscController::check_escs_status();
 
-	if (_esc_status_pub != nullptr) {
-		(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
-
-	} else {
-		_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
-	}
+	_esc_status_pub.publish(_esc_status);
 }
 
-uint8_t UavcanEscController::check_escs_status()
+uint8_t
+UavcanEscController::check_escs_status()
 {
 	int esc_status_flags = 0;
-	hrt_abstime now = hrt_absolute_time();
+	const hrt_abstime now = hrt_absolute_time();
 
 	for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
 

src/drivers/uavcan/actuators/esc.hpp

@@ -47,12 +47,12 @@
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
-#include <perf/perf_counter.h>
-#include <uORB/topics/esc_status.h>
+#include <lib/perf/perf_counter.h>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
 
-
 class UavcanEscController
 {
 public:
@@ -95,17 +95,19 @@ private:
 	static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 5;	///< 0..31, inclusive, 0 - highest, 31 - lowest
 
 	typedef uavcan::MethodBinder<UavcanEscController *,
-		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
-		StatusCbBinder;
+		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;
 
-	typedef uavcan::MethodBinder<UavcanEscController *, void (UavcanEscController::*)(const uavcan::TimerEvent &)>
-	TimerCbBinder;
+	typedef uavcan::MethodBinder<UavcanEscController *,
+		void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;
+
+	bool		_armed{false};
+	bool		_run_at_idle_throttle_when_armed{false};
+
+	esc_status_s	_esc_status{};
+
+	uORB::PublicationMulti<actuator_outputs_s> _actuator_outputs_pub{ORB_ID(actuator_outputs)};
+	uORB::PublicationMulti<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
 
-	bool		_armed = false;
-	bool		_run_at_idle_throttle_when_armed = false;
-	esc_status_s	_esc_status = {};
-	orb_advert_t	_esc_status_pub = nullptr;
-	orb_advert_t _actuator_outputs_pub = nullptr;
 	uint8_t		_rotor_count = 0;
 
 	/*
@@ -120,12 +122,12 @@ private:
 	/*
 	 * ESC states
 	 */
-	uint32_t 			_armed_mask = 0;
-	uint8_t				_max_number_of_nonzero_outputs = 0;
+	uint32_t 			_armed_mask{0};
+	uint8_t				_max_number_of_nonzero_outputs{0};
 
 	/*
 	 * Perf counters
 	 */
-	perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
-	perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error");
+	perf_counter_t _perfcnt_invalid_input{perf_alloc(PC_COUNT, "uavcan_esc_invalid_input")};
+	perf_counter_t _perfcnt_scaling_error{perf_alloc(PC_COUNT, "uavcan_esc_scaling_error")};
 };

src/drivers/uavcan/actuators/hardpoint.cpp

@@ -48,13 +48,13 @@ UavcanHardpointController::UavcanHardpointController(uavcan::INode &node) :
 	_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::MiddleLower);
 }
 
-
 UavcanHardpointController::~UavcanHardpointController()
 {
 
 }
 
-int UavcanHardpointController::init()
+int
+UavcanHardpointController::init()
 {
 	/*
 	 * Setup timer and call back function for periodic updates
@@ -63,7 +63,8 @@ int UavcanHardpointController::init()
 	return 0;
 }
 
-void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command)
+void
+UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command)
 {
 	_cmd.command = command;
 	_cmd.hardpoint_id = hardpoint_id;
@@ -71,7 +72,7 @@ void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t comma
 	/*
 	 * Publish the command message to the bus
 	 */
-	(void)_uavcan_pub_raw_cmd.broadcast(_cmd);
+	_uavcan_pub_raw_cmd.broadcast(_cmd);
 
 	/*
 	 * Start the periodic update timer after a command is set
@@ -81,10 +82,10 @@ void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t comma
 	}
 
 }
-void UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
+
+void
+UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
 {
-	/*
-	 * Broadcast command at MAX_RATE_HZ
-	 */
-	(void)_uavcan_pub_raw_cmd.broadcast(_cmd);
+	// Broadcast command at MAX_RATE_HZ
+	_uavcan_pub_raw_cmd.broadcast(_cmd);
 }

src/drivers/uavcan/allocator.hpp

@@ -61,8 +61,8 @@ struct Allocator : public uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSiz
 	~Allocator()
 	{
 		if (getNumAllocatedBlocks() > 0) {
-			warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
-			      getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
+			PX4_ERR("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
+				getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
 		}
 	}
 };

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,

src/drivers/uavcan/uavcan_main.hpp

@@ -45,6 +45,12 @@
 #include <px4_config.h>
 
 #include "uavcan_driver.hpp"
+#include "uavcan_servers.hpp"
+#include "allocator.hpp"
+#include "actuators/esc.hpp"
+#include "actuators/hardpoint.hpp"
+#include "sensors/sensor_bridge.hpp"
+
 #include <uavcan/helpers/heap_based_pool_allocator.hpp>
 #include <uavcan/protocol/global_time_sync_master.hpp>
 #include <uavcan/protocol/global_time_sync_slave.hpp>
@@ -53,21 +59,16 @@
 #include <uavcan/protocol/param/ExecuteOpcode.hpp>
 #include <uavcan/protocol/RestartNode.hpp>
 
-#include <drivers/device/device.h>
-#include <perf/perf_counter.h>
+#include <lib/drivers/device/device.h>
+#include <lib/perf/perf_counter.h>
 
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_outputs.h>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/test_motor.h>
+#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_direct.h>
-
-#include "actuators/esc.hpp"
-#include "actuators/hardpoint.hpp"
-#include "sensors/sensor_bridge.hpp"
-
-#include "uavcan_servers.hpp"
-#include "allocator.hpp"
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/test_motor.h>
 
 #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN	4
 
@@ -131,10 +132,10 @@ public:
 
 	void		hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command);
 
-	static UavcanNode *instance() { return _instance; }
+	static UavcanNode	*instance() { return _instance; }
 	static int		 getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
 	int			 fw_server(eServerAction action);
-	void			attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
+	void			 attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
 	int			 list_params(int remote_node_id);
 	int			 save_params(int remote_node_id);
 	int			 set_param(int remote_node_id, const char *name, char *value);
@@ -146,42 +147,38 @@ private:
 	int		init(uavcan::NodeID node_id);
 	void		node_spin_once();
 	int		run();
+
 	int		add_poll_fd(int fd);			///< add a fd to poll list, returning index into _poll_fds[]
+
 	int		start_fw_server();
 	int		stop_fw_server();
 	int		request_fw_check();
+
 	int		print_params(uavcan::protocol::param::GetSet::Response &resp);
 	int		get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
 	void 		update_params();
-	void		set_setget_response(uavcan::protocol::param::GetSet::Response *resp)
-	{
-		_setget_response = resp;
-	}
-	void		free_setget_response(void)
-	{
-		_setget_response = nullptr;
-	}
 
-	int			_task = -1;			///< handle to the OS task
-	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
-	volatile eServerAction	_fw_server_action;
-	int			 _fw_server_status;
-	int			_armed_sub = -1;		///< uORB subscription of the arming status
-	actuator_armed_s	_armed = {};			///< the arming request of the system
-	bool			_is_armed = false;		///< the arming status of the actuators on the bus
+	void		set_setget_response(uavcan::protocol::param::GetSet::Response *resp) { _setget_response = resp; }
+	void		free_setget_response(void) { _setget_response = nullptr; }
 
-	int			_test_motor_sub = -1;   ///< uORB subscription of the test_motor status
-	test_motor_s		_test_motor = {};
-	bool			_test_in_progress = false;
+	int			_task{-1};			///< handle to the OS task
+	bool			_task_should_exit{false};	///< flag to indicate to tear down the CAN driver
+	volatile eServerAction	_fw_server_action{None};
+	int			 _fw_server_status{-1};
 
-	unsigned		_output_count = 0;		///< number of actuators currently available
+	bool			_is_armed{false};		///< the arming status of the actuators on the bus
+
+	test_motor_s		_test_motor{};
+	bool			_test_in_progress{false};
+
+	unsigned		_output_count{0};		///< number of actuators currently available
 
 	static UavcanNode	*_instance;			///< singleton pointer
 
 	uavcan_node::Allocator	 _pool_allocator;
 
 	uavcan::Node<>			_node;				///< library instance
-	pthread_mutex_t			_node_mutex;
+	pthread_mutex_t			_node_mutex{};
 	px4_sem_t			_server_command_sem;
 	UavcanEscController		_esc_controller;
 	UavcanHardpointController	_hardpoint_controller;
@@ -191,44 +188,52 @@ private:
 
 	List<IUavcanSensorBridge *>	_sensor_bridges;		///< List of active sensor bridges
 
-	MixerGroup			*_mixers = nullptr;
-	ITxQueueInjector		*_tx_injector;
-	uint32_t			_groups_required = 0;
-	uint32_t			_groups_subscribed = 0;
-	int				_control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
-	actuator_controls_s		_controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
-	orb_id_t			_control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
-	pollfd				_poll_fds[UAVCAN_NUM_POLL_FDS] = {};
-	unsigned			_poll_fds_num = 0;
-	int32_t 			_idle_throttle_when_armed = 0;
+	MixerGroup			*_mixers{nullptr};
+	ITxQueueInjector		*_tx_injector{nullptr};
 
-	int				_actuator_direct_sub = -1;   ///< uORB subscription of the actuator_direct topic
-	uint8_t				_actuator_direct_poll_fd_num = 0;
-	actuator_direct_s		_actuator_direct = {};
+	uint32_t			_groups_required{0};
+	uint32_t			_groups_subscribed{0};
+	int				_control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {};
+	actuator_controls_s		_controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {};
+	orb_id_t			_control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {};
+	pollfd				_poll_fds[UAVCAN_NUM_POLL_FDS] {};
+	unsigned			_poll_fds_num{0};
+	int32_t 			_idle_throttle_when_armed{0};
 
-	actuator_outputs_s		_outputs = {};
+	uORB::Subscription		_armed_sub{ORB_ID(actuator_armed)};
+	uORB::Subscription		_parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription		_test_motor_sub{ORB_ID(test_motor)};
 
-	perf_counter_t			_perf_control_latency;
+	int				_actuator_direct_sub{-1};   ///< uORB subscription of the actuator_direct topic
+	uint8_t				_actuator_direct_poll_fd_num{0};
 
-	Mixer::Airmode 			_airmode = Mixer::Airmode::disabled;
-	float 					_thr_mdl_factor = 0.0f;
+
+	perf_counter_t			_cycle_perf;
+	perf_counter_t			_interval_perf;
+	perf_counter_t			_control_latency_perf;
+
+	Mixer::Airmode 			_airmode{Mixer::Airmode::disabled};
+	float 				_thr_mdl_factor{0.0f};
 
 	// index into _poll_fds for each _control_subs handle
-	uint8_t				_poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
+	uint8_t				_poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] {};
 
 	void handle_time_sync(const uavcan::TimerEvent &);
 
 	typedef uavcan::MethodBinder<UavcanNode *, void (UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback;
 	uavcan::TimerEventForwarder<TimerCallback> _master_timer;
 
-	bool _callback_success;
-	uavcan::protocol::param::GetSet::Response *_setget_response;
+	bool				_callback_success{false};
+
+	uavcan::protocol::param::GetSet::Response *_setget_response{nullptr};
+
 	typedef uavcan::MethodBinder<UavcanNode *,
 		void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
 	typedef uavcan::MethodBinder<UavcanNode *,
 		void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
 	typedef uavcan::MethodBinder<UavcanNode *,
 		void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
+
 	void cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
 	void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
 	void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);

src/drivers/uavcan/uavcan_servers.hpp

@@ -35,9 +35,11 @@
 
 #include <px4_config.h>
 #include "uavcan_driver.hpp"
-#include <drivers/device/device.h>
+#include <drivers/device/Device.hpp>
 #include <lib/perf/perf_counter.h>
+#include <uORB/Publication.hpp>
 #include <uORB/PublicationQueued.hpp>
+#include <uORB/topics/uavcan_parameter_value.h>
 #include <uORB/topics/vehicle_command_ack.h>
 
 #include <uavcan/node/sub_node.hpp>
@@ -110,9 +112,9 @@ public:
 	bool guessIfAllDynamicNodesAreAllocated() { return _server_instance.guessIfAllDynamicNodesAreAllocated(); }
 
 private:
-	pthread_t         _subnode_thread;
-	pthread_mutex_t   _subnode_mutex;
-	volatile bool     _subnode_thread_should_exit = false;
+	pthread_t         _subnode_thread{-1};
+	pthread_mutex_t   _subnode_mutex{};
+	volatile bool     _subnode_thread_should_exit{false};
 
 	int		init();
 
@@ -161,7 +163,7 @@ private:
 	bool _cmd_in_progress = false;
 
 	// uORB topic handle for MAVLink parameter responses
-	orb_advert_t _param_response_pub = nullptr;
+	uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
 	uORB::PublicationQueued<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
 
 	typedef uavcan::MethodBinder<UavcanServers *,
@@ -171,6 +173,7 @@ private:
 		ExecuteOpcodeCallback;
 	typedef uavcan::MethodBinder<UavcanServers *,
 		void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
+
 	void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
 	void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
 	void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);

src/lib/drivers/device/Device.hpp

@@ -140,12 +140,23 @@ public:
 		DeviceBusType_SIMULATION = 4,
 	};
 
-	/**
-	 * Return the bus ID the device is connected to.
-	 *
-	 * @return The bus ID
+	/*
+	  broken out device elements. The bitfields are used to keep
+	  the overall value small enough to fit in a float accurately,
+	  which makes it possible to transport over the MAVLink
+	  parameter protocol without loss of information.
 	 */
-	uint8_t get_device_bus() const { return _device_id.devid_s.bus; }
+	struct DeviceStructure {
+		DeviceBusType bus_type : 3;
+		uint8_t bus: 5;    // which instance of the bus type
+		uint8_t address;   // address on the bus (eg. I2C address)
+		uint8_t devtype;   // device class specific device type
+	};
+
+	union DeviceId {
+		struct DeviceStructure devid_s;
+		uint32_t devid;
+	};
 
 	uint32_t get_device_id() const { return _device_id.devid; }
 
@@ -154,7 +165,8 @@ public:
 	 *
 	 * @return The bus type
 	 */
-	DeviceBusType get_device_bus_type() const { return _device_id.devid_s.bus_type; }
+	DeviceBusType	get_device_bus_type() const { return _device_id.devid_s.bus_type; }
+	void		set_device_bus_type(DeviceBusType bus_type) { _device_id.devid_s.bus_type = bus_type; }
 
 	static const char *get_device_bus_string(DeviceBusType bus)
 	{
@@ -177,41 +189,29 @@ public:
 		}
 	}
 
+	/**
+	 * Return the bus ID the device is connected to.
+	 *
+	 * @return The bus ID
+	 */
+	uint8_t get_device_bus() const { return _device_id.devid_s.bus; }
+	void	set_device_bus(uint8_t bus) { _device_id.devid_s.bus = bus; }
+
 	/**
 	 * Return the bus address of the device.
 	 *
 	 * @return The bus address
 	 */
-	uint8_t get_device_address() const { return _device_id.devid_s.address; }
-
-	void set_device_address(int address) { _device_id.devid_s.address = address; }
+	uint8_t	get_device_address() const { return _device_id.devid_s.address; }
+	void	set_device_address(int address) { _device_id.devid_s.address = address; }
 
 	/**
-	 * Set the device type
+	 * Return the device type
 	 *
 	 * @return The device type
 	 */
-	void set_device_type(uint8_t devtype) { _device_id.devid_s.devtype = devtype; }
-
-	virtual bool external() { return false; }
-
-	/*
-	  broken out device elements. The bitfields are used to keep
-	  the overall value small enough to fit in a float accurately,
-	  which makes it possible to transport over the MAVLink
-	  parameter protocol without loss of information.
-	 */
-	struct DeviceStructure {
-		enum DeviceBusType bus_type : 3;
-		uint8_t bus: 5;    // which instance of the bus type
-		uint8_t address;   // address on the bus (eg. I2C address)
-		uint8_t devtype;   // device class specific device type
-	};
-
-	union DeviceId {
-		struct DeviceStructure devid_s;
-		uint32_t devid;
-	};
+	uint8_t	get_device_type() const { return _device_id.devid_s.devtype; }
+	void	set_device_type(uint8_t devtype) { _device_id.devid_s.devtype = devtype; }
 
 	/**
 	 * Print decoded device id string to a buffer.
@@ -234,30 +234,34 @@ public:
 		return num_written;
 	}
 
+	virtual bool external() const { return false; }
+
 protected:
-	union DeviceId	_device_id {};            /**< device identifier information */
+	union DeviceId	_device_id {};            	/**< device identifier information */
 
 	const char	*_name{nullptr};		/**< driver name */
 	bool		_debug_enabled{false};		/**< if true, debug messages are printed */
 
 	explicit Device(const char *name) : _name(name)
 	{
-		/* setup a default device ID. When bus_type is UNKNOWN the
-		   other fields are invalid */
-		_device_id.devid = 0;
-		_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
-		_device_id.devid_s.bus = 0;
-		_device_id.devid_s.address = 0;
-		_device_id.devid_s.devtype = 0;
+		set_device_bus_type(DeviceBusType_UNKNOWN);
+	}
+
+	Device(const char *name, DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype = 0)
+		: _name(name)
+	{
+		set_device_bus_type(bus_type);
+		set_device_bus(bus);
+		set_device_address(address);
+		set_device_type(devtype);
 	}
 
 	Device(DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype = 0)
 	{
-		_device_id.devid = 0;
-		_device_id.devid_s.bus_type = bus_type;
-		_device_id.devid_s.bus = bus;
-		_device_id.devid_s.address = address;
-		_device_id.devid_s.devtype = devtype;
+		set_device_bus_type(bus_type);
+		set_device_bus(bus);
+		set_device_address(address);
+		set_device_type(devtype);
 	}
 
 };

src/lib/drivers/device/nuttx/I2C.hpp

@@ -65,7 +65,7 @@ public:
 	I2C(I2C &&) = delete;
 	I2C &operator=(I2C &&) = delete;
 
-	virtual int	init();
+	virtual int	init() override;
 
 	static int	set_bus_clock(unsigned bus, unsigned clock_hz);
 
@@ -109,7 +109,7 @@ protected:
 	 */
 	int		transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
 
-	bool		external() { return px4_i2c_bus_external(_device_id.devid_s.bus); }
+	virtual bool	external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); }
 
 private:
 	uint32_t		_frequency{0};

src/lib/drivers/device/nuttx/SPI.hpp

@@ -80,7 +80,7 @@ protected:
 		LOCK_NONE		/**< perform no locking, only safe if the bus is entirely private */
 	};
 
-	virtual int	init();
+	virtual int	init() override;
 
 	/**
 	 * Check for the presence of the device on the bus.
@@ -164,7 +164,7 @@ protected:
 
 	int	_transferhword(uint16_t *send, uint16_t *recv, unsigned len);
 
-	bool	external() { return px4_spi_bus_external(get_device_bus()); }
+	virtual bool	external() const override { return px4_spi_bus_external(get_device_bus()); }
 
 };
 

src/lib/drivers/device/posix/I2C.hpp

@@ -66,7 +66,7 @@ public:
 	I2C(I2C &&) = delete;
 	I2C &operator=(I2C &&) = delete;
 
-	virtual int	init();
+	virtual int	init() override;
 
 protected:
 	/**
@@ -106,7 +106,7 @@ protected:
 	 */
 	int		transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
 
-	bool		external() { return px4_i2c_bus_external(_device_id.devid_s.bus); }
+	virtual bool	external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); }
 
 private:
 	int			_fd{-1};

src/lib/drivers/device/posix/SPI.hpp

@@ -95,7 +95,7 @@ protected:
 		LOCK_NONE		/**< perform no locking, only safe if the bus is entirely private */
 	};
 
-	virtual int	init();
+	virtual int	init() override;
 
 	/**
 	 * Check for the presence of the device on the bus.
@@ -179,7 +179,7 @@ protected:
 
 	int	_transferhword(uint16_t *send, uint16_t *recv, unsigned len);
 
-	bool	external() { return px4_spi_bus_external(get_device_bus()); }
+	virtual bool	external() const override { return px4_spi_bus_external(get_device_bus()); }
 
 };
 

src/lib/parameters/parameters_injected.xml

@@ -92,7 +92,7 @@
       <min>0</min>
       <max>4000</max>
     </parameter>
-      <parameter default="0.0" name="mot_ls" type="FLOAT">
+    <parameter default="0.0" name="mot_ls" type="FLOAT">
       <short_desc>READ ONLY: Motor inductance in henries.</short_desc>
       <long_desc>READ ONLY: Motor inductance in henries. This is measured on start-up.</long_desc>
       <unit>henries</unit>
@@ -135,4 +135,66 @@
       <min>0</min>
     </parameter>
   </group>
-</parameters>
+  <group name="UAVCAN GNSS" no_code_generation="true">
+    <parameter name="gnss.warn_dimens" default="0" type="INT32">
+      <short_desc>device health warning</short_desc>
+      <long_desc>Set the device health to Warning if the dimensionality of
+              the GNSS solution is less than this value. 3 for the full (3D)
+              solution, 2 for planar (2D) solution, 1 for time-only solution,
+              0 disables the feature.
+      </long_desc>
+      <min>0</min>
+      <max>3</max>
+      <values>
+        <value code="0">disables the feature</value>
+        <value code="1">time-only solution</value>
+        <value code="2">planar (2D) solution</value>
+        <value code="3">full (3D) solution</value>
+      </values>
+    </parameter>
+    <parameter name="gnss.warn_sats" default="0" type="INT32">
+      <short_desc></short_desc>
+      <long_desc>Set the device health to Warning if the number of satellites
+        used in the GNSS solution is below this threshold. Zero
+        disables the feature
+      </long_desc>
+    </parameter>
+    <parameter name="gnss.dyn_model" default="2" type="INT32">
+      <short_desc>GNSS dynamic model</short_desc>
+      <long_desc>Dynamic model used in the GNSS positioning engine. 0 –
+        Automotive, 1 – Sea, 2 – Airborne.
+      </long_desc>
+      <min>0</min>
+      <max>2</max>
+      <values>
+        <value code="0">Automotive</value>
+        <value code="1">Sea</value>
+        <value code="2">Airborne</value>
+      </values>
+    </parameter>
+    <parameter name="gnss.old_fix_msg" default="1" type="INT32">
+      <short_desc>Broadcast old GNSS fix message</short_desc>
+      <long_desc>Broadcast the old (deprecated) GNSS fix message
+        uavcan.equipment.gnss.Fix alongside the new alternative
+        uavcan.equipment.gnss.Fix2. It is recommended to
+        disable this feature to reduce the CAN bus traffic.
+      </long_desc>
+      <min>0</min>
+      <max>1</max>
+      <values>
+        <value code="0">Fix2</value>
+        <value code="1">Fix and Fix2</value>
+      </values>
+    </parameter>
+    <parameter name="uavcan.pubp-pres" default="0" type="INT32">
+      <short_desc></short_desc>
+      <long_desc>Set the device health to Warning if the number of satellites
+        used in the GNSS solution is below this threshold. Zero
+        disables the feature
+      </long_desc>
+      <unit>microseconds</unit>
+      <min>0</min>
+      <max>1000000</max>
+    </parameter>
+  </group>
+</parameters>