From bdd37b0b82324489d933c487fb5acb90f34a55fe Mon Sep 17 00:00:00 2001
From: Jukka Laitinen <jukkax@ssrc.tii.ae>
Date: Fri, 7 Jun 2024 11:11:26 +0300
Subject: [PATCH] mavlink_stream: Add all stream poll requests into a sorted
 list

This is preparation for finding interval common denominators (root intervals)

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
---
 src/modules/mavlink/mavlink_stream.cpp | 25 +++++++------
 src/modules/mavlink/mavlink_stream.h   | 51 ++++++++++++++++++++++++--
 2 files changed, 61 insertions(+), 15 deletions(-)

diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index 54d44ae83606..b4b1cd4d61b2 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -143,7 +143,7 @@ MavlinkStreamPoll::~MavlinkStreamPoll()
 {
 	for (auto req : _reqs) {
 		_reqs.remove(req);
-		hrt_cancel(&req->_hrt_req);
+		req->stop_interval();
 		delete (req);
 	}
 
@@ -153,16 +153,17 @@ MavlinkStreamPoll::~MavlinkStreamPoll()
 int
 MavlinkStreamPoll::register_poll(uint16_t stream_id, uint32_t interval_ms)
 {
+	MavStreamPollReq *req = new MavStreamPollReq(stream_id, interval_ms);
+
+	if (req) {
+		pthread_mutex_lock(&_mtx);
+		_reqs.add_sorted(req);
+		req->start_interval(hrt_callback, &_poll_sem);
+		pthread_mutex_unlock(&_mtx);
+		return OK;
+	}
 
-	MavStreamPollReq *req = new MavStreamPollReq(stream_id);
-
-	pthread_mutex_lock(&_mtx);
-	_reqs.add(req);
-	pthread_mutex_unlock(&_mtx);
-
-	hrt_call_every(&req->_hrt_req, (hrt_abstime)interval_ms * 1000,
-		       (hrt_abstime)interval_ms * 1000, hrt_callback, &_poll_sem);
-	return OK;
+	return -ENOMEM;
 }
 
 int
@@ -172,9 +173,9 @@ MavlinkStreamPoll::unregister_poll(uint16_t stream_id)
 	pthread_mutex_lock(&_mtx);
 
 	for (auto req : _reqs) {
-		if (req->_stream_id == stream_id) {
+		if (req->stream_id() == stream_id) {
 			_reqs.remove(req);
-			hrt_cancel(&req->_hrt_req);
+			req->stop_interval();
 			delete (req);
 			break;
 		}
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index ff1209c76eb3..52872b9cef0d 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -177,17 +177,62 @@ class MavlinkStreamPoll
 	class MavStreamPollReq :  public ListNode<MavStreamPollReq *>
 	{
 	public:
-		MavStreamPollReq(uint16_t stream_id) : _stream_id(stream_id) {}
-		uint16_t    _stream_id;
+		MavStreamPollReq(uint16_t stream_id, uint32_t interval_ms) : _stream_id(stream_id), _interval_ms(interval_ms),
+			_is_root(false) {}
+		void start_interval(hrt_callout cb, px4_sem_t *sem)
+		{
+			_is_root = true;
+			hrt_call_every(&_hrt_req, (hrt_abstime)_interval_ms * 1000,
+				       (hrt_abstime)_interval_ms * 1000, cb, sem);
+		}
+
+		void stop_interval() { _is_root = false; hrt_cancel(&_hrt_req); }
+
+		uint32_t interval_ms() { return _interval_ms; }
+		uint16_t stream_id() { return _stream_id; }
+		bool is_root() { return _is_root; }
+	private:
+		uint16_t        _stream_id;
+		uint32_t        _interval_ms;
+		bool            _is_root;
 		struct hrt_call _hrt_req;
 	};
 
+	class MavlinkStreamPollReqList : public List<MavStreamPollReq *>
+	{
+	public:
+		void add_sorted(MavStreamPollReq *req)
+		{
+			if (_head == nullptr || _head->interval_ms() > req->interval_ms()) {
+				// 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_ms() > req->interval_ms()) {
+						// found the end or the correct place
+						req->setSibling(node->getSibling());
+						node->setSibling(req);
+						return;
+					}
+
+					node = node->getSibling();
+				}
+			}
+		}
+	};
+
 	static void hrt_callback(void *arg);
 
 	/**
 	 * Requests from stream objects
 	 */
-	List<class MavStreamPollReq *> _reqs;
+	MavlinkStreamPollReqList _reqs;
 
 	/**
 	 * Signalling semaphore to release the poll