uavcan: move to work queue and use MixingOutput

Main UAVCAN protocol handling and ESC updates run on the same thread/wq as
before. There are 2 WorkItems for separate scheduling of the 2, so that
ESC updates run in sync with actuator_control updates. UAVCAN is scheduled
at a fixed rate of 3ms (previously the poll timeout) and on each UAVCAN
bus event.
This leads to roughly the same behavior as before. CPU & RAM usage are
pretty much the same (tested on Pixhawk 4).

Testing done: Motors still work (with feedback), param changes and a
UAVCAN optical flow sensor.

src/drivers/uavcannode/uavcannode_main.cpp

@@ -64,6 +64,8 @@ __END_DECLS
 
 #include "boot_app_shared.h"
 
+using namespace time_literals;
+
 /**
  * @file uavcan_main.cpp
  *
@@ -124,6 +126,9 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 		std::abort();
 	}
 
+	px4_sem_init(&_sem, 0, 0);
+	/* semaphore use case is a signal */
+	px4_sem_setprotocol(&_sem, SEM_PRIO_NONE);
 }
 
 UavcanNode::~UavcanNode()
@@ -148,7 +153,7 @@ UavcanNode::~UavcanNode()
 	}
 
 	_instance = nullptr;
-
+	px4_sem_destroy(&_sem);
 }
 
 int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
@@ -189,7 +194,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 
 
 	resources("Before _instance->init:");
-	const int node_init_res = _instance->init(node_id);
+	const int node_init_res = _instance->init(node_id, can.driver.updateEvent());
 	resources("After _instance->init:");
 
 	if (node_init_res < 0) {
@@ -282,7 +287,7 @@ void UavcanNode::cb_beginfirmware_update(const uavcan::ReceivedDataStructure<Uav
 	}
 }
 
-int UavcanNode::init(uavcan::NodeID node_id)
+int UavcanNode::init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events)
 {
 	int ret = -1;
 
@@ -306,6 +311,8 @@ int UavcanNode::init(uavcan::NodeID node_id)
 		return ret;
 	}
 
+	bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline);
+
 	return _node.start();
 }
 
@@ -333,26 +340,30 @@ 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)
+static void signal_callback(void *arg)
 {
-	int ret = _poll_fds_num;
+	/* Note: we are in IRQ context here */
+	px4_sem_t *sem = (px4_sem_t *)arg;
+	int semaphore_value;
 
-	if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
-		errx(1, "uavcan: too many poll fds, exiting");
+	if (px4_sem_getvalue(sem, &semaphore_value) == 0 && semaphore_value > 1) {
+		return;
 	}
 
-	_poll_fds[_poll_fds_num] = ::pollfd();
-	_poll_fds[_poll_fds_num].fd = fd;
-	_poll_fds[_poll_fds_num].events = POLLIN;
-	_poll_fds_num += 1;
-	return ret;
+	px4_sem_post(sem);
 }
 
 
+void
+UavcanNode::busevent_signal_trampoline()
+{
+	if (_instance) {
+		signal_callback(&_instance->_sem);
+	}
+}
+
+
+
 int UavcanNode::run()
 {
 
@@ -381,52 +392,26 @@ int UavcanNode::run()
 		_task_should_exit = true;
 	}
 
-	const unsigned PollTimeoutMs = 50;
-
-	const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
-
-	if (busevent_fd < 0) {
-		warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
-		_task_should_exit = true;
-	}
-
-	/* If we had an  RTC we would call uavcan_stm32::clock::setUtc()
-	* but for now we use adjustUtc with a correction of 0
-	*/
-//        uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
-
 	_node.setModeOperational();
 
-	/*
-	 * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
-	 * Please note that with such multiplexing it is no longer possible to rely only on
-	 * the value returned from poll() to detect whether actuator control has timed out or not.
-	 * Instead, all ORB events need to be checked individually (see below).
-	 */
-	add_poll_fd(busevent_fd);
-
 	uint32_t start_tick = clock_systimer();
 
+	hrt_call timer_call{};
+	hrt_call_every(&timer_call, 50_ms, 50_ms, signal_callback, &_sem);
+
 	while (!_task_should_exit) {
 		// Mutex is unlocked while the thread is blocked on IO multiplexing
 		(void)pthread_mutex_unlock(&_node_mutex);
 
-		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
-
+		while (px4_sem_wait(&_sem) != 0);
 
 		(void)pthread_mutex_lock(&_node_mutex);
 
 		node_spin_once();  // Non-blocking
 
 
-		// this would be bad...
-		if (poll_ret < 0) {
-			PX4_ERR("poll error %d", errno);
-			continue;
+		// Do Something
 
-		} else {
-			// Do Something
-		}
 
 		if (clock_systimer() - start_tick > TICK_PER_SEC) {
 			start_tick = clock_systimer();
@@ -455,8 +440,9 @@ int UavcanNode::run()
 
 	}
 
+	hrt_cancel(&timer_call);
 	teardown();
-	warnx("exiting.");
+	(void)pthread_mutex_unlock(&_node_mutex);
 
 	exit(0);
 }

src/drivers/uavcannode/uavcannode_main.hpp

@@ -118,12 +118,13 @@ public:
 
 private:
 	void		fill_node_info();
-	int		init(uavcan::NodeID node_id);
+	int		init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events);
 	void		node_spin_once();
 	int		run();
-	int		add_poll_fd(int fd);			///< add a fd to poll list, returning index into _poll_fds[]
+	static void	busevent_signal_trampoline();
 
 
+	px4_sem_t		_sem;				///< semaphore for scheduling the task
 	int			_task = -1;			///< handle to the OS task
 	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
 
@@ -132,9 +133,6 @@ private:
 	pthread_mutex_t		_node_mutex;
 	uavcan::GlobalTimeSyncSlave _time_sync_slave;
 
-	pollfd			_poll_fds[UAVCAN_NUM_POLL_FDS] = {};
-	unsigned		_poll_fds_num = 0;
-
 	typedef uavcan::MethodBinder<UavcanNode *,
 		void (UavcanNode::*)(const uavcan::ReceivedDataStructure<UavcanNode::BeginFirmwareUpdate::Request> &,
 				     uavcan::ServiceResponseDataStructure<UavcanNode::BeginFirmwareUpdate::Response> &)>