From 36f15c4717efc8a541e973fb0e1aa339e841aac4 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Thu, 6 Jun 2024 15:13:52 +0300 Subject: [PATCH] Implement HRT-based polling into mavlink main loop instead of sleep Sleeping depends on the systick, which prevents using slower than 1ms systicks. Using HRT provides as performant way of handling the mavlink polling and is also more accurate. Co-authored-by: Jari Nippula Signed-off-by: Jukka Laitinen --- src/modules/mavlink/mavlink_main.cpp | 29 ++++- src/modules/mavlink/mavlink_main.h | 4 + src/modules/mavlink/mavlink_stream.cpp | 151 +++++++++++++++++++++++++ src/modules/mavlink/mavlink_stream.h | 124 ++++++++++++++++++++ 4 files changed, 305 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 26e86c64df29..2d23c2e29e37 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -134,6 +134,7 @@ Mavlink::Mavlink() : _event_sub.subscribe(); _telemetry_status_pub.advertise(); + _stream_poller = new MavlinkStreamPoll(); } Mavlink::~Mavlink() @@ -175,6 +176,8 @@ Mavlink::~Mavlink() } } + delete _stream_poller; + perf_free(_loop_perf); perf_free(_loop_interval_perf); perf_free(_send_byte_error_perf); @@ -1156,7 +1159,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) if (strcmp(stream_name, stream->get_name()) == 0) { if (interval != 0) { /* set new interval */ - stream->set_interval(interval); + set_stream_interval(stream, interval); } else { /* delete stream */ @@ -1173,7 +1176,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) MavlinkStream *stream = create_mavlink_stream(stream_name, this); if (stream != nullptr) { - stream->set_interval(interval); + set_stream_interval(stream, interval); _streams.add(stream); return OK; @@ -2325,9 +2328,22 @@ Mavlink::task_main(int argc, char *argv[]) _task_running.store(true); + int mavlink_poll_error_counter = 0; + while (!should_exit()) { /* main loop */ - px4_usleep(_main_loop_delay); + + int mavlink_poll_ret = _stream_poller->poll(MAIN_LOOP_DELAY); + + if (mavlink_poll_ret < 0) { + /* this is seriously bad - should be an emergency */ + if (mavlink_poll_error_counter < 10 || mavlink_poll_error_counter % 50 == 0) { + /* use a counter to prevent flooding (and slowing us down) */ + PX4_ERR("ERROR while polling streams: %d", mavlink_poll_ret); + } + + mavlink_poll_error_counter++; + } if (!should_transmit()) { check_requested_subscriptions(); @@ -2849,6 +2865,13 @@ void Mavlink::configure_sik_radio() } } +int +Mavlink::set_stream_interval(MavlinkStream *stream, int interval) +{ + stream->set_interval(interval); + return _stream_poller->set_interval(stream->get_id(), interval); +} + int Mavlink::start_helper(int argc, char *argv[]) { /* create the instance in task context */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 85c0361de3ad..f9ff0a45f827 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -524,7 +524,11 @@ class Mavlink final : public ModuleParams bool radio_status_critical() const { return _radio_status_critical; } + int set_stream_interval(MavlinkStream *stream, int interval); + private: + MavlinkStreamPoll *_stream_poller {nullptr}; + MavlinkReceiver _receiver; int _instance_id{-1}; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 6b00fd4e49f5..c2ef5b33122a 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -44,6 +44,13 @@ #include "mavlink_stream.h" #include "mavlink_main.h" +/** + * If stream rate is set to unlimited, set the rate to 50 Hz. To get higher + * rates, it needs to be set explicitly. + */ + +const uint32_t MavlinkStreamUnlimitedInterval = 20000; + MavlinkStream::MavlinkStream(Mavlink *mavlink) : _mavlink(mavlink) { @@ -128,3 +135,147 @@ MavlinkStream::update(const hrt_abstime &t) return -1; } + +MavlinkStreamPoll::MavlinkStreamPoll() +{ + px4_sem_init(&_poll_sem, 1, 0); +#if defined(__PX4_NUTTX) + sem_setprotocol(&_poll_sem, SEM_PRIO_NONE); +#endif + + pthread_mutex_init(&_mtx, nullptr); +} + +MavlinkStreamPoll::~MavlinkStreamPoll() +{ + // 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_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) { + 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) +{ + // Streans with interval 0 are disabled and don't need to be registered here + + if (interval_us == 0) { + return OK; + } + + MavStreamPollReq *req = new MavStreamPollReq(stream_id, interval_us); + + 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); + + 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; + } + } + + pthread_mutex_unlock(&_mtx); + + return OK; +} + +int +MavlinkStreamPoll::set_interval(uint16_t stream_id, int interval_us) +{ + unregister_poll(stream_id); + + if (interval_us < 0) { + interval_us = MavlinkStreamUnlimitedInterval; + } + + return register_poll(stream_id, interval_us); +} + +/** + * Perform orb polling + */ +int +MavlinkStreamPoll::poll(const hrt_abstime timeout) +{ + int ret; + + // Wait event for a maximum timeout time + + struct timespec to; +#if defined(CONFIG_ARCH_BOARD_PX4_SITL) + px4_clock_gettime(CLOCK_MONOTONIC, &to); +#else + px4_clock_gettime(CLOCK_REALTIME, &to); +#endif + hrt_abstime now = ts_to_abstime(&to); + abstime_to_ts(&to, now + (hrt_abstime)timeout * 1000); + + ret = px4_sem_timedwait(&_poll_sem, &to); + + return ret; +} + +void +MavlinkStreamPoll::hrt_callback(void *arg) +{ + px4_sem_post((px4_sem_t *)arg); +} + diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 67034e0c9304..26a9c6b87bdc 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -43,6 +43,7 @@ #include #include +#include #include class Mavlink; @@ -141,5 +142,128 @@ class MavlinkStream : public ListNode bool _first_message_sent{false}; }; +/** + * Class to manage polling of stream intervals + */ + +class MavlinkStreamPoll +{ +public: + MavlinkStreamPoll(); + ~MavlinkStreamPoll(); + + /** + * Add a stream to the poll list + */ + int register_poll(uint16_t stream_id, uint32_t interval_us); + + /** + * Remove a stream from the poll list + */ + int unregister_poll(uint16_t stream_id); + + /** + * Re-set interval + */ + int set_interval(uint16_t stream_id, int interval_us); + + /** + * Poll all streams for updates + */ + int poll(const hrt_abstime timeout); + +private: + + class MavStreamPollReq : public ListNode + { + public: + MavStreamPollReq(uint16_t stream_id, uint32_t interval_us) : _stream_id(stream_id), _interval_us(interval_us), + _is_root(false) {} + ~MavStreamPollReq() + { + if (_is_root) { + hrt_cancel(&_hrt_req); + } + } + + 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 + */ + static void hrt_callback(void *arg); + + /** + * Requests from stream objects + */ + MavlinkStreamPollReqList _reqs; + + /** + * Signalling semaphore to release the poll + */ + px4_sem_t _poll_sem; + + /** + * Mutex to protect the list of poll request (hrt) items + */ + pthread_mutex_t _mtx {}; +}; #endif /* MAVLINK_STREAM_H_ */