From 57ea7389f2716d523a8ec791b60cf6cae1085ab4 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 7 Jun 2024 13:23:36 +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 | 72 +++++++++++++++++++++----- src/modules/mavlink/mavlink_stream.h | 21 ++++++++ 2 files changed, 79 insertions(+), 14 deletions(-) diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index b4b1cd4d61b2..6f3b90386c46 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/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; } diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 52872b9cef0d..b81df89736b2 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/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); /**