From 2251ae4ef5f2d81a12be51112edc0056ffeb2824 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Fri, 7 Jun 2024 09:31:27 +0300 Subject: [PATCH] Revert "mavlink_main: use orb polling" This reverts commit 090bb19df991e994f46db66749215ef93df29fde. --- src/modules/mavlink/Kconfig | 8 - src/modules/mavlink/mavlink_main.cpp | 73 +------- src/modules/mavlink/mavlink_main.h | 12 -- src/modules/mavlink/mavlink_stream.cpp | 229 ------------------------- src/modules/mavlink/mavlink_stream.h | 97 +---------- 5 files changed, 3 insertions(+), 416 deletions(-) diff --git a/src/modules/mavlink/Kconfig b/src/modules/mavlink/Kconfig index e3e07c6f7ac6..8fb9219d9d61 100644 --- a/src/modules/mavlink/Kconfig +++ b/src/modules/mavlink/Kconfig @@ -24,11 +24,3 @@ menuconfig USER_MAVLINK depends on BOARD_PROTECTED && MODULES_MAVLINK ---help--- Put mavlink in userspace memory - -menuconfig MAVLINK_UORB_POLL - bool "mavlink uorb poll in main loop" - default n - depends on MODULES_MAVLINK - ---help--- - Use uorb topic polling in mavlink main loop instead of - using px4_usleep for stream rate handling. diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index c88a154115ca..26e86c64df29 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -134,10 +134,6 @@ Mavlink::Mavlink() : _event_sub.subscribe(); _telemetry_status_pub.advertise(); - -#if defined(CONFIG_MAVLINK_UORB_POLL) - _stream_poller = new MavlinkStreamPoll(); -#endif } Mavlink::~Mavlink() @@ -179,14 +175,6 @@ Mavlink::~Mavlink() } } -#if defined(CONFIG_MAVLINK_UORB_POLL) - - if (_stream_poller) { - delete _stream_poller; - } - -#endif - perf_free(_loop_perf); perf_free(_loop_interval_perf); perf_free(_send_byte_error_perf); @@ -1168,7 +1156,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 */ - set_stream_interval(stream, interval); + stream->set_interval(interval); } else { /* delete stream */ @@ -1185,7 +1173,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate) MavlinkStream *stream = create_mavlink_stream(stream_name, this); if (stream != nullptr) { - set_stream_interval(stream, interval); + stream->set_interval(interval); _streams.add(stream); return OK; @@ -2337,29 +2325,9 @@ Mavlink::task_main(int argc, char *argv[]) _task_running.store(true); -#if defined(CONFIG_MAVLINK_UORB_POLL) - int uorb_poll_error_counter = 0; -#endif - while (!should_exit()) { /* main loop */ -#if defined(CONFIG_MAVLINK_UORB_POLL) - int uorb_poll_ret = _stream_poller->poll(MAIN_LOOP_DELAY); - - if (uorb_poll_ret < 0) { - /* this is seriously bad - should be an emergency */ - if (uorb_poll_error_counter < 10 || uorb_poll_error_counter % 50 == 0) { - /* use a counter to prevent flooding (and slowing us down) */ - PX4_ERR("ERROR while polling uorbs: %d", uorb_poll_ret); - } - - uorb_poll_error_counter++; - } - - _stream_poller->ack_all(); -#else px4_usleep(_main_loop_delay); -#endif if (!should_transmit()) { check_requested_subscriptions(); @@ -2881,43 +2849,6 @@ void Mavlink::configure_sik_radio() } } - -int -Mavlink::register_orb_poll(uint16_t stream_id, ORB_ID *orbs, int count) -{ -#if defined(CONFIG_MAVLINK_UORB_POLL) - return _stream_poller->register_orbs(stream_id, orbs, count); -#else - (void)stream_id; - (void)orbs; - (void)count; - return PX4_OK; -#endif -} - -int -Mavlink::unregister_orb_poll(uint16_t stream_id) -{ -#if defined(CONFIG_MAVLINK_UORB_POLL) - return _stream_poller->unregister_orbs(stream_id); -#else - (void)stream_id; - return PX4_OK; -#endif -} - -int -Mavlink::set_stream_interval(MavlinkStream *stream, int interval) -{ - stream->set_interval(interval); -#if defined(CONFIG_MAVLINK_UORB_POLL) - return _stream_poller->set_interval(stream->get_id(), interval / 1000); -#else - return PX4_OK; -#endif -} - - 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 cfbfbb129aa0..85c0361de3ad 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -524,19 +524,7 @@ class Mavlink final : public ModuleParams bool radio_status_critical() const { return _radio_status_critical; } - /** - * Register/Unregister a stream orbs for polling - */ - int register_orb_poll(uint16_t stream_id, ORB_ID *orbs, int count); - int unregister_orb_poll(uint16_t stream_id); - - int set_stream_interval(MavlinkStream *stream, int interval); - private: -#if defined(CONFIG_MAVLINK_UORB_POLL) - MavlinkStreamPoll *_stream_poller {nullptr}; -#endif - MavlinkReceiver _receiver; int _instance_id{-1}; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 98fafdf95ec7..6b00fd4e49f5 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -128,232 +128,3 @@ MavlinkStream::update(const hrt_abstime &t) return -1; } - -#if defined(CONFIG_MAVLINK_UORB_POLL) -// 20ms default update rate for polled topics, if not set otherwise -const unsigned int MavlinkStreamDefaultTopicInterval = 20; - -MavlinkStreamPoll::MavlinkStreamPoll() : - _fds{nullptr}, - _orbs{nullptr}, - _reqs{nullptr}, - _reqs_capacity{16}, - _reqs_count{0}, - _capacity{16}, - _count{0} -{ - _orbs = (MavStreamPollItem *)malloc(16 * sizeof(MavStreamPollItem)); - _fds = (orb_poll_struct_t *)malloc(16 * sizeof(orb_poll_struct_t)); - _reqs = (MavStreamOrbPollReq *)malloc(16 * sizeof(MavStreamOrbPollReq)); -} - -MavlinkStreamPoll::~MavlinkStreamPoll() -{ - if (_fds) { - free(_fds); - } - - if (_orbs) { - free(_orbs); - } - - if (_reqs) { - free(_reqs); - } -} - -int -MavlinkStreamPoll::_add_orb(ORB_ID orb_id, int interval_ms) -{ - // Check if the orb is already in the list - bool exists = false; - - for (int i = 0; i < _count; i++) { - if (_orbs[i].orb_id == orb_id && _orbs[i].interval == interval_ms) { - _orbs[i].usage_count++; - exists = true; - break; - } - } - - // Did not exist, add new one to the list - if (!exists) { - while (_count >= _capacity) { - _capacity = _capacity + 16; - _orbs = (MavStreamPollItem *)realloc(_orbs, _capacity * sizeof(MavStreamPollItem)); - _fds = (orb_poll_struct_t *)realloc(_fds, _capacity * sizeof(orb_poll_struct_t)); - - if (!_fds || !_orbs) { - PX4_ERR("failed to allocate memory for orb poll items"); - return PX4_ERROR; - } - } - - _orbs[_count].orb_id = orb_id; - _orbs[_count].interval = interval_ms; - _orbs[_count].usage_count = 1; - _orbs[_count].fd = orb_subscribe(get_orb_meta(orb_id)); - orb_set_interval(_orbs[_count].fd, interval_ms); - _count++; - } - - return PX4_OK; -} - -int -MavlinkStreamPoll::_remove_orb(ORB_ID orb_id, int interval_ms) -{ - // Decrement usage/remove the item from the orbs list - for (int i = 0; i < _count; i++) { - if (_orbs[i].orb_id == orb_id && - _orbs[i].interval == interval_ms) { - _orbs[i].usage_count--; - - // If this was the last request for the orb, - // unsubscribe and remove from orbs list - if (_orbs[i].usage_count == 0) { - orb_unsubscribe(_orbs[i].fd); - // Replace the current item with - // the last item in the _orbs list. - // Loop counting is not disturbed because - // we break out from loop after this. - _orbs[i] = _orbs[--_count]; - } - - break; - } - } - - return PX4_OK; -} - -/** - * Register stream object to poll list of orbs - */ -int -MavlinkStreamPoll::register_orbs(uint16_t stream_id, ORB_ID *orbs, int cnt) -{ - if (!orbs || cnt <= 0) { - return PX4_OK; - } - - for (int i = 0; i < cnt; i++) { - - // Increase reqs capacity if necessary - while (_reqs_count >= _reqs_capacity) { - _reqs_capacity = _reqs_capacity + 16; - _reqs = (MavStreamOrbPollReq *)realloc(_reqs, _reqs_capacity * sizeof(MavStreamOrbPollReq)); - - if (!_reqs) { - PX4_ERR("failed to allocate memory for orb poll reqs"); - return PX4_ERROR; - } - } - - MavStreamOrbPollReq *req = &_reqs[_reqs_count]; - req->stream_id = stream_id; - req->orb_id = orbs[i]; - req->interval = MavlinkStreamDefaultTopicInterval; - _reqs_count++; - - // Update orbs list - _add_orb(orbs[i], req->interval); - } - - // Update fds - for (int i = 0; i < _count; i++) { - _fds[i].fd = _orbs[i].fd; - _fds[i].events = POLLIN; - _fds[i].revents = 0; - } - - return PX4_OK; -} - -/** - * Unregister stream object from orbs polling - */ -int -MavlinkStreamPoll::unregister_orbs(uint16_t stream_id) -{ - int i = 0; - - while (i < _reqs_count) { - if (_reqs[i].stream_id == stream_id) { - - // Remove orb from the orbs list - _remove_orb(_reqs[i].orb_id, _reqs[i].interval); - - // Finally, replace the current item with - // the last item in the _reqs list - _reqs[i] = _reqs[--_reqs_count]; - - } else { - // Only increment if current _reqs item is not removed. - // Otherwise we have moved the last item to the current - // position, so we need to check the same index again. - i++; - } - } - - // Update fds - for (int j = 0; j < _count; j++) { - _fds[j].fd = _orbs[j].fd; - _fds[j].events = POLLIN; - _fds[j].revents = 0; - } - - return PX4_OK; -} - -/** - * Set stream update interval to adjust orb polling accordingly - */ -int -MavlinkStreamPoll::set_interval(uint16_t stream_id, int interval_ms) -{ - // Update interval for all orb subscriptions and find the maximum interval - for (int i = 0; i < _count; i++) { - if (_reqs[i].stream_id == stream_id) { - - // Remove orb_id subscription with current old interval - _remove_orb(_reqs[i].orb_id, _reqs[i].interval); - - // Update interval - _reqs[i].interval = interval_ms; - - // Add orb_id subscription with new interval - _add_orb(_reqs[i].orb_id, _reqs[i].interval); - - } - } - - return PX4_OK; -} - -/** - * Perform orb polling - */ -int -MavlinkStreamPoll::poll(const hrt_abstime timeout) -{ - int timeout_ms = timeout / 1000; - - if (timeout_ms <= 0) { - timeout_ms = 1; - } - - return px4_poll(_fds, _count, timeout_ms); -} - -/** - * Acknowledge all orb data for next poll - */ -void -MavlinkStreamPoll::ack_all() -{ - for (int i = 0; i < _count; i++) { - orb_ack(_orbs[i].fd); - } -} -#endif /* CONFIG_MAVLINK_UORB_POLL */ diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 18fa1c5c794f..67034e0c9304 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -44,7 +44,6 @@ #include #include #include -#include class Mavlink; @@ -67,10 +66,7 @@ class MavlinkStream : public ListNode * * @param interval the interval in microseconds (us) between messages */ - void set_interval(const int interval) - { - _interval = interval; - } + void set_interval(const int interval) { _interval = interval; } /** * Get the interval @@ -145,96 +141,5 @@ class MavlinkStream : public ListNode bool _first_message_sent{false}; }; -#if defined(CONFIG_MAVLINK_UORB_POLL) - -/** - * Structure of objects in _reqs list - */ -struct MavStreamOrbPollReq { - uint16_t stream_id; - ORB_ID orb_id; - int interval; -}; - -/** - * Structure of objects in _orbs list - */ -struct MavStreamPollItem { - ORB_ID orb_id; - int interval; - int usage_count; - orb_sub_t fd; -}; - -class MavlinkStreamPoll -{ -public: - MavlinkStreamPoll(); - ~MavlinkStreamPoll(); - - /** - * Add a stream to the poll list - */ - int register_orbs(uint16_t stream_id, ORB_ID *orbs, int cnt); - - /** - * Remove a stream from the poll list - */ - int unregister_orbs(uint16_t stream_id); - - /** - * Set stream update interval - */ - int set_interval(uint16_t stream_id, int interval_ms); - - /** - * Poll all streams for updates - */ - int poll(const hrt_abstime timeout); - - /** - * Acknowledge all orb data for next poll - */ - void ack_all(); - -private: - - /** - * Add a orb_id/interval pair to the orbs list - */ - int _add_orb(ORB_ID orb_id, int interval_ms); - - /** - * Remove a orb_id/interval pair from the orbs list - */ - int _remove_orb(ORB_ID orb_id, int interval_ms); - - /** - * Poll file descriptors for updates - */ - orb_poll_struct_t *_fds; - - /** - * List of different orbs to poll - */ - MavStreamPollItem *_orbs; - - /** - * Requests from stream objects, contains orb poll requests - * count and capacity of the requests list - */ - MavStreamOrbPollReq *_reqs; - int _reqs_capacity; - int _reqs_count; - - /** - * Count and capacity of the orbs/fds lists - */ - int _capacity; - int _count; - -}; - -#endif /* CONFIG_MAVLINK_UORB_POLL */ #endif /* MAVLINK_STREAM_H_ */