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

@@ -141,41 +141,85 @@ MavlinkStreamPoll::MavlinkStreamPoll()
 
 MavlinkStreamPoll::~MavlinkStreamPoll()
 {
-	for (auto req : _reqs) {
-		_reqs.remove(req);
-		req->stop_interval();
-		delete (req);
-	}
+	// 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_ms = req->interval_ms();
+
+	for (auto r : _reqs) {
+		if (r->interval_ms() <= interval_ms) {
+			if (r->is_root() && interval_ms % r->interval_ms() == 0) {
+				is_root = false;
+			}
+
+		} else {
+			if (is_root && r->is_root() && r->interval_ms() % interval_ms) {
+				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_ms)
 {
-	MavStreamPollReq *req = new MavStreamPollReq(stream_id, interval_ms);
+	// Streans with interval 0 are disabled and don't need to be registered here
 
-	if (req) {
-		pthread_mutex_lock(&_mtx);
-		_reqs.add_sorted(req);
-		req->start_interval(hrt_callback, &_poll_sem);
-		pthread_mutex_unlock(&_mtx);
+	if (interval_ms == 0) {
 		return OK;
 	}
 
-	return -ENOMEM;
+	MavStreamPollReq *req = new MavStreamPollReq(stream_id, interval_ms);
+
+	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);
-			req->stop_interval();
+
+			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

@@ -179,6 +179,13 @@ class MavlinkStreamPoll
 	public:
 		MavStreamPollReq(uint16_t stream_id, uint32_t interval_ms) : _stream_id(stream_id), _interval_ms(interval_ms),
 			_is_root(false) {}
+		~MavStreamPollReq()
+		{
+			if (_is_root) {
+				hrt_cancel(&_hrt_req);
+			}
+		}
+
 		void start_interval(hrt_callout cb, px4_sem_t *sem)
 		{
 			_is_root = true;
@@ -227,6 +234,20 @@ class MavlinkStreamPoll
 		}
 	};
 
+	/**
+	 * 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);
 
 	/**