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 && mavlink_poll_ret != -ETIMEDOUT) {
+			/* 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,113 @@ 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);
+}
+
+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);
+
+	if (req == nullptr) {
+		return -ENOMEM;
+	}
+
+	pthread_mutex_lock(&_mtx);
+	_reqs.add(req);
+	hrt_call_every(&req->_hrt_req, (hrt_abstime)interval_us,
+		       (hrt_abstime)interval_us, hrt_callback, &_poll_sem);
+	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);
+			hrt_cancel(&req->_hrt_req);
+			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_us)
+{
+	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 + timeout_us);
+
+	ret = px4_sem_timedwait(&_poll_sem, &to);
+
+	if (ret < 0) {
+		ret = -errno;
+	}
+
+	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,71 @@ 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_us);
+
+private:
+
+	class MavStreamPollReq :  public ListNode<MavStreamPollReq *>
+	{
+	public:
+		MavStreamPollReq(uint16_t stream_id) : _stream_id(stream_id) {}
+		~MavStreamPollReq()
+		{
+			hrt_cancel(&_hrt_req);
+		}
+
+		uint16_t    _stream_id;
+		struct hrt_call _hrt_req;
+	};
+
+
+	/**
+	 * HRT interrupt callback posting the semaphore
+	 */
+	static void hrt_callback(void *arg);
+
+	/**
+	 * Requests from stream objects
+	 */
+	List<class MavStreamPollReq *> _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_ */