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) {
+	actuator_outputs.timestamp = hrt_absolute_time();
-		orb_publish(ORB_ID(actuator_outputs), _actuator_outputs_pub, &actuator_outputs);
+	_actuator_outputs_pub.publish(actuator_outputs);
-
-	} else {
-		int instance;
-		_actuator_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &actuator_outputs,
-					&instance, ORB_PRIO_DEFAULT);
-	}
-
 }
 
-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) {
+	_esc_status_pub.publish(_esc_status);
-		(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
-
-	} else {
-		_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_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 <lib/perf/perf_counter.h>
-#include <uORB/topics/esc_status.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>&)>
+		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;
-		StatusCbBinder;
 
-	typedef uavcan::MethodBinder<UavcanEscController *, void (UavcanEscController::*)(const uavcan::TimerEvent &)>
+	typedef uavcan::MethodBinder<UavcanEscController *,
-	TimerCbBinder;
+		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;
+	uint32_t 			_armed_mask{0};
-	uint8_t				_max_number_of_nonzero_outputs = 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_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_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
-	 * Broadcast command at MAX_RATE_HZ
+	_uavcan_pub_raw_cmd.broadcast(_cmd);
-	 */
-	(void)_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",
+			PX4_ERR("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
-			      getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
+				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),
+	_sub_air_temperature_data(node)
-	_reports(2, sizeof(sensor_baro_s))
 { }
 
 int UavcanBarometerBridge::init()
@@ -73,49 +72,15 @@ int UavcanBarometerBridge::init()
 	return 0;
 }
 
-ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen)
+void
-{
+UavcanBarometerBridge::air_temperature_sub_cb(const
-	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
 		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)),
+	_orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb))
-	_report_pub(nullptr)
 {
 }
 
-UavcanGnssBridge::~UavcanGnssBridge()
+int
-{
+UavcanGnssBridge::init()
-	(void) orb_unsubscribe(_orb_sub_gnss);
-}
-
-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(
+	_orb_to_uavcan_pub_timer.startPeriodic(uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ));
-		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.
+		break;
-	// 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
 	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 (&pos_cov)[9], const float (&vel_cov)[9],
-				    const float (&vel_cov)[9],
+				    const bool valid_pos_cov, const bool valid_vel_cov)
-				    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: {
+	case FixType::GNSS_TIME_STANDARD_UTC:
-			report.time_utc_usec = gnss_ts_usec;
+		report.time_utc_usec = gnss_ts_usec;
-			break;
+		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: {
+		break;
-			if (msg.num_leap_seconds > 0) {
-				report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds + 9;
-			}
 
-			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: {
+		break;
-			if (msg.num_leap_seconds > 0) {
-				report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds - 10;
-			}
 
-			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;
+		timespec ts{};
-		memset(&ts, 0, sizeof(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
+	if (hrt_elapsed_time(&_last_gnss_auxiliary_timestamp) < 2_s) {
-	// Relevant discussion: https://github.com/PX4/Firmware/issues/5153
+		report.hdop = _last_gnss_auxiliary_hdop;
-	report.hdop = msg.pdop;
+		report.vdop = _last_gnss_auxiliary_vdop;
-	report.vdop = msg.pdop;
+
+	} 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;
+	_gps_pub.publish(report);
-	orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_pub, &report, &gps_orb_instance,
-			 ORB_PRIO_HIGH);
 
 	// 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) {
+	vehicle_gps_position_s orb_msg{};
-		// ORB subscription stops working if this is relocated into init()
-		_orb_sub_gnss = orb_subscribe(ORB_ID(vehicle_gps_position));
 
-		if (_orb_sub_gnss < 0) {
+	if (!_orb_sub_gnss.update(&orb_msg)) {
-			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);
 		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 (&pos_cov)[9], const float (&vel_cov)[9],
-			  const float (&vel_cov)[9],
+			  const bool valid_pos_cov, const bool valid_vel_cov);
-			  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)
+int
-{
+UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
-	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)
 {
 	switch (cmd) {
 
@@ -127,9 +111,33 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 	}
 }
 
-void UavcanMagnetometerBridge::mag_sub_cb(const
+void
-		uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
+UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
-		&msg)
+				     &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 = {};
+	uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength2, Mag2CbBinder> _sub_mag2;
-	mag_report _report =  {};
+
+	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.cpp

@@ -55,7 +55,6 @@
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>
 
-#include <uORB/Subscription.hpp>
 #include <uORB/topics/esc_status.h>
 #include <uORB/topics/parameter_update.h>
 
@@ -77,23 +76,20 @@
  * UavcanNode
  */
 UavcanNode *UavcanNode::_instance;
+
 UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
 	CDev(UAVCAN_DEVICE_PATH),
 	_node(can_driver, system_clock, _pool_allocator),
-	_node_mutex(),
 	_esc_controller(_node),
 	_hardpoint_controller(_node),
 	_time_sync_master(_node),
 	_time_sync_slave(_node),
 	_node_status_monitor(_node),
-	_perf_control_latency(perf_alloc(PC_ELAPSED, "uavcan control latency")),
+	_cycle_perf(perf_alloc(PC_ELAPSED, "uavcan: cycle time")),
-	_master_timer(_node),
+	_interval_perf(perf_alloc(PC_INTERVAL, "uavcan: cycle interval")),
-	_setget_response(0)
+	_control_latency_perf(perf_alloc(PC_ELAPSED, "uavcan: control latency")),
+	_master_timer(_node)
 {
-	_task_should_exit = false;
-	_fw_server_action = None;
-	_fw_server_status = -1;
-	_tx_injector = nullptr;
 	_control_topics[0] = ORB_ID(actuator_controls_0);
 	_control_topics[1] = ORB_ID(actuator_controls_1);
 	_control_topics[2] = ORB_ID(actuator_controls_2);
@@ -121,7 +117,6 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 
 UavcanNode::~UavcanNode()
 {
-
 	fw_server(Stop);
 
 	if (_task != -1) {
@@ -144,8 +139,6 @@ UavcanNode::~UavcanNode()
 		} while (_task != -1);
 	}
 
-	(void)orb_unsubscribe(_armed_sub);
-	(void)orb_unsubscribe(_test_motor_sub);
 	(void)orb_unsubscribe(_actuator_direct_sub);
 
 	// Removing the sensor bridges
@@ -161,10 +154,13 @@ UavcanNode::~UavcanNode()
 		delete _mixers;
 	}
 
-	perf_free(_perf_control_latency);
+	perf_free(_cycle_perf);
+	perf_free(_interval_perf);
+	perf_free(_control_latency_perf);
 }
 
-int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
+int
+UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
 {
 	int rv = -1;
 
@@ -185,7 +181,8 @@ int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
 	return rv;
 }
 
-int UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
+int
+UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
 {
 	if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
 		return std::printf("name: %s %lld\n", resp.name.c_str(),
@@ -207,7 +204,8 @@ int UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
 	return -1;
 }
 
-void UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
+void
+UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
 {
 	uavcan::protocol::param::ExecuteOpcode::Response resp;
 	_callback_success = result.isSuccessful();
@@ -215,7 +213,8 @@ void UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::par
 	_callback_success &= resp.ok;
 }
 
-int  UavcanNode::save_params(int remote_node_id)
+int
+UavcanNode::save_params(int remote_node_id)
 {
 	uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
 	opcode_req.opcode = opcode_req.OPCODE_SAVE;
@@ -238,7 +237,8 @@ int  UavcanNode::save_params(int remote_node_id)
 	return 0;
 }
 
-void UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
+void
+UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
 {
 	uavcan::protocol::RestartNode::Response resp;
 	_callback_success = result.isSuccessful();
@@ -246,7 +246,8 @@ void UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::Re
 	_callback_success &= resp.ok;
 }
 
-int  UavcanNode::reset_node(int remote_node_id)
+int
+UavcanNode::reset_node(int remote_node_id)
 {
 	uavcan::protocol::RestartNode::Request restart_req;
 	restart_req.magic_number = restart_req.MAGIC_NUMBER;
@@ -269,7 +270,8 @@ int  UavcanNode::reset_node(int remote_node_id)
 	return 0;
 }
 
-int  UavcanNode::list_params(int remote_node_id)
+int
+UavcanNode::list_params(int remote_node_id)
 {
 	int rv = 0;
 	int index = 0;
@@ -299,14 +301,15 @@ int  UavcanNode::list_params(int remote_node_id)
 	return rv;
 }
 
-
+void
-void UavcanNode::cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
+UavcanNode::cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
 {
 	_callback_success = result.isSuccessful();
 	*_setget_response = result.getResponse();
 }
 
-int  UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::protocol::param::GetSet::Request &req)
+int
+UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::protocol::param::GetSet::Request &req)
 {
 	if (name != nullptr) {
 		req.name = name;
@@ -331,7 +334,8 @@ int  UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::pro
 	return call_res;
 }
 
-int  UavcanNode::set_param(int remote_node_id, const char *name, char *value)
+int
+UavcanNode::set_param(int remote_node_id, const char *name, char *value)
 {
 	uavcan::protocol::param::GetSet::Request req;
 	uavcan::protocol::param::GetSet::Response resp;
@@ -398,7 +402,8 @@ int  UavcanNode::set_param(int remote_node_id, const char *name, char *value)
 	return rv;
 }
 
-int  UavcanNode::get_param(int remote_node_id, const char *name)
+int
+UavcanNode::get_param(int remote_node_id, const char *name)
 {
 	uavcan::protocol::param::GetSet::Request req;
 	uavcan::protocol::param::GetSet::Response resp;
@@ -418,7 +423,8 @@ int  UavcanNode::get_param(int remote_node_id, const char *name)
 	return rv;
 }
 
-void UavcanNode::update_params()
+void
+UavcanNode::update_params()
 {
 	// multicopter air-mode
 	param_t param_handle = param_find("MC_AIRMODE");
@@ -434,7 +440,8 @@ void UavcanNode::update_params()
 	}
 }
 
-int UavcanNode::start_fw_server()
+int
+UavcanNode::start_fw_server()
 {
 	int rv = -1;
 	_fw_server_action = Busy;
@@ -460,11 +467,12 @@ int UavcanNode::start_fw_server()
 	return rv;
 }
 
-int UavcanNode::request_fw_check()
+int
+UavcanNode::request_fw_check()
 {
 	int rv = -1;
 	_fw_server_action = Busy;
-	UavcanServers   *_servers  = UavcanServers::instance();
+	UavcanServers *_servers  = UavcanServers::instance();
 
 	if (_servers != nullptr) {
 		_servers->requestCheckAllNodesFirmwareAndUpdate();
@@ -474,14 +482,14 @@ int UavcanNode::request_fw_check()
 	_fw_server_action = None;
 	px4_sem_post(&_server_command_sem);
 	return rv;
-
 }
 
-int UavcanNode::stop_fw_server()
+int
+UavcanNode::stop_fw_server()
 {
 	int rv = -1;
 	_fw_server_action = Busy;
-	UavcanServers   *_servers  = UavcanServers::instance();
+	UavcanServers *_servers = UavcanServers::instance();
 
 	if (_servers != nullptr) {
 		/*
@@ -500,8 +508,8 @@ int UavcanNode::stop_fw_server()
 	return rv;
 }
 
-
+int
-int UavcanNode::fw_server(eServerAction action)
+UavcanNode::fw_server(eServerAction action)
 {
 	int rv = -EAGAIN;
 
@@ -525,8 +533,8 @@ int UavcanNode::fw_server(eServerAction action)
 	return rv;
 }
 
-
+int
-int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
+UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 {
 	if (_instance != nullptr) {
 		PX4_WARN("Already started");
@@ -580,8 +588,12 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 	 * Start the task. Normally it should never exit.
 	 */
 	static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
-	_instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
+	_instance->_task = px4_task_spawn_cmd("uavcan",
-					      static_cast<main_t>(run_trampoline), nullptr);
+					      SCHED_DEFAULT,
+					      SCHED_PRIORITY_ACTUATOR_OUTPUTS,
+					      StackSize,
+					      static_cast<main_t>(run_trampoline),
+					      nullptr);
 
 	if (_instance->_task < 0) {
 		PX4_ERR("start failed: %d", errno);
@@ -591,7 +603,8 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 	return OK;
 }
 
-void UavcanNode::fill_node_info()
+void
+UavcanNode::fill_node_info()
 {
 	/* software version */
 	uavcan::protocol::SoftwareVersion swver;
@@ -614,13 +627,11 @@ void UavcanNode::fill_node_info()
 	_node.setHardwareVersion(hwver);
 }
 
-
+int
-int UavcanNode::init(uavcan::NodeID node_id)
+UavcanNode::init(uavcan::NodeID node_id)
 {
-	int ret = -1;
-
 	// Do regular cdev init
-	ret = CDev::init();
+	int ret = CDev::init();
 
 	if (ret != OK) {
 		return ret;
@@ -640,7 +651,7 @@ int UavcanNode::init(uavcan::NodeID node_id)
 	}
 
 	{
-		(void) param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed);
+		param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed);
 		_esc_controller.enable_idle_throttle_when_armed(_idle_throttle_when_armed > 0);
 	}
 
@@ -668,7 +679,8 @@ int UavcanNode::init(uavcan::NodeID node_id)
 	return _node.start();
 }
 
-void UavcanNode::node_spin_once()
+void
+UavcanNode::node_spin_once()
 {
 	const int spin_res = _node.spinOnce();
 
@@ -686,7 +698,8 @@ void UavcanNode::node_spin_once()
   add a fd to the list of polled events. This assumes you want
   POLLIN for now.
  */
-int UavcanNode::add_poll_fd(int fd)
+int
+UavcanNode::add_poll_fd(int fd)
 {
 	int ret = _poll_fds_num;
 
@@ -698,13 +711,13 @@ int UavcanNode::add_poll_fd(int fd)
 	_poll_fds[_poll_fds_num].fd	= fd;
 	_poll_fds[_poll_fds_num].events	= POLLIN;
 	_poll_fds_num += 1;
+
 	return ret;
 }
 
-
+void
-void UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
+UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
 {
-
 	/*
 	 * Check whether there are higher priority masters in the network.
 	 * If there are, we need to activate the local slave in order to sync with them.
@@ -742,25 +755,19 @@ void UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
 	_time_sync_master.publish();
 }
 
-
+int
-
+UavcanNode::run()
-int UavcanNode::run()
 {
-	(void)pthread_mutex_lock(&_node_mutex);
+	pthread_mutex_lock(&_node_mutex);
 
 	// XXX figure out the output count
 	_output_count = 2;
 
-	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
 	_actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct));
 
-	memset(&_outputs, 0, sizeof(_outputs));
+	actuator_outputs_s outputs{};
-
-	/*
-	 * Set up the time synchronization
-	 */
 
+	// Set up the time synchronization
 	const int slave_init_res = _time_sync_slave.start();
 
 	if (slave_init_res < 0) {
@@ -811,15 +818,16 @@ int UavcanNode::run()
 
 	update_params();
 
-	uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
-
 	while (!_task_should_exit) {
 
+		perf_begin(_cycle_perf);
+		perf_count(_interval_perf);
+
 		// check for parameter updates
-		if (parameter_update_sub.updated()) {
+		if (_parameter_update_sub.updated()) {
 			// clear update
 			parameter_update_s pupdate;
-			parameter_update_sub.copy(&pupdate);
+			_parameter_update_sub.copy(&pupdate);
 
 			// update parameters from storage
 			update_params();
@@ -878,30 +886,31 @@ int UavcanNode::run()
 				}
 			}
 
-			/*
+			// see if we have any direct actuator updates
-			  see if we have any direct actuator updates
+			actuator_direct_s actuator_direct{};
-			 */
+
 			if (_actuator_direct_sub != -1 &&
 			    (_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
-			    orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
+			    orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &actuator_direct) == OK &&
 			    !_test_in_progress) {
-				if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
+
-					_actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
+				if (actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
+					actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
 				}
 
-				memcpy(&_outputs.output[0], &_actuator_direct.values[0],
+				memcpy(&outputs.output[0], &actuator_direct.values[0], actuator_direct.nvalues * sizeof(float));
-				       _actuator_direct.nvalues * sizeof(float));
+				outputs.noutputs = actuator_direct.nvalues;
-				_outputs.noutputs = _actuator_direct.nvalues;
 				new_output = true;
 			}
 
 			// can we mix?
 			if (_test_in_progress) {
-				memset(&_outputs, 0, sizeof(_outputs));
+
+				memset(&outputs, 0, sizeof(outputs));
 
 				if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
-					_outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f;
+					outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f;
-					_outputs.noutputs = _test_motor.motor_number + 1;
+					outputs.noutputs = _test_motor.motor_number + 1;
 				}
 
 				new_output = true;
@@ -916,7 +925,7 @@ int UavcanNode::run()
 				_mixers->set_airmode(_airmode);
 
 				// Do mixing
-				_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max);
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
 
 				new_output = true;
 			}
@@ -924,31 +933,25 @@ int UavcanNode::run()
 
 		if (new_output) {
 			// iterate actuators, checking for valid values
-			for (uint8_t i = 0; i < _outputs.noutputs; i++) {
+			for (uint8_t i = 0; i < outputs.noutputs; i++) {
+
 				// last resort: catch NaN, INF and out-of-band errors
-				if (!isfinite(_outputs.output[i])) {
+				if (!isfinite(outputs.output[i])) {
 					/*
 					 * Value is NaN, INF or out of band - set to the minimum value.
 					 * This will be clearly visible on the servo status and will limit the risk of accidentally
 					 * spinning motors. It would be deadly in flight.
 					 */
-					_outputs.output[i] = -1.0f;
+					outputs.output[i] = -1.0f;
 				}
 
-				// never go below min
+				// never go below min or max
-				if (_outputs.output[i] < -1.0f) {
+				outputs.output[i] = math::constrain(outputs.output[i], -1.0f, 1.0f);
-					_outputs.output[i] = -1.0f;
-				}
-
-				// never go above max
-				if (_outputs.output[i] > 1.0f) {
-					_outputs.output[i] = 1.0f;
-				}
 			}
 
 			// Output to the bus
-			_esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
+			_esc_controller.update_outputs(outputs.output, outputs.noutputs);
-			_outputs.timestamp = hrt_absolute_time();
+			outputs.timestamp = hrt_absolute_time();
 
 			// use first valid timestamp_sample for latency tracking
 			for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
@@ -956,19 +959,15 @@ int UavcanNode::run()
 				const hrt_abstime &timestamp_sample = _controls[i].timestamp_sample;
 
 				if (required && (timestamp_sample > 0)) {
-					perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample);
+					perf_set_elapsed(_control_latency_perf, outputs.timestamp - timestamp_sample);
 					break;
 				}
 			}
 		}
 
-
 		// Check motor test state
-		bool updated = false;
+		if (_test_motor_sub.updated()) {
-		orb_check(_test_motor_sub, &updated);
+			_test_motor_sub.copy(&_test_motor);
-
-		if (updated) {
-			orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
 
 			// Update the test status and check that we're not locked down
 			_test_in_progress = (_test_motor.action == test_motor_s::ACTION_RUN);
@@ -976,24 +975,25 @@ int UavcanNode::run()
 		}
 
 		// Check arming state
-		orb_check(_armed_sub, &updated);
+		if (_armed_sub.updated()) {
-
+			actuator_armed_s armed{};
-		if (updated) {
+			_armed_sub.copy(&armed);
-			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
 			// Update the armed status and check that we're not locked down and motor
 			// test is not running
-			bool set_armed = _armed.armed && !_armed.lockdown && !_armed.manual_lockdown && !_test_in_progress;
+			bool set_armed = (armed.armed && !armed.lockdown && !armed.manual_lockdown && !_test_in_progress);
 
 			arm_actuators(set_armed);
 
-			if (_armed.soft_stop) {
+			if (armed.soft_stop) {
 				_esc_controller.enable_idle_throttle_when_armed(false);
 
 			} else {
 				_esc_controller.enable_idle_throttle_when_armed(_idle_throttle_when_armed > 0);
 			}
 		}
+
+		perf_end(_cycle_perf);
 	}
 
 	(void)::close(busevent_fd);
@@ -1024,7 +1024,7 @@ UavcanNode::teardown()
 		}
 	}
 
-	return (_armed_sub >= 0) ? orb_unsubscribe(_armed_sub) : 0;
+	return 0;
 }
 
 int
@@ -1032,6 +1032,7 @@ UavcanNode::arm_actuators(bool arm)
 {
 	_is_armed = arm;
 	_esc_controller.arm_all_escs(arm);
+
 	return OK;
 }
 
@@ -1186,6 +1187,8 @@ UavcanNode::print_info()
 	printf("\tReserved:  %u blocks\n", _pool_allocator.getNumReservedBlocks());
 	printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks());
 
+	printf("\n");
+
 	// UAVCAN node perfcounters
 	printf("UAVCAN node status:\n");
 	printf("\tInternal failures: %llu\n", _node.getInternalFailureCount());
@@ -1193,6 +1196,8 @@ UavcanNode::print_info()
 	printf("\tRX transfers:      %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount());
 	printf("\tTX transfers:      %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount());
 
+	printf("\n");
+
 	// CAN driver status
 	for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
 		printf("CAN%u status:\n", unsigned(i + 1));
@@ -1206,28 +1211,14 @@ UavcanNode::print_info()
 		printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx);
 	}
 
+	printf("\n");
+
 	// ESC mixer status
-	printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
+	printf("ESC actuators control groups: sub: %X / req: %X / fds: %u\n",
 	       (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
 	printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
 
-	if (_outputs.noutputs != 0) {
+	printf("\n");
-		PX4_INFO("ESC output: ");
-
-		for (uint8_t i = 0; i < _outputs.noutputs; i++) {
-			printf("%d ", (int)(_outputs.output[i] * 1000));
-		}
-
-		printf("\n");
-
-		// ESC status
-		int esc_sub = orb_subscribe(ORB_ID(esc_status));
-		esc_status_s esc = {};
-		orb_copy(ORB_ID(esc_status), esc_sub, &esc);
-		orb_unsubscribe(esc_sub);
-
-		print_message(esc);
-	}
 
 	// Sensor bridges
 	for (const auto &br : _sensor_bridges) {
@@ -1237,33 +1228,38 @@ UavcanNode::print_info()
 	}
 
 	// Printing all nodes that are online
-	std::printf("Online nodes (Node ID, Health, Mode):\n");
+	printf("Online nodes (Node ID, Health, Mode):\n");
 	_node_status_monitor.forEachNode([](uavcan::NodeID nid, uavcan::NodeStatusMonitor::NodeStatus ns) {
-		static constexpr const char *HEALTH[] = {
+		static constexpr const char *HEALTH[] = {"OK", "WARN", "ERR", "CRIT"};
-			"OK", "WARN", "ERR", "CRIT"
+		static constexpr const char *MODES[] = {"OPERAT", "INIT", "MAINT", "SW_UPD", "?", "?", "?", "OFFLN"};
-		};
+		printf("\t% 3d %-10s %-10s\n", int(nid.get()), HEALTH[ns.health], MODES[ns.mode]);
-		static constexpr const char *MODES[] = {
-			"OPERAT", "INIT", "MAINT", "SW_UPD", "?", "?", "?", "OFFLN"
-		};
-		std::printf("\t% 3d %-10s %-10s\n", int(nid.get()), HEALTH[ns.health], MODES[ns.mode]);
 	});
 
+	printf("\n");
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_interval_perf);
+	perf_print_counter(_control_latency_perf);
+
 	(void)pthread_mutex_unlock(&_node_mutex);
 }
 
-void UavcanNode::shrink()
+void
+UavcanNode::shrink()
 {
 	(void)pthread_mutex_lock(&_node_mutex);
 	_pool_allocator.shrink();
 	(void)pthread_mutex_unlock(&_node_mutex);
 }
 
-void UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command)
+void
+UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command)
 {
 	(void)pthread_mutex_lock(&_node_mutex);
 	_hardpoint_controller.set_command(hardpoint_id, command);
 	(void)pthread_mutex_unlock(&_node_mutex);
 }
+
 /*
  * App entry point
  */

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 <lib/drivers/device/device.h>
-#include <perf/perf_counter.h>
+#include <lib/perf/perf_counter.h>
 
-#include <uORB/topics/actuator_controls.h>
+#include <uORB/Subscription.hpp>
-#include <uORB/topics/actuator_outputs.h>
 #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 <uORB/topics/actuator_outputs.h>
-#include "actuators/esc.hpp"
+#include <uORB/topics/parameter_update.h>
-#include "actuators/hardpoint.hpp"
+#include <uORB/topics/test_motor.h>
-#include "sensors/sensor_bridge.hpp"
-
-#include "uavcan_servers.hpp"
-#include "allocator.hpp"
 
 #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
+	void		set_setget_response(uavcan::protocol::param::GetSet::Response *resp) { _setget_response = resp; }
-	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
+	void		free_setget_response(void) { _setget_response = nullptr; }
-	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
 
-	int			_test_motor_sub = -1;   ///< uORB subscription of the test_motor status
+	int			_task{-1};			///< handle to the OS task
-	test_motor_s		_test_motor = {};
+	bool			_task_should_exit{false};	///< flag to indicate to tear down the CAN driver
-	bool			_test_in_progress = false;
+	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;
+	MixerGroup			*_mixers{nullptr};
-	ITxQueueInjector		*_tx_injector;
+	ITxQueueInjector		*_tx_injector{nullptr};
-	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;
 
-	int				_actuator_direct_sub = -1;   ///< uORB subscription of the actuator_direct topic
+	uint32_t			_groups_required{0};
-	uint8_t				_actuator_direct_poll_fd_num = 0;
+	uint32_t			_groups_subscribed{0};
-	actuator_direct_s		_actuator_direct = {};
+	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;
+	bool				_callback_success{false};
-	uavcan::protocol::param::GetSet::Response *_setget_response;
+
+	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.cpp

@@ -60,11 +60,9 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/uavcan_parameter_request.h>
-#include <uORB/topics/uavcan_parameter_value.h>
 
 #include <v2.0/common/mavlink.h>
 
-
 /**
  * @file uavcan_servers.cpp
  *
@@ -78,8 +76,8 @@
  * UavcanNode
  */
 UavcanServers *UavcanServers::_instance;
+
 UavcanServers::UavcanServers(uavcan::INode &main_node) :
-	_subnode_thread(-1),
 	_vdriver(NumIfaces, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
 	_subnode(_vdriver, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator()),
 	_main_node(main_node),
@@ -96,7 +94,6 @@ UavcanServers::UavcanServers(uavcan::INode &main_node) :
 	_enumeration_client(_subnode),
 	_enumeration_getset_client(_subnode),
 	_enumeration_save_client(_subnode)
-
 {
 }
 
@@ -109,17 +106,18 @@ UavcanServers::~UavcanServers()
 	_main_node.getDispatcher().removeRxFrameListener();
 }
 
-int UavcanServers::stop()
+int
+UavcanServers::stop()
 {
 	UavcanServers *server = instance();
 
 	if (server == nullptr) {
-		warnx("Already stopped");
+		PX4_INFO("Already stopped");
 		return -1;
 	}
 
 	if (server->_subnode_thread) {
-		warnx("stopping fw srv thread...");
+		PX4_INFO("stopping fw srv thread...");
 		server->_subnode_thread_should_exit = true;
 		(void)pthread_join(server->_subnode_thread, NULL);
 	}
@@ -132,10 +130,11 @@ int UavcanServers::stop()
 	return 0;
 }
 
-int UavcanServers::start(uavcan::INode &main_node)
+int
+UavcanServers::start(uavcan::INode &main_node)
 {
 	if (_instance != nullptr) {
-		warnx("Already started");
+		PX4_INFO("Already started");
 		return -1;
 	}
 
@@ -145,14 +144,14 @@ int UavcanServers::start(uavcan::INode &main_node)
 	_instance = new UavcanServers(main_node);
 
 	if (_instance == nullptr) {
-		warnx("Out of memory");
+		PX4_ERR("Out of memory");
 		return -2;
 	}
 
-	int rv  = _instance->init();
+	int rv = _instance->init();
 
 	if (rv < 0) {
-		warnx("Node init failed: %d", rv);
+		PX4_ERR("Node init failed: %d", rv);
 		delete _instance;
 		_instance = nullptr;
 		return rv;
@@ -170,7 +169,7 @@ int UavcanServers::start(uavcan::INode &main_node)
 	param.sched_priority = Priority;
 
 	if (pthread_attr_setschedparam(&tattr, &param)) {
-		warnx("setting sched params failed");
+		PX4_ERR("setting sched params failed");
 	}
 
 	static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);};
@@ -179,7 +178,7 @@ int UavcanServers::start(uavcan::INode &main_node)
 
 	if (rv != 0) {
 		rv = -rv;
-		warnx("pthread_create() failed: %d", rv);
+		PX4_ERR("pthread_create() failed: %d", rv);
 		delete _instance;
 		_instance = nullptr;
 	}
@@ -187,7 +186,8 @@ int UavcanServers::start(uavcan::INode &main_node)
 	return rv;
 }
 
-int UavcanServers::init()
+int
+UavcanServers::init()
 {
 	errno = 0;
 
@@ -195,11 +195,10 @@ int UavcanServers::init()
 	 * Initialize the mutex.
 	 * giving it its path
 	 */
-
 	int ret = Lock::init(_subnode_mutex);
 
 	if (ret < 0) {
-		warnx("Lock init: %d", errno);
+		PX4_ERR("Lock init: %d", errno);
 		return ret;
 	}
 
@@ -208,7 +207,6 @@ int UavcanServers::init()
 	_subnode.setNodeID(_main_node.getNodeID());
 	_main_node.getDispatcher().installRxFrameListener(&_vdriver);
 
-
 	/*
 	 * Initialize the fw version checker.
 	 * giving it its path
@@ -216,7 +214,7 @@ int UavcanServers::init()
 	ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
 
 	if (ret < 0) {
-		warnx("FirmwareVersionChecker init: %d, errno: %d", ret, errno);
+		PX4_ERR("FirmwareVersionChecker init: %d, errno: %d", ret, errno);
 		return ret;
 	}
 
@@ -225,7 +223,7 @@ int UavcanServers::init()
 	ret = _fw_server.start();
 
 	if (ret < 0) {
-		warnx("BasicFileServer init: %d, errno: %d", ret, errno);
+		PX4_ERR("BasicFileServer init: %d, errno: %d", ret, errno);
 		return ret;
 	}
 
@@ -234,7 +232,7 @@ int UavcanServers::init()
 	ret = _storage_backend.init(UAVCAN_NODE_DB_PATH);
 
 	if (ret < 0) {
-		warnx("FileStorageBackend init: %d, errno: %d", ret, errno);
+		PX4_ERR("FileStorageBackend init: %d, errno: %d", ret, errno);
 		return ret;
 	}
 
@@ -243,7 +241,7 @@ int UavcanServers::init()
 	ret = _tracer.init(UAVCAN_LOG_FILE);
 
 	if (ret < 0) {
-		warnx("FileEventTracer init: %d, errno: %d", ret, errno);
+		PX4_ERR("FileEventTracer init: %d, errno: %d", ret, errno);
 		return ret;
 	}
 
@@ -255,34 +253,32 @@ int UavcanServers::init()
 	ret = _server_instance.init(hwver.unique_id);
 
 	if (ret < 0) {
-		warnx("CentralizedServer init: %d", ret);
+		PX4_ERR("CentralizedServer init: %d", ret);
 		return ret;
 	}
 
 	/* Start node info retriever to fetch node info from new nodes */
-
 	ret = _node_info_retriever.start();
 
 	if (ret < 0) {
-		warnx("NodeInfoRetriever init: %d", ret);
+		PX4_ERR("NodeInfoRetriever init: %d", ret);
 		return ret;
 	}
 
 	/* Start the fw version checker   */
-
 	ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath());
 
 	if (ret < 0) {
-		warnx("FirmwareUpdateTrigger init: %d", ret);
+		PX4_ERR("FirmwareUpdateTrigger init: %d", ret);
 		return ret;
 	}
 
 	/*  Start the Node   */
-
 	return 0;
 }
 
-pthread_addr_t UavcanServers::run(pthread_addr_t)
+pthread_addr_t
+UavcanServers::run(pthread_addr_t)
 {
 	prctl(PR_SET_NAME, "uavcan fw srv", 0);
 
@@ -296,9 +292,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 
 	/* the subscribe call needs to happen in the same thread,
 	 * so not in the constructor */
-	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	uORB::Subscription armed_sub{ORB_ID(actuator_armed)};
-	int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request));
+	uORB::Subscription vcmd_sub{ORB_ID(vehicle_command)};
-	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	uORB::Subscription param_request_sub{ORB_ID(uavcan_parameter_request)};
 
 	/* Set up shared service clients */
 	_param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset));
@@ -326,16 +322,13 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 		const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
 
 		if (spin_res < 0) {
-			warnx("node spin error %i", spin_res);
+			PX4_ERR("node spin error %i", spin_res);
 		}
 
 		// Check for parameter requests (get/set/list)
-		bool param_request_ready;
+		if (param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
-		orb_check(param_request_sub, &param_request_ready);
+			uavcan_parameter_request_s request{};
-
+			param_request_sub.copy(&request);
-		if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
-			struct uavcan_parameter_request_s request;
-			orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request);
 
 			if (_param_counts[request.node_id]) {
 				/*
@@ -355,7 +348,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 					int call_res = _param_getset_client.call(request.node_id, req);
 
 					if (call_res < 0) {
-						warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
+						PX4_ERR("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
 
 					} else {
 						_param_in_progress = true;
@@ -388,7 +381,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 					int call_res = _param_getset_client.call(request.node_id, req);
 
 					if (call_res < 0) {
-						warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
+						PX4_ERR("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
 
 					} else {
 						_param_in_progress = true;
@@ -402,7 +395,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 					_param_list_node_id = request.node_id;
 					_param_list_all_nodes = false;
 
-					warnx("UAVCAN command bridge: starting component-specific param list");
+					PX4_INFO("UAVCAN command bridge: starting component-specific param list");
 				}
 
 			} else if (request.node_id == MAV_COMP_ID_ALL) {
@@ -416,7 +409,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 					_param_list_node_id = get_next_active_node_id(0);
 					_param_list_all_nodes = true;
 
-					warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id);
+					PX4_INFO("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id);
 
 					if (_param_counts[_param_list_node_id] == 0) {
 						param_count(_param_list_node_id);
@@ -435,7 +428,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 		// Handle parameter listing index/node ID advancement
 		if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
 			if (_param_index >= _param_counts[_param_list_node_id]) {
-				warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
+				PX4_INFO("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
 				// Reached the end of the current node's parameter set.
 				_param_list_in_progress = false;
 
@@ -457,7 +450,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 						// Keep on listing.
 						_param_index = 0;
 						_param_list_in_progress = true;
-						warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
+						PX4_INFO("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
 					}
 				}
 			}
@@ -474,7 +467,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 
 			if (call_res < 0) {
 				_param_list_in_progress = false;
-				warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
+				PX4_ERR("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
 
 			} else {
 				_param_in_progress = true;
@@ -482,12 +475,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 		}
 
 		// Check for ESC enumeration commands
-		bool cmd_ready;
+		if (vcmd_sub.updated() && !_cmd_in_progress) {
-		orb_check(cmd_sub, &cmd_ready);
+			vehicle_command_s cmd{};
-
+			vcmd_sub.copy(&cmd);
-		if (cmd_ready && !_cmd_in_progress) {
-			struct vehicle_command_s cmd;
-			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
 			uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
 
@@ -496,7 +486,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 				int node_id = static_cast<int>(cmd.param2 + 0.5f);
 				int call_res;
 
-				warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);
+				PX4_INFO("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);
 
 				switch (command_id) {
 				case 0:
@@ -512,7 +502,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 						call_res = _enumeration_client.call(get_next_active_node_id(0), req);
 
 						if (call_res < 0) {
-							warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
+							PX4_ERR("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
 							beep(BeepFrequencyError);
 							cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
 
@@ -524,7 +514,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 					}
 
 				default: {
-						warnx("UAVCAN command bridge: unknown command ID %d", command_id);
+						PX4_ERR("UAVCAN command bridge: unknown command ID %d", command_id);
 						cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
 						break;
 					}
@@ -533,7 +523,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
 				int command_id = static_cast<int>(cmd.param1 + 0.5f);
 
-				warnx("UAVCAN command bridge: received storage command ID %d", command_id);
+				PX4_INFO("UAVCAN command bridge: received storage command ID %d", command_id);
 
 				switch (command_id) {
 				case 1: {
@@ -576,15 +566,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 
 		// Shut down once armed
 		// TODO (elsewhere): start up again once disarmed?
-		bool updated;
+		if (armed_sub.updated()) {
-		orb_check(armed_sub, &updated);
+			actuator_armed_s armed{};
-
+			armed_sub.copy(&armed);
-		if (updated) {
-			struct actuator_armed_s armed;
-			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
 
 			if (armed.armed && !(armed.lockdown || armed.manual_lockdown)) {
-				warnx("UAVCAN command bridge: system armed, exiting now.");
+				PX4_INFO("UAVCAN command bridge: system armed, exiting now.");
 				break;
 			}
 		}
@@ -592,11 +579,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
 
 	_subnode_thread_should_exit = false;
 
-	warnx("exiting");
+	PX4_INFO("exiting");
 	return (pthread_addr_t) 0;
 }
 
-void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
+void
+UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
 {
 	if (_count_in_progress) {
 		/*
@@ -622,14 +610,14 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
 				if (call_res < 0) {
 					_count_in_progress = false;
 					_count_index = 0;
-					warnx("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res);
+					PX4_ERR("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res);
 					beep(BeepFrequencyError);
 				}
 
 			} else {
 				_count_in_progress = false;
 				_count_index = 0;
-				warnx("UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
+				PX4_INFO("UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
 				beep(BeepFrequencyGenericIndication);
 			}
 
@@ -637,7 +625,7 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
 			_param_counts[node_id] = 0;
 			_count_in_progress = false;
 			_count_index = 0;
-			warnx("UAVCAN command bridge: GetSet error during param count");
+			PX4_ERR("UAVCAN command bridge: GetSet error during param count");
 		}
 
 	} else {
@@ -669,15 +657,10 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
 				response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
 			}
 
-			if (_param_response_pub == nullptr) {
+			_param_response_pub.publish(response);
-				_param_response_pub = orb_advertise(ORB_ID(uavcan_parameter_value), &response);
-
-			} else {
-				orb_publish(ORB_ID(uavcan_parameter_value), _param_response_pub, &response);
-			}
 
 		} else {
-			warnx("UAVCAN command bridge: GetSet error");
+			PX4_ERR("UAVCAN command bridge: GetSet error");
 		}
 
 		_param_in_progress = false;
@@ -685,38 +668,41 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
 	}
 }
 
-void UavcanServers::param_count(uavcan::NodeID node_id)
+void
+UavcanServers::param_count(uavcan::NodeID node_id)
 {
 	uavcan::protocol::param::GetSet::Request req;
 	req.index = 0;
 	int call_res = _param_getset_client.call(node_id, req);
 
 	if (call_res < 0) {
-		warnx("UAVCAN command bridge: couldn't start parameter count: %d", call_res);
+		PX4_ERR("UAVCAN command bridge: couldn't start parameter count: %d", call_res);
 
 	} else {
 		_count_in_progress = true;
 		_count_index = 0;
-		warnx("UAVCAN command bridge: starting param count");
+		PX4_INFO("UAVCAN command bridge: starting param count");
 	}
 }
 
-void UavcanServers::param_opcode(uavcan::NodeID node_id)
+void
+UavcanServers::param_opcode(uavcan::NodeID node_id)
 {
 	uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
 	opcode_req.opcode = _param_save_opcode;
 	int call_res = _param_opcode_client.call(node_id, opcode_req);
 
 	if (call_res < 0) {
-		warnx("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res);
+		PX4_ERR("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res);
 
 	} else {
 		_cmd_in_progress = true;
-		warnx("UAVCAN command bridge: sent ExecuteOpcode");
+		PX4_INFO("UAVCAN command bridge: sent ExecuteOpcode");
 	}
 }
 
-void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
+void
+UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
 {
 	bool success = result.isSuccessful();
 	uint8_t node_id = result.getCallID().server_node_id.get();
@@ -725,23 +711,23 @@ void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::
 	_cmd_in_progress = false;
 
 	if (!result.isSuccessful()) {
-		warnx("UAVCAN command bridge: save request for node %hhu timed out.", node_id);
+		PX4_ERR("UAVCAN command bridge: save request for node %hhu timed out.", node_id);
 
 	} else if (!result.getResponse().ok) {
-		warnx("UAVCAN command bridge: save request for node %hhu rejected.", node_id);
+		PX4_ERR("UAVCAN command bridge: save request for node %hhu rejected.", node_id);
 
 	} else {
-		warnx("UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id);
+		PX4_INFO("UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id);
 
 		uavcan::protocol::RestartNode::Request restart_req;
 		restart_req.magic_number = restart_req.MAGIC_NUMBER;
 		int call_res = _param_restartnode_client.call(node_id, restart_req);
 
 		if (call_res < 0) {
-			warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res);
+			PX4_ERR("UAVCAN command bridge: couldn't send RestartNode: %d", call_res);
 
 		} else {
-			warnx("UAVCAN command bridge: sent RestartNode");
+			PX4_ERR("UAVCAN command bridge: sent RestartNode");
 			_cmd_in_progress = true;
 		}
 	}
@@ -761,7 +747,8 @@ void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::
 	}
 }
 
-void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
+void
+UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
 {
 	bool success = result.isSuccessful();
 	uint8_t node_id = result.getCallID().server_node_id.get();
@@ -770,12 +757,13 @@ void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol:
 	_cmd_in_progress = false;
 
 	if (success) {
-		warnx("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id);
+		PX4_INFO("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id);
+
 		// Clear the dirty flag
 		clear_node_params_dirty(node_id);
 
 	} else {
-		warnx("UAVCAN command bridge: restart request for node %hhu failed.", node_id);
+		PX4_ERR("UAVCAN command bridge: restart request for node %hhu failed.", node_id);
 	}
 
 	// Get the next dirty node ID and send the same command to it
@@ -786,7 +774,8 @@ void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol:
 	}
 }
 
-uint8_t UavcanServers::get_next_active_node_id(uint8_t base)
+uint8_t
+UavcanServers::get_next_active_node_id(uint8_t base)
 {
 	base++;
 
@@ -796,7 +785,8 @@ uint8_t UavcanServers::get_next_active_node_id(uint8_t base)
 	return base;
 }
 
-uint8_t UavcanServers::get_next_dirty_node_id(uint8_t base)
+uint8_t
+UavcanServers::get_next_dirty_node_id(uint8_t base)
 {
 	base++;
 
@@ -805,7 +795,8 @@ uint8_t UavcanServers::get_next_dirty_node_id(uint8_t base)
 	return base;
 }
 
-void UavcanServers::beep(float frequency)
+void
+UavcanServers::beep(float frequency)
 {
 	uavcan::equipment::indication::BeepCommand cmd;
 	cmd.frequency = frequency;
@@ -813,20 +804,21 @@ void UavcanServers::beep(float frequency)
 	(void)_beep_pub.broadcast(cmd);
 }
 
-void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result)
+void
+UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result)
 {
 	uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get());
 
 	if (!result.isSuccessful()) {
-		warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
+		PX4_ERR("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
 
 	} else if (result.getResponse().error) {
-		warnx("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(),
+		PX4_ERR("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(),
-		      result.getResponse().error);
+			result.getResponse().error);
 
 	} else {
 		_esc_count++;
-		warnx("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get());
+		PX4_INFO("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get());
 	}
 
 	if (next_id < 128) {
@@ -840,22 +832,23 @@ void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan:
 		int call_res = _enumeration_client.call(next_id, req);
 
 		if (call_res < 0) {
-			warnx("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
+			PX4_ERR("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
 
 		} else {
-			warnx("UAVCAN ESC enumeration: sent Begin request");
+			PX4_INFO("UAVCAN ESC enumeration: sent Begin request");
 		}
 
 	} else {
-		warnx("UAVCAN ESC enumeration: begun enumeration on all nodes.");
+		PX4_INFO("UAVCAN ESC enumeration: begun enumeration on all nodes.");
 	}
 }
 
-void UavcanServers::cb_enumeration_indication(const
+void
-		uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg)
+UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>
+		&msg)
 {
 	// Called whenever an ESC thinks it has received user input.
-	warnx("UAVCAN ESC enumeration: got indication");
+	PX4_INFO("UAVCAN ESC enumeration: got indication");
 
 	if (!_esc_enumeration_active) {
 		// Ignore any messages received when we're not expecting them
@@ -867,33 +860,33 @@ void UavcanServers::cb_enumeration_indication(const
 
 	for (; i < _esc_enumeration_index; i++) {
 		if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) {
-			warnx("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored", _esc_enumeration_ids[i], i);
+			PX4_INFO("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored", _esc_enumeration_ids[i], i);
 			return;
 		}
 	}
 
 	uavcan::protocol::param::GetSet::Request req;
-	req.name =
+	req.name = msg.parameter_name;	// 'esc_index' or something alike, the name is not standardized
-		msg.parameter_name;                                           // 'esc_index' or something alike, the name is not standardized
 	req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
 
 	int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req);
 
 	if (call_res < 0) {
-		warnx("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
+		PX4_ERR("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
 
 	} else {
-		warnx("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i);
+		PX4_INFO("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i);
 	}
 }
 
-void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
+void
+UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
 {
 	if (!result.isSuccessful()) {
-		warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
+		PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
 
 	} else {
-		warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
+		PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
 
 		uavcan::protocol::param::GetSet::Response resp = result.getResponse();
 		uint8_t esc_index = (uint8_t)resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
@@ -907,41 +900,43 @@ void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan
 		int call_res = _enumeration_save_client.call(result.getCallID().server_node_id, opcode_req);
 
 		if (call_res < 0) {
-			warnx("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
+			PX4_ERR("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
 
 		} else {
-			warnx("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index],
+			PX4_INFO("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index],
-			      esc_index);
+				 esc_index);
 		}
 	}
 }
 
-void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
+void
+UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
 {
 	const bool this_is_the_last_one =
-		_esc_enumeration_index >= uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 ||
+		(_esc_enumeration_index >= uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1) ||
-		_esc_enumeration_index >= _esc_count;
+		(_esc_enumeration_index >= _esc_count);
 
 	if (!result.isSuccessful()) {
-		warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
+		PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
 		beep(BeepFrequencyError);
 
 	} else if (!result.getResponse().ok) {
-		warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
+		PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
 		beep(BeepFrequencyError);
 
 	} else {
-		warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
+		PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
 		beep(this_is_the_last_one ? BeepFrequencySuccess : BeepFrequencyGenericIndication);
 	}
 
-	warnx("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);
+	PX4_INFO("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);
 
 	if (this_is_the_last_one) {
 		_esc_enumeration_active = false;
 
 		// Tell all ESCs to stop enumerating
 		uavcan::protocol::enumeration::Begin::Request req;
+
 		// TODO: Incorrect implementation; the parameter name field should be left empty.
 		//       Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
 		req.parameter_name = "esc_index";
@@ -949,15 +944,16 @@ void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::
 		int call_res = _enumeration_client.call(get_next_active_node_id(0), req);
 
 		if (call_res < 0) {
-			warnx("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
+			PX4_ERR("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
 
 		} else {
-			warnx("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
+			PX4_INFO("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
 		}
 	}
 }
 
-void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_path)
+void
+UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_path)
 {
 	/*
 	Copy the ROMFS firmware directory to the appropriate location on SD, without
@@ -1025,14 +1021,14 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
 		size_t srcpath_dev_len = romfs_path_len + 1 + dev_dirname_len;
 
 		if (srcpath_dev_len > maxlen) {
-			warnx("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name);
+			PX4_WARN("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name);
 			continue;
 		}
 
 		size_t dstpath_dev_len = sd_path_len + 1 + dev_dirname_len;
 
 		if (dstpath_dev_len > maxlen) {
-			warnx("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name);
+			PX4_WARN("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name);
 			continue;
 		}
 
@@ -1044,7 +1040,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
 			rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
 
 			if (rv != 0) {
-				warnx("dev: couldn't create '%s'", dstpath);
+				PX4_ERR("dev: couldn't create '%s'", dstpath);
 				continue;
 			}
 		}
@@ -1056,7 +1052,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
 		DIR *const dev_dir = opendir(srcpath);
 
 		if (!dev_dir) {
-			warnx("dev: couldn't open '%s'", srcpath);
+			PX4_ERR("dev: couldn't open '%s'", srcpath);
 			continue;
 		}
 
@@ -1074,14 +1070,14 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
 			size_t srcpath_ver_len = srcpath_dev_len + 1 + ver_dirname_len;
 
 			if (srcpath_ver_len > maxlen) {
-				warnx("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name);
+				PX4_ERR("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name);
 				continue;
 			}
 
 			size_t dstpath_ver_len = dstpath_dev_len + 1 + ver_dirname_len;
 
 			if (dstpath_ver_len > maxlen) {
-				warnx("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name);
+				PX4_ERR("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name);
 				continue;
 			}
 
@@ -1093,7 +1089,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
 				rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
 
 				if (rv != 0) {
-					warnx("ver: couldn't create '%s'", dstpath);
+					PX4_ERR("ver: couldn't create '%s'", dstpath);
 					continue;
 				}
 			}
@@ -1107,7 +1103,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
 			DIR *const src_ver_dir = opendir(srcpath);
 
 			if (!src_ver_dir) {
-				warnx("ver: couldn't open '%s'", srcpath);
+				PX4_ERR("ver: couldn't open '%s'", srcpath);
 				continue;
 			}
 
@@ -1129,7 +1125,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
 			DIR *const dst_ver_dir = opendir(dstpath);
 
 			if (!dst_ver_dir) {
-				warnx("unlink: couldn't open '%s'", dstpath);
+				PX4_ERR("unlink: couldn't open '%s'", dstpath);
 
 			} else {
 				struct dirent *fw_dirent = NULL;
@@ -1153,14 +1149,14 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
 
 						if (dstpath_fw_len > maxlen) {
 							// sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator
-							warnx("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name);
+							PX4_ERR("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name);
 
 						} else {
 							// File name starts with "_", delete it.
 							dstpath[dstpath_ver_len] = '/';
 							memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, dst_fw_len + 1);
 							unlink(dstpath);
-							warnx("unlink: removed '%s'", dstpath);
+							PX4_ERR("unlink: removed '%s'", dstpath);
 						}
 
 					} else {
@@ -1178,10 +1174,10 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
 				size_t dstpath_fw_len = dstpath_ver_len + fw_len;
 
 				if (srcpath_fw_len > maxlen) {
-					warnx("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name);
+					PX4_ERR("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name);
 
 				} else if (dstpath_fw_len > maxlen) {
-					warnx("copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name);
+					PX4_ERR("copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name);
 
 				} else {
 					// All OK, make the paths and copy the file
@@ -1194,10 +1190,10 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
 					rv = copyFw(dstpath, srcpath);
 
 					if (rv != 0) {
-						warnx("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv);
+						PX4_ERR("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv);
 
 					} else {
-						warnx("copy: '%s' -> '%s' succeeded", srcpath, dstpath);
+						PX4_INFO("copy: '%s' -> '%s' succeeded", srcpath, dstpath);
 					}
 				}
 			}
@@ -1211,24 +1207,24 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
 	(void)closedir(romfs_dir);
 }
 
-int UavcanServers::copyFw(const char *dst, const char *src)
+int
+UavcanServers::copyFw(const char *dst, const char *src)
 {
 	int rv = 0;
-	int dfd, sfd;
+	uint8_t buffer[512] {};
-	uint8_t buffer[512];
 
-	dfd = open(dst, O_WRONLY | O_CREAT, 0666);
+	int dfd = open(dst, O_WRONLY | O_CREAT, 0666);
 
 	if (dfd < 0) {
-		warnx("copyFw: couldn't open dst");
+		PX4_ERR("copyFw: couldn't open dst");
 		return -errno;
 	}
 
-	sfd = open(src, O_RDONLY, 0);
+	int sfd = open(src, O_RDONLY, 0);
 
 	if (sfd < 0) {
 		(void)close(dfd);
-		warnx("copyFw: couldn't open src");
+		PX4_ERR("copyFw: couldn't open src");
 		return -errno;
 	}
 
@@ -1238,7 +1234,7 @@ int UavcanServers::copyFw(const char *dst, const char *src)
 		size = read(sfd, buffer, sizeof(buffer));
 
 		if (size < 0) {
-			warnx("copyFw: couldn't read");
+			PX4_ERR("copyFw: couldn't read");
 			rv = -errno;
 
 		} else if (size > 0) {
@@ -1251,7 +1247,7 @@ int UavcanServers::copyFw(const char *dst, const char *src)
 				written = write(dfd, &buffer[total_written], remaining);
 
 				if (written < 0) {
-					warnx("copyFw: couldn't write");
+					PX4_ERR("copyFw: couldn't write");
 					rv = -errno;
 
 				} else {

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_t         _subnode_thread{-1};
-	pthread_mutex_t   _subnode_mutex;
+	pthread_mutex_t   _subnode_mutex{};
-	volatile bool     _subnode_thread_should_exit = false;
+	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.
+	  broken out device elements. The bitfields are used to keep
-	 *
+	  the overall value small enough to fit in a float accurately,
-	 * @return The bus ID
+	  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; }
+	uint8_t	get_device_address() const { return _device_id.devid_s.address; }
-
+	void	set_device_address(int address) { _device_id.devid_s.address = 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; }
+	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; }
-	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;
-	};
 
 	/**
 	 * 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
+		set_device_bus_type(DeviceBusType_UNKNOWN);
-		   other fields are invalid */
+	}
-		_device_id.devid = 0;
+
-		_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
+	Device(const char *name, DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype = 0)
-		_device_id.devid_s.bus = 0;
+		: _name(name)
-		_device_id.devid_s.address = 0;
+	{
-		_device_id.devid_s.devtype = 0;
+		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;
+		set_device_bus_type(bus_type);
-		_device_id.devid_s.bus_type = bus_type;
+		set_device_bus(bus);
-		_device_id.devid_s.bus = bus;
+		set_device_address(address);
-		_device_id.devid_s.address = address;
+		set_device_type(devtype);
-		_device_id.devid_s.devtype = 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>
+  <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>