uavcan minor cleanup

src/drivers/uavcan/actuators/esc.cpp

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

src/drivers/uavcan/actuators/esc.hpp

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

src/drivers/uavcan/actuators/hardpoint.cpp

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

src/drivers/uavcan/allocator.hpp

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

src/drivers/uavcan/sensors/baro.cpp

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

src/drivers/uavcan/sensors/baro.hpp

@@ -39,13 +39,10 @@
 
 #include "sensor_bridge.hpp"
 #include <drivers/drv_baro.h>
-#include <drivers/device/ringbuffer.h>
 
 #include <uavcan/equipment/air_data/StaticPressure.hpp>
 #include <uavcan/equipment/air_data/StaticTemperature.hpp>
 
-class RingBuffer;
-
 class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
 {
 public:
@@ -58,8 +55,6 @@ class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
 	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 @@ class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
 	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};
 
 };