From ec5088ae5231904b23cf6b07af9cec0322ab817d 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 | 117 +++++++++++++++++++++++++ src/modules/mavlink/mavlink_stream.h | 67 ++++++++++++++ 4 files changed, 214 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 26e86c64df29..32389b719130 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 && mavlink_poll_ret != -ETIMEDOUT) { + /* 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..f414cefb733c 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,113 @@ 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); +} + +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); + + 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); + + 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); + hrt_cancel(&req->_hrt_req); + 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_us) +{ + 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 + timeout_us); + + ret = px4_sem_timedwait(&_poll_sem, &to); + + if (ret < 0) { + ret = -errno; + } + + 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..3d5c6e809be7 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,71 @@ 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_us); + +private: + + class MavStreamPollReq : public ListNode + { + public: + MavStreamPollReq(uint16_t stream_id) : _stream_id(stream_id) {} + ~MavStreamPollReq() + { + hrt_cancel(&_hrt_req); + } + + uint16_t _stream_id; + struct hrt_call _hrt_req; + }; + + + /** + * HRT interrupt callback posting the semaphore + */ + static void hrt_callback(void *arg); + + /** + * Requests from stream objects + */ + List _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_ */