Implement HRT-based polling into mavlink main loop instead of sleep

Sleeping depends on the systick, which prevents using slower than 1ms systicks.

Using HRT provides as performant way of handling the mavlink polling and is also more accurate.

Co-authored-by: Jari Nippula <[email protected]>
Signed-off-by: Jukka Laitinen <[email protected]>

src/modules/mavlink/mavlink_main.cpp

@@ -134,6 +134,7 @@ Mavlink::Mavlink() :
 
 	_event_sub.subscribe();
 	_telemetry_status_pub.advertise();
+	_stream_poller = new MavlinkStreamPoll();
 }
 
 Mavlink::~Mavlink()
@@ -175,6 +176,8 @@ Mavlink::~Mavlink()
 		}
 	}
 
+	delete _stream_poller;
+
 	perf_free(_loop_perf);
 	perf_free(_loop_interval_perf);
 	perf_free(_send_byte_error_perf);
@@ -1156,7 +1159,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 		if (strcmp(stream_name, stream->get_name()) == 0) {
 			if (interval != 0) {
 				/* set new interval */
-				stream->set_interval(interval);
+				set_stream_interval(stream, interval);
 
 			} else {
 				/* delete stream */
@@ -1173,7 +1176,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 	MavlinkStream *stream = create_mavlink_stream(stream_name, this);
 
 	if (stream != nullptr) {
-		stream->set_interval(interval);
+		set_stream_interval(stream, interval);
 		_streams.add(stream);
 
 		return OK;
@@ -2325,9 +2328,22 @@ Mavlink::task_main(int argc, char *argv[])
 
 	_task_running.store(true);
 
+	int mavlink_poll_error_counter = 0;
+
 	while (!should_exit()) {
 		/* main loop */
-		px4_usleep(_main_loop_delay);
+
+		int mavlink_poll_ret = _stream_poller->poll(MAIN_LOOP_DELAY);
+
+		if (mavlink_poll_ret < 0) {
+			/* this is seriously bad - should be an emergency */
+			if (mavlink_poll_error_counter < 10 || mavlink_poll_error_counter % 50 == 0) {
+				/* use a counter to prevent flooding (and slowing us down) */
+				PX4_ERR("ERROR while polling streams: %d", mavlink_poll_ret);
+			}
+
+			mavlink_poll_error_counter++;
+		}
 
 		if (!should_transmit()) {
 			check_requested_subscriptions();
@@ -2849,6 +2865,13 @@ void Mavlink::configure_sik_radio()
 	}
 }
 
+int
+Mavlink::set_stream_interval(MavlinkStream *stream, int interval)
+{
+	stream->set_interval(interval);
+	return _stream_poller->set_interval(stream->get_id(), interval);
+}
+
 int Mavlink::start_helper(int argc, char *argv[])
 {
 	/* create the instance in task context */

src/modules/mavlink/mavlink_main.h

@@ -524,7 +524,11 @@ class Mavlink final : public ModuleParams
 
 	bool radio_status_critical() const { return _radio_status_critical; }
 
+	int set_stream_interval(MavlinkStream *stream, int interval);
+
 private:
+	MavlinkStreamPoll	*_stream_poller {nullptr};
+
 	MavlinkReceiver 	_receiver;
 
 	int			_instance_id{-1};

src/modules/mavlink/mavlink_stream.cpp

@@ -44,6 +44,13 @@
 #include "mavlink_stream.h"
 #include "mavlink_main.h"
 
+/**
+ * If stream rate is set to unlimited, set the rate to 50 Hz. To get higher
+ * rates, it needs to be set explicitly.
+ */
+
+const uint32_t MavlinkStreamUnlimitedInterval = 20000;
+
 MavlinkStream::MavlinkStream(Mavlink *mavlink) :
 	_mavlink(mavlink)
 {
@@ -128,3 +135,147 @@ MavlinkStream::update(const hrt_abstime &t)
 
 	return -1;
 }
+
+MavlinkStreamPoll::MavlinkStreamPoll()
+{
+	px4_sem_init(&_poll_sem, 1, 0);
+#if defined(__PX4_NUTTX)
+	sem_setprotocol(&_poll_sem, SEM_PRIO_NONE);
+#endif
+
+	pthread_mutex_init(&_mtx, nullptr);
+}
+
+MavlinkStreamPoll::~MavlinkStreamPoll()
+{
+	// This removes and deletes every object in the list
+	_reqs.clear();
+
+	px4_sem_destroy(&_poll_sem);
+	pthread_mutex_destroy(&_mtx);
+}
+
+void MavlinkStreamPoll::recalculate_roots_and_start(MavStreamPollReq *req)
+{
+	// Now go through the ordered _reqs list again, to see if this timer needs to
+	// be started, and if some others can be stopped
+
+	bool is_root = true;
+	uint32_t interval_us = req->interval_us();
+
+	for (auto r : _reqs) {
+		if (r->interval_us() <= interval_us) {
+			if (r->is_root() && interval_us % r->interval_us() == 0) {
+				is_root = false;
+			}
+
+		} else {
+			if (is_root && r->is_root() && r->interval_us() % interval_us) {
+				r->stop_interval();
+			}
+		}
+	}
+
+	// If this was a new root interval, start the hrt
+
+	if (is_root) {
+		req->start_interval(hrt_callback, &_poll_sem);
+	}
+}
+
+int
+MavlinkStreamPoll::register_poll(uint16_t stream_id, uint32_t interval_us)
+{
+	// Streans with interval 0 are disabled and don't need to be registered here
+
+	if (interval_us == 0) {
+		return OK;
+	}
+
+	MavStreamPollReq *req = new MavStreamPollReq(stream_id, interval_us);
+
+	if (req == nullptr) {
+		return -ENOMEM;
+	}
+
+	pthread_mutex_lock(&_mtx);
+
+	_reqs.add_sorted(req);
+	recalculate_roots_and_start(req);
+
+	pthread_mutex_unlock(&_mtx);
+	return OK;
+}
+
+int
+MavlinkStreamPoll::unregister_poll(uint16_t stream_id)
+{
+	pthread_mutex_lock(&_mtx);
+
+	for (auto req : _reqs) {
+		if (req->stream_id() == stream_id) {
+			_reqs.remove(req);
+
+			if (req->is_root()) {
+				// This interval may be driving other streams. Re-calculate root clocks for all the
+				// remaining requests
+
+				for (auto r : _reqs) {
+					recalculate_roots_and_start(r);
+				}
+
+				req->stop_interval();
+			}
+
+			delete (req);
+			break;
+		}
+	}
+
+	pthread_mutex_unlock(&_mtx);
+
+	return OK;
+}
+
+int
+MavlinkStreamPoll::set_interval(uint16_t stream_id, int interval_us)
+{
+	unregister_poll(stream_id);
+
+	if (interval_us < 0) {
+		interval_us = MavlinkStreamUnlimitedInterval;
+	}
+
+	return register_poll(stream_id, interval_us);
+}
+
+/**
+ * Perform orb polling
+ */
+int
+MavlinkStreamPoll::poll(const hrt_abstime timeout)
+{
+	int ret;
+
+	// Wait event for a maximum timeout time
+
+	struct timespec to;
+#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
+	px4_clock_gettime(CLOCK_MONOTONIC, &to);
+#else
+	px4_clock_gettime(CLOCK_REALTIME, &to);
+#endif
+	hrt_abstime now = ts_to_abstime(&to);
+	abstime_to_ts(&to, now + (hrt_abstime)timeout * 1000);
+
+	ret = px4_sem_timedwait(&_poll_sem, &to);
+
+	return ret;
+}
+
+void
+MavlinkStreamPoll::hrt_callback(void *arg)
+{
+	px4_sem_post((px4_sem_t *)arg);
+}
+

src/modules/mavlink/mavlink_stream.h

@@ -43,6 +43,7 @@
 
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
+#include <px4_platform_common/sem.h>
 #include <containers/List.hpp>
 
 class Mavlink;
@@ -141,5 +142,128 @@ class MavlinkStream : public ListNode<MavlinkStream *>
 	bool _first_message_sent{false};
 };
 
+/**
+ * Class to manage polling of stream intervals
+ */
+
+class MavlinkStreamPoll
+{
+public:
+	MavlinkStreamPoll();
+	~MavlinkStreamPoll();
+
+	/**
+	 * Add a stream to the poll list
+	 */
+	int register_poll(uint16_t stream_id, uint32_t interval_us);
+
+	/**
+	 * Remove a stream from the poll list
+	 */
+	int unregister_poll(uint16_t stream_id);
+
+	/**
+	 * Re-set interval
+	 */
+	int set_interval(uint16_t stream_id, int interval_us);
+
+	/**
+	 * Poll all streams for updates
+	 */
+	int poll(const hrt_abstime timeout);
+
+private:
+
+	class MavStreamPollReq :  public ListNode<MavStreamPollReq *>
+	{
+	public:
+		MavStreamPollReq(uint16_t stream_id, uint32_t interval_us) : _stream_id(stream_id), _interval_us(interval_us),
+			_is_root(false) {}
+		~MavStreamPollReq()
+		{
+			if (_is_root) {
+				hrt_cancel(&_hrt_req);
+			}
+		}
+
+		void start_interval(hrt_callout cb, px4_sem_t *sem)
+		{
+			_is_root = true;
+			hrt_call_every(&_hrt_req, _interval_us,
+				       _interval_us, cb, sem);
+		}
+
+		void stop_interval() { _is_root = false; hrt_cancel(&_hrt_req); }
+
+		uint32_t interval_us() { return _interval_us; }
+		uint16_t stream_id() { return _stream_id; }
+		bool is_root() { return _is_root; }
+	private:
+		uint16_t        _stream_id;
+		uint32_t        _interval_us;
+		bool            _is_root;
+		struct hrt_call _hrt_req;
+	};
+
+	class MavlinkStreamPollReqList : public List<MavStreamPollReq *>
+	{
+	public:
+		void add_sorted(MavStreamPollReq *req)
+		{
+			if (_head == nullptr || _head->interval_us() > req->interval_us()) {
+				// add as head
+				req->setSibling(_head);
+				_head = req;
+				return;
+
+			} else {
+				// find the correct place in the list, sorted by the interval
+				MavStreamPollReq *node = _head;
+
+				while (node != nullptr) {
+					if (node->getSibling() == nullptr || node->getSibling()->interval_us() > req->interval_us()) {
+						// found the end or the correct place
+						req->setSibling(node->getSibling());
+						node->setSibling(req);
+						return;
+					}
+
+					node = node->getSibling();
+				}
+			}
+		}
+	};
+
+	/**
+	 * Check if some stream already runs hrt at an interval, by which
+	 * this request is evenly divisible with. This means that there is
+	 * no need to start another periodic timer, i.e. the interval is
+	 * not root.
+	 *
+	 * If the stream is root, start the timer for it and stop all the
+	 * other timers which are evenly divisible with this one.
+	 */
+	void recalculate_roots_and_start(MavStreamPollReq *req);
+
+	/**
+	 * HRT interrupt callback posting the semaphore
+	 */
+	static void hrt_callback(void *arg);
+
+	/**
+	 * Requests from stream objects
+	 */
+	MavlinkStreamPollReqList _reqs;
+
+	/**
+	 * Signalling semaphore to release the poll
+	 */
+	px4_sem_t _poll_sem;
+
+	/**
+	 * Mutex to protect the list of poll request (hrt) items
+	 */
+	pthread_mutex_t		_mtx {};
+};
 
 #endif /* MAVLINK_STREAM_H_ */