From 8521d9b68d428bcfaf3b8d7089d1b45e1b84674e Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 7 Jun 2024 11:11:26 +0300 Subject: [PATCH] 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 --- src/modules/mavlink/mavlink_stream.cpp | 55 ++++++++++++++++++---- src/modules/mavlink/mavlink_stream.h | 65 ++++++++++++++++++++++++-- 2 files changed, 108 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index f414cefb733c..3a08186baf3a 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/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; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 3d5c6e809be7..999447ca5908 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -177,16 +177,73 @@ class MavlinkStreamPoll class MavStreamPollReq : public ListNode { 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 + { + 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 _reqs; + MavlinkStreamPollReqList _reqs; /** * Signalling semaphore to release the poll