mavlink: Optimize mavlink polling with HRT timer

Only use the needed HRT intervals, i.e. the shortest one by which all the other ones are evenly divisible with.

Signed-off-by: Jukka Laitinen <[email protected]>

src/modules/mavlink/mavlink_stream.cpp

@@ -155,6 +155,34 @@ MavlinkStreamPoll::~MavlinkStreamPoll()
 	pthread_mutex_destroy(&_mtx);
 }
 
+void MavlinkStreamPoll::recalculate_roots_and_start(MavStreamPollReq *req)
+{
+	// Now go through the ordered _reqs list, 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 == 0) {
+				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)
 {
@@ -164,31 +192,42 @@ MavlinkStreamPoll::register_poll(uint16_t stream_id, uint32_t interval_us)
 		return OK;
 	}
 
-	MavStreamPollReq *req = new MavStreamPollReq(stream_id);
+	MavStreamPollReq *req = new MavStreamPollReq(stream_id, interval_us);
 
 	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);
 
+	recalculate_roots_and_start(req);
+	_reqs.add_sorted(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) {
+		if (req->stream_id() == stream_id) {
 			_reqs.remove(req);
-			hrt_cancel(&req->_hrt_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;
 		}

src/modules/mavlink/mavlink_stream.h

@@ -177,16 +177,73 @@ class MavlinkStreamPoll
 	class MavStreamPollReq :  public ListNode<MavStreamPollReq *>
 	{
 	public:
-		MavStreamPollReq(uint16_t stream_id) : _stream_id(stream_id) {}
+		MavStreamPollReq(uint16_t stream_id, uint32_t interval_us) : _stream_id(stream_id), _interval_us(interval_us),
+			_is_root(false) {}
 		~MavStreamPollReq()
 		{
-			hrt_cancel(&_hrt_req);
+			if (_is_root) {
+				hrt_cancel(&_hrt_req);
+			}
 		}
 
-		uint16_t    _stream_id;
+		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
@@ -196,7 +253,7 @@ class MavlinkStreamPoll
 	/**
 	 * Requests from stream objects
 	 */
-	List<class MavStreamPollReq *> _reqs;
+	MavlinkStreamPollReqList _reqs;
 
 	/**
 	 * Signalling semaphore to release the poll