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_ */