diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index eefa7e310413..6850e280d5e3 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -143,17 +143,6 @@ class Subscription return valid() ? Manager::orb_data_copy(_node, dst, _last_generation, true) : false; } - void ack() - { - if (!valid()) { - subscribe(); - } - - if (valid()) { - Manager::orb_data_ack(_node, _last_generation); - } - } - /** * Copy the struct * @param dst The uORB message struct we are updating. diff --git a/platforms/common/uORB/SubscriptionInterval.hpp b/platforms/common/uORB/SubscriptionInterval.hpp index 045011a00634..83f27a520dea 100644 --- a/platforms/common/uORB/SubscriptionInterval.hpp +++ b/platforms/common/uORB/SubscriptionInterval.hpp @@ -100,17 +100,6 @@ class SubscriptionInterval return false; } - /** - * Acknowledge a new update. - */ - void ack() - { - _subscription.ack(); - const hrt_abstime now = hrt_absolute_time(); - // shift last update time forward, but don't let it get further behind than the interval - _last_update = math::constrain(_last_update + _interval_us, now - _interval_us, now); - } - /** * Copy the struct if updated. * @param dst The destination pointer where the struct will be copied. diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index 3dcf20155e4d..d09be27d6326 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -130,11 +130,6 @@ int orb_check(orb_sub_t handle, bool *updated) return uORB::Manager::get_instance()->orb_check(handle, updated); } -int orb_ack(orb_sub_t handle) -{ - return uORB::Manager::get_instance()->orb_ack(handle); -} - int orb_exists(const struct orb_metadata *meta, int instance) { return uORB::Manager::orb_exists(meta, instance); diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index 3b3b80ee7ee1..2006ef08ebcd 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -246,8 +246,6 @@ extern int orb_copy(const struct orb_metadata *meta, orb_sub_t handle, void *buf */ extern int orb_check(orb_sub_t handle, bool *updated) __EXPORT; -extern int orb_ack(orb_sub_t handle) __EXPORT; - /** * @see uORB::Manager::orb_exists() */ diff --git a/platforms/common/uORB/uORBDeviceNode.hpp b/platforms/common/uORB/uORBDeviceNode.hpp index 83e1d5752e33..c0f94174b757 100644 --- a/platforms/common/uORB/uORBDeviceNode.hpp +++ b/platforms/common/uORB/uORBDeviceNode.hpp @@ -219,8 +219,6 @@ class DeviceNode */ bool copy(void *dst, orb_advert_t &handle, unsigned &generation); - void ack(unsigned &generation) { generation = _generation.load(); } - static bool register_callback(orb_advert_t &node_handle, SubscriptionCallback *callback_sub, int8_t poll_lock, hrt_abstime last_update, uint32_t interval_us, uorb_cb_handle_t &cb_handle) { diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index e2846c096bbc..73f559470db1 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -365,12 +365,6 @@ int uORB::Manager::orb_check(orb_sub_t handle, bool *updated) return PX4_OK; } -int uORB::Manager::orb_ack(orb_sub_t handle) -{ - ((uORB::SubscriptionInterval *)handle)->ack(); - return PX4_OK; -} - int uORB::Manager::orb_set_interval(orb_sub_t handle, unsigned interval) { ((uORB::SubscriptionInterval *)handle)->set_interval_us(interval * 1000); diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 96e95c7b04b5..cfc297cfedc1 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -292,9 +292,6 @@ class Manager */ static int orb_check(orb_sub_t handle, bool *updated); - - static int orb_ack(orb_sub_t handle); - /** * Check if a topic has already been created and published (advertised) * @@ -360,11 +357,6 @@ class Manager static uint8_t orb_get_queue_size(const orb_advert_t &node_handle) {return node(node_handle)->get_queue_size();} - static void orb_data_ack(orb_advert_t &node_handle, unsigned &generation) - { - node(node_handle)->ack(generation); - } - static bool orb_data_copy(orb_advert_t &node_handle, void *dst, unsigned &generation, bool only_if_updated) { 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..32389b719130 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -134,10 +134,7 @@ Mavlink::Mavlink() : _event_sub.subscribe(); _telemetry_status_pub.advertise(); - -#if defined(CONFIG_MAVLINK_UORB_POLL) _stream_poller = new MavlinkStreamPoll(); -#endif } Mavlink::~Mavlink() @@ -179,13 +176,7 @@ Mavlink::~Mavlink() } } -#if defined(CONFIG_MAVLINK_UORB_POLL) - - if (_stream_poller) { - delete _stream_poller; - } - -#endif + delete _stream_poller; perf_free(_loop_perf); perf_free(_loop_interval_perf); @@ -2337,30 +2328,23 @@ Mavlink::task_main(int argc, char *argv[]) _task_running.store(true); -#if defined(CONFIG_MAVLINK_UORB_POLL) - int uorb_poll_error_counter = 0; -#endif + int mavlink_poll_error_counter = 0; 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) { + 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 (uorb_poll_error_counter < 10 || uorb_poll_error_counter % 50 == 0) { + 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 uorbs: %d", uorb_poll_ret); + PX4_ERR("ERROR while polling streams: %d", mavlink_poll_ret); } - uorb_poll_error_counter++; + mavlink_poll_error_counter++; } - _stream_poller->ack_all(); -#else - px4_usleep(_main_loop_delay); -#endif - if (!should_transmit()) { check_requested_subscriptions(); continue; @@ -2881,43 +2865,13 @@ 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 + 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 cfbfbb129aa0..f9ff0a45f827 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -524,18 +524,10 @@ 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; diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index b5e3e53d9da2..3a08186baf3a 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) { @@ -129,262 +136,151 @@ 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} +MavlinkStreamPoll::MavlinkStreamPoll() { - _orbs = (MavStreamPollItem *)malloc(16 * sizeof(MavStreamPollItem)); - _fds = (orb_poll_struct_t *)malloc(16 * sizeof(orb_poll_struct_t)); - _reqs = (MavStreamOrbPollReq *)malloc(16 * sizeof(MavStreamOrbPollReq)); + 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() { - if (_fds) { - free(_fds); - } - - if (_orbs) { - free(_orbs); - } - - if (_reqs) { - free(_reqs); - } + // This removes and deletes every object in the list + _reqs.clear(); + px4_sem_destroy(&_poll_sem); pthread_mutex_destroy(&_mtx); } -int -MavlinkStreamPoll::_add_orb(ORB_ID orb_id, int interval_ms) +void MavlinkStreamPoll::recalculate_roots_and_start(MavStreamPollReq *req) { - // Check if the orb is already in the list - bool exists = false; + // Now go through the ordered _reqs list, to see if this timer needs to + // be started, and if some others can be stopped - 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; - } - } + bool is_root = true; + uint32_t interval_us = req->interval_us(); - // 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)); + for (auto r : _reqs) { + if (r->interval_us() <= interval_us) { + if (r->is_root() && interval_us % r->interval_us() == 0) { + is_root = false; + } - if (!_fds || !_orbs) { - PX4_ERR("failed to allocate memory for orb poll items"); - return PX4_ERROR; + } else { + if (is_root && r->is_root() && r->interval_us() % interval_us == 0) { + r->stop_interval(); } } - - _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; + // If this was a new root interval, start the hrt + + if (is_root) { + req->start_interval(hrt_callback, &_poll_sem); + } } int -MavlinkStreamPoll::_remove_orb(ORB_ID orb_id, int interval_ms) +MavlinkStreamPoll::register_poll(uint16_t stream_id, uint32_t interval_us) { - // 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]; - } + // Streans with interval 0 are disabled and don't need to be registered here - break; - } + if (interval_us == 0) { + return OK; } - return PX4_OK; -} + MavStreamPollReq *req = new MavStreamPollReq(stream_id, interval_us); -/** - * 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; + if (req == nullptr) { + return -ENOMEM; } pthread_mutex_lock(&_mtx); - 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"); - pthread_mutex_unlock(&_mtx); - 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); - } + recalculate_roots_and_start(req); + _reqs.add_sorted(req); - // Update fds - for (int i = 0; i < _count; i++) { - _fds[i].fd = _orbs[i].fd; - _fds[i].events = POLLIN; - _fds[i].revents = 0; - } pthread_mutex_unlock(&_mtx); - return PX4_OK; + return OK; } -/** - * Unregister stream object from orbs polling - */ int -MavlinkStreamPoll::unregister_orbs(uint16_t stream_id) +MavlinkStreamPoll::unregister_poll(uint16_t stream_id) { - int i = 0; - pthread_mutex_lock(&_mtx); - while (i < _reqs_count) { - if (_reqs[i].stream_id == stream_id) { + for (auto req : _reqs) { + if (req->stream_id() == stream_id) { + _reqs.remove(req); - // Remove orb from the orbs list - _remove_orb(_reqs[i].orb_id, _reqs[i].interval); + if (req->is_root()) { + // This interval may be driving other streams. Re-calculate root clocks for all the + // remaining requests - // Finally, replace the current item with - // the last item in the _reqs list - _reqs[i] = _reqs[--_reqs_count]; + for (auto r : _reqs) { + recalculate_roots_and_start(r); + } - } 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++; - } - } + req->stop_interval(); + } - // Update fds - for (int j = 0; j < _count; j++) { - _fds[j].fd = _orbs[j].fd; - _fds[j].events = POLLIN; - _fds[j].revents = 0; + delete (req); + break; + } } pthread_mutex_unlock(&_mtx); - return PX4_OK; + + return OK; } -/** - * Set stream update interval to adjust orb polling accordingly - */ int -MavlinkStreamPoll::set_interval(uint16_t stream_id, int interval_ms) +MavlinkStreamPoll::set_interval(uint16_t stream_id, int interval_us) { - pthread_mutex_lock(&_mtx); - - // Renew all uorb subscriptions of given stream with new interval value - 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; + unregister_poll(stream_id); - // Add orb_id subscription with new interval - _add_orb(_reqs[i].orb_id, _reqs[i].interval); - - } + if (interval_us < 0) { + interval_us = MavlinkStreamUnlimitedInterval; } - // Update fds - for (int j = 0; j < _count; j++) { - _fds[j].fd = _orbs[j].fd; - _fds[j].events = POLLIN; - _fds[j].revents = 0; - } - - pthread_mutex_unlock(&_mtx); - return PX4_OK; + return register_poll(stream_id, interval_us); } /** * Perform orb polling */ int -MavlinkStreamPoll::poll(const hrt_abstime timeout) +MavlinkStreamPoll::poll(const hrt_abstime timeout_us) { - int timeout_ms = timeout / 1000; int ret; - if (timeout_ms <= 0) { - timeout_ms = 1; - } + // Wait event for a maximum timeout time - pthread_mutex_lock(&_mtx); + 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_poll(_fds, _count, timeout_ms); + ret = px4_sem_timedwait(&_poll_sem, &to); - pthread_mutex_unlock(&_mtx); + if (ret < 0) { + ret = -errno; + } return ret; } -/** - * Acknowledge all orb data for next poll - */ void -MavlinkStreamPoll::ack_all() +MavlinkStreamPoll::hrt_callback(void *arg) { - pthread_mutex_lock(&_mtx); - - for (int i = 0; i < _count; i++) { - orb_ack(_orbs[i].fd); - } - - pthread_mutex_unlock(&_mtx); + px4_sem_post((px4_sem_t *)arg); } -#endif /* CONFIG_MAVLINK_UORB_POLL */ + diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index cefe9bedb4f2..999447ca5908 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -42,10 +42,9 @@ #define MAVLINK_STREAM_H_ #include -#include #include +#include #include -#include class Mavlink; @@ -68,10 +67,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 @@ -146,26 +142,9 @@ class MavlinkStream : public ListNode bool _first_message_sent{false}; }; -#if defined(CONFIG_MAVLINK_UORB_POLL) - /** - * Structure of objects in _reqs list + * Class to manage polling of stream intervals */ -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 { @@ -176,67 +155,115 @@ class MavlinkStreamPoll /** * Add a stream to the poll list */ - int register_orbs(uint16_t stream_id, ORB_ID *orbs, int cnt); + int register_poll(uint16_t stream_id, uint32_t interval_us); /** * Remove a stream from the poll list */ - int unregister_orbs(uint16_t stream_id); + int unregister_poll(uint16_t stream_id); /** - * Set stream update interval + * Re-set interval */ - int set_interval(uint16_t stream_id, int interval_ms); + int set_interval(uint16_t stream_id, int interval_us); /** * Poll all streams for updates */ - int poll(const hrt_abstime timeout); - - /** - * Acknowledge all orb data for next poll - */ - void ack_all(); + int poll(const hrt_abstime timeout_us); private: - /** - * Add a orb_id/interval pair to the orbs list - */ - int _add_orb(ORB_ID orb_id, int interval_ms); + 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(); + } + } + } + }; /** - * Remove a orb_id/interval pair from the orbs list + * 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. */ - int _remove_orb(ORB_ID orb_id, int interval_ms); + void recalculate_roots_and_start(MavStreamPollReq *req); /** - * Poll file descriptors for updates + * HRT interrupt callback posting the semaphore */ - orb_poll_struct_t *_fds; + static void hrt_callback(void *arg); /** - * List of different orbs to poll + * Requests from stream objects */ - MavStreamPollItem *_orbs; + MavlinkStreamPollReqList _reqs; /** - * Requests from stream objects, contains orb poll requests - * count and capacity of the requests list + * Signalling semaphore to release the poll */ - MavStreamOrbPollReq *_reqs; - int _reqs_capacity; - int _reqs_count; + px4_sem_t _poll_sem; /** - * Count and capacity of the orbs/fds lists + * Mutex to protect the list of poll request (hrt) items */ - int _capacity; - int _count; - pthread_mutex_t _mtx {}; }; -#endif /* CONFIG_MAVLINK_UORB_POLL */ - #endif /* MAVLINK_STREAM_H_ */ diff --git a/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp b/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp index 76ced56aa243..5c5cfbdefd7f 100644 --- a/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp +++ b/src/modules/mavlink/streams/ACTUATOR_OUTPUT_STATUS.hpp @@ -53,17 +53,7 @@ class MavlinkStreamActuatorOutputStatus : public MavlinkStream } private: - explicit MavlinkStreamActuatorOutputStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamActuatorOutputStatus() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { ORB_ID::actuator_outputs }; + explicit MavlinkStreamActuatorOutputStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _act_output_sub{ORB_ID(actuator_outputs)}; diff --git a/src/modules/mavlink/streams/ADSB_VEHICLE.hpp b/src/modules/mavlink/streams/ADSB_VEHICLE.hpp index 16353299fa7c..56f2a94e42a6 100644 --- a/src/modules/mavlink/streams/ADSB_VEHICLE.hpp +++ b/src/modules/mavlink/streams/ADSB_VEHICLE.hpp @@ -55,17 +55,7 @@ class MavlinkStreamADSBVehicle : public MavlinkStream } private: - explicit MavlinkStreamADSBVehicle(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamADSBVehicle() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { ORB_ID::transponder_report }; + explicit MavlinkStreamADSBVehicle(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _transponder_report_sub{ORB_ID(transponder_report)}; diff --git a/src/modules/mavlink/streams/ALTITUDE.hpp b/src/modules/mavlink/streams/ALTITUDE.hpp index ea88da73ba26..f3c437d0d9ae 100644 --- a/src/modules/mavlink/streams/ALTITUDE.hpp +++ b/src/modules/mavlink/streams/ALTITUDE.hpp @@ -56,21 +56,7 @@ class MavlinkStreamAltitude : public MavlinkStream } private: - explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamAltitude() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[3] { - ORB_ID::vehicle_air_data, - ORB_ID::home_position, - ORB_ID::vehicle_local_position - }; + explicit MavlinkStreamAltitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; uORB::Subscription _home_sub{ORB_ID(home_position)}; diff --git a/src/modules/mavlink/streams/ATTITUDE.hpp b/src/modules/mavlink/streams/ATTITUDE.hpp index 7b0380eaaa39..94c1f4a42209 100644 --- a/src/modules/mavlink/streams/ATTITUDE.hpp +++ b/src/modules/mavlink/streams/ATTITUDE.hpp @@ -54,20 +54,7 @@ class MavlinkStreamAttitude : public MavlinkStream } private: - explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamAttitude() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::vehicle_attitude, - ORB_ID::vehicle_angular_velocity - }; + explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; diff --git a/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp b/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp index 6d6e54d3158a..64189419ad1e 100644 --- a/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp +++ b/src/modules/mavlink/streams/ATTITUDE_QUATERNION.hpp @@ -55,22 +55,7 @@ class MavlinkStreamAttitudeQuaternion : public MavlinkStream } private: - explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamAttitudeQuaternion() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - - ORB_ID _orbs[3] { - ORB_ID::vehicle_attitude, - ORB_ID::vehicle_angular_velocity, - ORB_ID::vehicle_status - }; + explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; diff --git a/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp b/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp index deaaee7ef1f1..dd93fc8dfe00 100644 --- a/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp +++ b/src/modules/mavlink/streams/ATTITUDE_TARGET.hpp @@ -54,21 +54,7 @@ class MavlinkStreamAttitudeTarget : public MavlinkStream } private: - explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamAttitudeTarget() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - - ORB_ID _orbs[2] { - ORB_ID::vehicle_attitude_setpoint, - ORB_ID::vehicle_rates_setpoint - }; + explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _att_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; diff --git a/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp b/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp index eb015f89b3bf..5e4dea2a03d1 100644 --- a/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp +++ b/src/modules/mavlink/streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp @@ -62,25 +62,7 @@ class MavlinkStreamAutopilotStateForGimbalDevice : public MavlinkStream } private: - explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamAutopilotStateForGimbalDevice() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[6] { - ORB_ID::estimator_selector_status, - ORB_ID::estimator_status, - ORB_ID::vehicle_attitude, - ORB_ID::vehicle_local_position, - ORB_ID::vehicle_attitude_setpoint, - ORB_ID::vehicle_land_detected - }; - + explicit MavlinkStreamAutopilotStateForGimbalDevice(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index c59762fcf3ba..73068f0de360 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -54,19 +54,7 @@ class MavlinkStreamBatteryStatus : public MavlinkStream } private: - explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamBatteryStatus() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::battery_status - }; + explicit MavlinkStreamBatteryStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; diff --git a/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp b/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp index b1bb255d66a2..fede0f3c27df 100644 --- a/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp +++ b/src/modules/mavlink/streams/CAMERA_IMAGE_CAPTURED.hpp @@ -55,19 +55,7 @@ class MavlinkStreamCameraImageCaptured : public MavlinkStream } private: - explicit MavlinkStreamCameraImageCaptured(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamCameraImageCaptured() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::camera_capture - }; + explicit MavlinkStreamCameraImageCaptured(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _capture_sub{ORB_ID(camera_capture)}; diff --git a/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp b/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp index f5f45a1910e5..e64a772d2d22 100644 --- a/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp +++ b/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp @@ -61,20 +61,7 @@ class MavlinkStreamCameraTrigger : public MavlinkStream } private: - explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamCameraTrigger() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::camera_trigger, - ORB_ID::camera_status - }; + explicit MavlinkStreamCameraTrigger(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _camera_trigger_sub{ORB_ID(camera_trigger)}; uORB::Subscription _camera_status_sub{ORB_ID(camera_status)}; diff --git a/src/modules/mavlink/streams/COLLISION.hpp b/src/modules/mavlink/streams/COLLISION.hpp index 320638fe1a9a..a3469c62d9a0 100644 --- a/src/modules/mavlink/streams/COLLISION.hpp +++ b/src/modules/mavlink/streams/COLLISION.hpp @@ -53,19 +53,7 @@ class MavlinkStreamCollision : public MavlinkStream } private: - explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamCollision() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::collision_report - }; + explicit MavlinkStreamCollision(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _collision_sub{ORB_ID(collision_report)}; diff --git a/src/modules/mavlink/streams/COMMAND_LONG.hpp b/src/modules/mavlink/streams/COMMAND_LONG.hpp index 4f33575a1d9c..89f4a77dbe4f 100644 --- a/src/modules/mavlink/streams/COMMAND_LONG.hpp +++ b/src/modules/mavlink/streams/COMMAND_LONG.hpp @@ -53,19 +53,7 @@ class MavlinkStreamCommandLong : public MavlinkStream } private: - explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamCommandLong() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::vehicle_command - }; + explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; diff --git a/src/modules/mavlink/streams/DEBUG.hpp b/src/modules/mavlink/streams/DEBUG.hpp index 5e51da2bfa7e..c0c2a52322d9 100644 --- a/src/modules/mavlink/streams/DEBUG.hpp +++ b/src/modules/mavlink/streams/DEBUG.hpp @@ -53,19 +53,7 @@ class MavlinkStreamDebug : public MavlinkStream } private: - explicit MavlinkStreamDebug(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamDebug() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::debug_value - }; + explicit MavlinkStreamDebug(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _debug_value_sub{ORB_ID(debug_value)}; diff --git a/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp b/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp index 75cd39e4d576..a5029ffe398c 100644 --- a/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp +++ b/src/modules/mavlink/streams/DEBUG_FLOAT_ARRAY.hpp @@ -53,19 +53,7 @@ class MavlinkStreamDebugFloatArray : public MavlinkStream } private: - explicit MavlinkStreamDebugFloatArray(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamDebugFloatArray() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::debug_array - }; + explicit MavlinkStreamDebugFloatArray(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _debug_array_sub{ORB_ID(debug_array)}; diff --git a/src/modules/mavlink/streams/DEBUG_VECT.hpp b/src/modules/mavlink/streams/DEBUG_VECT.hpp index e67f6363abb1..9fa293188d89 100644 --- a/src/modules/mavlink/streams/DEBUG_VECT.hpp +++ b/src/modules/mavlink/streams/DEBUG_VECT.hpp @@ -53,19 +53,7 @@ class MavlinkStreamDebugVect : public MavlinkStream } private: - explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamDebugVect() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::debug_vect - }; + explicit MavlinkStreamDebugVect(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _debug_sub{ORB_ID(debug_vect)}; diff --git a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp index 7ed4335e41f6..9042403a4868 100644 --- a/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp +++ b/src/modules/mavlink/streams/DISTANCE_SENSOR.hpp @@ -54,19 +54,7 @@ class MavlinkStreamDistanceSensor : public MavlinkStream } private: - explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamDistanceSensor() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::distance_sensor - }; + explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; diff --git a/src/modules/mavlink/streams/EFI_STATUS.hpp b/src/modules/mavlink/streams/EFI_STATUS.hpp index 69c0d68465d9..48ea82459cf6 100644 --- a/src/modules/mavlink/streams/EFI_STATUS.hpp +++ b/src/modules/mavlink/streams/EFI_STATUS.hpp @@ -54,19 +54,7 @@ class MavlinkStreamEfiStatus : public MavlinkStream } private: - explicit MavlinkStreamEfiStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamEfiStatus() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::internal_combustion_engine_status - }; + explicit MavlinkStreamEfiStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _internal_combustion_engine_status_sub{ORB_ID(internal_combustion_engine_status)}; diff --git a/src/modules/mavlink/streams/ESC_INFO.hpp b/src/modules/mavlink/streams/ESC_INFO.hpp index e063054992cb..609112ed0ad8 100644 --- a/src/modules/mavlink/streams/ESC_INFO.hpp +++ b/src/modules/mavlink/streams/ESC_INFO.hpp @@ -54,19 +54,7 @@ class MavlinkStreamESCInfo : public MavlinkStream } private: - explicit MavlinkStreamESCInfo(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamESCInfo() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::esc_status - }; + explicit MavlinkStreamESCInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uint8_t _number_of_batches{0}; diff --git a/src/modules/mavlink/streams/ESC_STATUS.hpp b/src/modules/mavlink/streams/ESC_STATUS.hpp index 4bfa6627ccb3..1e8911577823 100644 --- a/src/modules/mavlink/streams/ESC_STATUS.hpp +++ b/src/modules/mavlink/streams/ESC_STATUS.hpp @@ -54,19 +54,7 @@ class MavlinkStreamESCStatus : public MavlinkStream } private: - explicit MavlinkStreamESCStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamESCStatus() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::esc_status - }; + explicit MavlinkStreamESCStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; uint8_t _number_of_batches{0}; diff --git a/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp b/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp index b3e3021f331c..3764940c312e 100644 --- a/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp +++ b/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp @@ -54,20 +54,7 @@ class MavlinkStreamEstimatorStatus : public MavlinkStream } private: - explicit MavlinkStreamEstimatorStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamEstimatorStatus() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::estimator_selector_status, - ORB_ID::estimator_status - }; + explicit MavlinkStreamEstimatorStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; diff --git a/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp b/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp index fc85da1498eb..b6cf44e63436 100644 --- a/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp +++ b/src/modules/mavlink/streams/EXTENDED_SYS_STATE.hpp @@ -60,21 +60,8 @@ class MavlinkStreamExtendedSysState : public MavlinkStream { _msg.vtol_state = MAV_VTOL_STATE_UNDEFINED; _msg.landed_state = MAV_LANDED_STATE_ON_GROUND; - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); } - ~MavlinkStreamExtendedSysState() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[4] { - ORB_ID::vehicle_status, - ORB_ID::vehicle_land_detected, - ORB_ID::position_setpoint_triplet, - ORB_ID::vehicle_control_mode - }; - uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _landed_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; diff --git a/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp b/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp index 253d24b9a242..5dd9f88c88d8 100644 --- a/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp +++ b/src/modules/mavlink/streams/FLIGHT_INFORMATION.hpp @@ -56,18 +56,8 @@ class MavlinkStreamFlightInformation : public MavlinkStream explicit MavlinkStreamFlightInformation(Mavlink *mavlink) : MavlinkStream(mavlink) { _param_com_flight_uuid = param_find("COM_FLIGHT_UUID"); - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); } - ~MavlinkStreamFlightInformation() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::vehicle_status - }; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; param_t _param_com_flight_uuid{PARAM_INVALID}; diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp index 876732ee4f58..615a797488d4 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_ATTITUDE_STATUS.hpp @@ -57,19 +57,7 @@ class MavlinkStreamGimbalDeviceAttitudeStatus : public MavlinkStream } private: - explicit MavlinkStreamGimbalDeviceAttitudeStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamGimbalDeviceAttitudeStatus() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::gimbal_device_attitude_status - }; + explicit MavlinkStreamGimbalDeviceAttitudeStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp index b6b4655c9922..413e7361e6e1 100644 --- a/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp +++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp @@ -57,19 +57,7 @@ class MavlinkStreamGimbalDeviceSetAttitude : public MavlinkStream } private: - explicit MavlinkStreamGimbalDeviceSetAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamGimbalDeviceSetAttitude() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::gimbal_device_set_attitude - }; + explicit MavlinkStreamGimbalDeviceSetAttitude(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)}; diff --git a/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp b/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp index 6f70e5958967..a6570362e138 100644 --- a/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp +++ b/src/modules/mavlink/streams/GIMBAL_MANAGER_INFORMATION.hpp @@ -57,19 +57,7 @@ class MavlinkStreamGimbalManagerInformation : public MavlinkStream } private: - explicit MavlinkStreamGimbalManagerInformation(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamGimbalManagerInformation() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::gimbal_manager_information - }; + explicit MavlinkStreamGimbalManagerInformation(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _gimbal_manager_information_sub{ORB_ID(gimbal_manager_information)}; diff --git a/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp b/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp index 7583e419c51e..ecab79410cbc 100644 --- a/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp +++ b/src/modules/mavlink/streams/GIMBAL_MANAGER_STATUS.hpp @@ -57,19 +57,7 @@ class MavlinkStreamGimbalManagerStatus : public MavlinkStream } private: - explicit MavlinkStreamGimbalManagerStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamGimbalManagerStatus() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::gimbal_manager_status - }; + explicit MavlinkStreamGimbalManagerStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _gimbal_manager_status_sub{ORB_ID(gimbal_manager_status)}; diff --git a/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp b/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp index bec081da7393..6d2997fe7188 100644 --- a/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp +++ b/src/modules/mavlink/streams/GLOBAL_POSITION_INT.hpp @@ -56,22 +56,7 @@ class MavlinkStreamGlobalPositionInt : public MavlinkStream } private: - explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamGlobalPositionInt() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[4] { - ORB_ID::vehicle_global_position, - ORB_ID::vehicle_local_position, - ORB_ID::home_position, - ORB_ID::vehicle_air_data - }; + explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _gpos_sub{ORB_ID(vehicle_global_position)}; uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/mavlink/streams/GPS2_RAW.hpp b/src/modules/mavlink/streams/GPS2_RAW.hpp index a4cd036e9851..eb77c8c3190d 100644 --- a/src/modules/mavlink/streams/GPS2_RAW.hpp +++ b/src/modules/mavlink/streams/GPS2_RAW.hpp @@ -55,19 +55,7 @@ class MavlinkStreamGPS2Raw : public MavlinkStream } private: - explicit MavlinkStreamGPS2Raw(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamGPS2Raw() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::sensor_gps - }; + explicit MavlinkStreamGPS2Raw(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 1}; hrt_abstime _last_send_ts {}; diff --git a/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp b/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp index c3fe5fbcf8e5..fb707b3d14cc 100644 --- a/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp +++ b/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp @@ -63,19 +63,7 @@ class MavlinkStreamGpsGlobalOrigin : public MavlinkStream } private: - explicit MavlinkStreamGpsGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamGpsGlobalOrigin() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::vehicle_local_position - }; + explicit MavlinkStreamGpsGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/mavlink/streams/GPS_RAW_INT.hpp b/src/modules/mavlink/streams/GPS_RAW_INT.hpp index 254de12f1179..f6887c73c992 100644 --- a/src/modules/mavlink/streams/GPS_RAW_INT.hpp +++ b/src/modules/mavlink/streams/GPS_RAW_INT.hpp @@ -55,19 +55,7 @@ class MavlinkStreamGPSRawInt : public MavlinkStream } private: - explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamGPSRawInt() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::sensor_gps - }; + explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 0}; hrt_abstime _last_send_ts {}; diff --git a/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp b/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp index 189f46991ade..8730d2e88109 100644 --- a/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp +++ b/src/modules/mavlink/streams/GPS_RTCM_DATA.hpp @@ -53,19 +53,7 @@ class MavlinkStreamGPSRTCMData : public MavlinkStream } private: - explicit MavlinkStreamGPSRTCMData(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamGPSRTCMData() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::gps_inject_data - }; + explicit MavlinkStreamGPSRTCMData(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data), 0}; diff --git a/src/modules/mavlink/streams/GPS_STATUS.hpp b/src/modules/mavlink/streams/GPS_STATUS.hpp index bbe2c26751ae..82d36f2eae9d 100644 --- a/src/modules/mavlink/streams/GPS_STATUS.hpp +++ b/src/modules/mavlink/streams/GPS_STATUS.hpp @@ -53,19 +53,7 @@ class MavlinkStreamGPSStatus : public MavlinkStream } private: - explicit MavlinkStreamGPSStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamGPSStatus() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::satellite_info - }; + explicit MavlinkStreamGPSStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _satellite_info_sub{ORB_ID(satellite_info)}; diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index 2424648be101..26d966a85ac5 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -57,21 +57,7 @@ class MavlinkStreamHeartbeat : public MavlinkStream } private: - explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamHeartbeat() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[3] { - ORB_ID::actuator_armed, - ORB_ID::vehicle_control_mode, - ORB_ID::vehicle_status - }; + explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _acturator_armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; diff --git a/src/modules/mavlink/streams/HIGHRES_IMU.hpp b/src/modules/mavlink/streams/HIGHRES_IMU.hpp index 678e8596d1a1..a6682213bf93 100644 --- a/src/modules/mavlink/streams/HIGHRES_IMU.hpp +++ b/src/modules/mavlink/streams/HIGHRES_IMU.hpp @@ -61,25 +61,7 @@ class MavlinkStreamHighresIMU : public MavlinkStream } private: - explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamHighresIMU() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[7] { - ORB_ID::vehicle_imu, - ORB_ID::estimator_sensor_bias, - ORB_ID::estimator_selector_status, - ORB_ID::sensor_selection, - ORB_ID::differential_pressure, - ORB_ID::vehicle_magnetometer, - ORB_ID::vehicle_air_data - }; + explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::SubscriptionMultiArray _vehicle_imu_subs{ORB_ID::vehicle_imu}; uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index e8582a680619..c96d0b6cb9d1 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -91,12 +91,6 @@ class MavlinkStreamHighLatency2 : public MavlinkStream _windspeed(SimpleAnalyzer::AVERAGE) { reset_last_sent(); - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamHighLatency2() - { - _mavlink->unregister_orb_poll(get_id_static()); } struct PerBatteryData { @@ -627,26 +621,6 @@ class MavlinkStreamHighLatency2 : public MavlinkStream msg.wp_num = UINT16_MAX; } - ORB_ID _orbs[17] { - ORB_ID::vehicle_thrust_setpoint, - ORB_ID::airspeed, - ORB_ID::vehicle_attitude_setpoint, - ORB_ID::estimator_selector_status, - ORB_ID::estimator_status, - ORB_ID::position_controller_status, - ORB_ID::geofence_result, - ORB_ID::vehicle_global_position, - ORB_ID::vehicle_local_position, - ORB_ID::vehicle_gps_position, - ORB_ID::mission_result, - ORB_ID::vehicle_status, - ORB_ID::failsafe_flags, - ORB_ID::tecs_status, - ORB_ID::wind, - ORB_ID::health_report, - ORB_ID::battery_status - }; - uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0}; uORB::Subscription _vehicle_thrust_setpoint_1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; diff --git a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp index 3a51aa279ef3..182c1d40fab4 100644 --- a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp +++ b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp @@ -62,7 +62,6 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams ModuleParams(nullptr) { _act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) { char param_name[17]; @@ -71,17 +70,6 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams } } - ~MavlinkStreamHILActuatorControls() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[3] { - ORB_ID::actuator_outputs, - ORB_ID::vehicle_status, - ORB_ID::vehicle_control_mode - }; - uORB::Subscription _act_sub{ORB_ID(actuator_outputs)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; diff --git a/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp b/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp index d614b428469f..b2dcb5c42615 100644 --- a/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp +++ b/src/modules/mavlink/streams/HIL_STATE_QUATERNION.hpp @@ -55,22 +55,7 @@ class MavlinkStreamHILStateQuaternion : public MavlinkStream } private: - explicit MavlinkStreamHILStateQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamHILStateQuaternion() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[4] { - ORB_ID::vehicle_angular_velocity_groundtruth, - ORB_ID::vehicle_attitude_groundtruth, - ORB_ID::vehicle_global_position_groundtruth, - ORB_ID::vehicle_local_position_groundtruth - }; + explicit MavlinkStreamHILStateQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _angular_velocity_sub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Subscription _att_sub{ORB_ID(vehicle_attitude_groundtruth)}; diff --git a/src/modules/mavlink/streams/HOME_POSITION.hpp b/src/modules/mavlink/streams/HOME_POSITION.hpp index 69831bf5b924..bb67d14d2dbf 100644 --- a/src/modules/mavlink/streams/HOME_POSITION.hpp +++ b/src/modules/mavlink/streams/HOME_POSITION.hpp @@ -53,19 +53,7 @@ class MavlinkStreamHomePosition : public MavlinkStream } private: - explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamHomePosition() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::home_position - }; + explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _home_sub{ORB_ID(home_position)}; diff --git a/src/modules/mavlink/streams/HYGROMETER_SENSOR.hpp b/src/modules/mavlink/streams/HYGROMETER_SENSOR.hpp index c85b8f75c851..9787f9dee279 100644 --- a/src/modules/mavlink/streams/HYGROMETER_SENSOR.hpp +++ b/src/modules/mavlink/streams/HYGROMETER_SENSOR.hpp @@ -55,19 +55,7 @@ class MavlinkStreamHygrometerSensor : public MavlinkStream } private: - explicit MavlinkStreamHygrometerSensor(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamHygrometerSensor() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::sensor_hygrometer - }; + explicit MavlinkStreamHygrometerSensor(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::SubscriptionMultiArray _sensor_hygrometer_subs{ORB_ID::sensor_hygrometer}; diff --git a/src/modules/mavlink/streams/LANDING_TARGET.hpp b/src/modules/mavlink/streams/LANDING_TARGET.hpp index 6db9dd681b9a..c4c52d0f71d3 100644 --- a/src/modules/mavlink/streams/LANDING_TARGET.hpp +++ b/src/modules/mavlink/streams/LANDING_TARGET.hpp @@ -53,19 +53,7 @@ class MavlinkStreamLandingTarget : public MavlinkStream } private: - explicit MavlinkStreamLandingTarget(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamLandingTarget() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::landing_target_pose - }; + explicit MavlinkStreamLandingTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _landing_target_sub{ORB_ID(landing_target_pose)}; diff --git a/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp b/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp index 25eb0ee1dd77..c06fef2120ec 100644 --- a/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp +++ b/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp @@ -53,19 +53,7 @@ class MavlinkStreamLocalPositionNED : public MavlinkStream } private: - explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamLocalPositionNED() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::vehicle_local_position - }; + explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp b/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp index c9b0553dd844..d52e26e71867 100644 --- a/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp +++ b/src/modules/mavlink/streams/MAG_CAL_REPORT.hpp @@ -56,20 +56,7 @@ class MavlinkStreamMagCalReport : public MavlinkStream } private: - explicit MavlinkStreamMagCalReport(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamMagCalReport() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::sensor_mag, - ORB_ID::parameter_update - }; + explicit MavlinkStreamMagCalReport(Mavlink *mavlink) : MavlinkStream(mavlink) {} static constexpr int MAX_SENSOR_COUNT = 4; diff --git a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp index 0ff8d597497a..8be68fdc3fad 100644 --- a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp +++ b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp @@ -55,20 +55,7 @@ class MavlinkStreamManualControl : public MavlinkStream } private: - explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamManualControl() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::manual_control_setpoint, - ORB_ID::manual_control_switches - }; + explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; diff --git a/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp b/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp index e145c3642698..a07159c90542 100644 --- a/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp +++ b/src/modules/mavlink/streams/MOUNT_ORIENTATION.hpp @@ -54,20 +54,7 @@ class MavlinkStreamMountOrientation : public MavlinkStream } private: - explicit MavlinkStreamMountOrientation(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamMountOrientation() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::mount_orientation, - ORB_ID::vehicle_local_position - }; + explicit MavlinkStreamMountOrientation(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _mount_orientation_sub{ORB_ID(mount_orientation)}; uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; diff --git a/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp b/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp index 9c8096398a57..96317960567f 100644 --- a/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp +++ b/src/modules/mavlink/streams/NAMED_VALUE_FLOAT.hpp @@ -53,19 +53,7 @@ class MavlinkStreamNamedValueFloat : public MavlinkStream } private: - explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamNamedValueFloat() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::debug_key_value - }; + explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _debug_key_value_sub{ORB_ID(debug_key_value)}; diff --git a/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp b/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp index 5f9c352884fa..5f0f50e9e049 100644 --- a/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp +++ b/src/modules/mavlink/streams/NAV_CONTROLLER_OUTPUT.hpp @@ -56,21 +56,7 @@ class MavlinkStreamNavControllerOutput : public MavlinkStream } private: - explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamNavControllerOutput() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[3] { - ORB_ID::position_controller_status, - ORB_ID::tecs_status, - ORB_ID::vehicle_global_position - }; + explicit MavlinkStreamNavControllerOutput(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _position_controller_status_sub{ORB_ID(position_controller_status)}; uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; diff --git a/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp b/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp index a11de4b8aaa5..93d936dea400 100644 --- a/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp +++ b/src/modules/mavlink/streams/OBSTACLE_DISTANCE.hpp @@ -54,19 +54,7 @@ class MavlinkStreamObstacleDistance : public MavlinkStream } private: - explicit MavlinkStreamObstacleDistance(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamObstacleDistance() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::obstacle_distance_fused - }; + explicit MavlinkStreamObstacleDistance(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _obstacle_distance_fused_sub{ORB_ID(obstacle_distance_fused)}; diff --git a/src/modules/mavlink/streams/ODOMETRY.hpp b/src/modules/mavlink/streams/ODOMETRY.hpp index 9d39d197bb6a..60b8564bc2ad 100644 --- a/src/modules/mavlink/streams/ODOMETRY.hpp +++ b/src/modules/mavlink/streams/ODOMETRY.hpp @@ -53,19 +53,7 @@ class MavlinkStreamOdometry : public MavlinkStream } private: - explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamOdometry() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::vehicle_odometry - }; + explicit MavlinkStreamOdometry(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_odometry_sub{ORB_ID(vehicle_odometry)}; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp index 61bd7a3319ae..d7869dcbb6c9 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp @@ -53,19 +53,7 @@ class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream } private: - explicit MavlinkStreamOpenDroneIdBasicId(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamOpenDroneIdBasicId() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::vehicle_status - }; + explicit MavlinkStreamOpenDroneIdBasicId(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp index 6f5f16ac4f82..f3586ae12655 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp @@ -58,24 +58,7 @@ class MavlinkStreamOpenDroneIdLocation : public MavlinkStream } private: - explicit MavlinkStreamOpenDroneIdLocation(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamOpenDroneIdLocation() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[6] { - ORB_ID::home_position, - ORB_ID::vehicle_air_data, - ORB_ID::vehicle_gps_position, - ORB_ID::vehicle_land_detected, - ORB_ID::vehicle_local_position, - ORB_ID::vehicle_status - }; + explicit MavlinkStreamOpenDroneIdLocation(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp index 6d0072375684..bcc7e0f1452e 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp @@ -58,20 +58,7 @@ class MavlinkStreamOpenDroneIdSystem : public MavlinkStream } private: - explicit MavlinkStreamOpenDroneIdSystem(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamOpenDroneIdSystem() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::home_position, - ORB_ID::vehicle_gps_position - }; + explicit MavlinkStreamOpenDroneIdSystem(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; diff --git a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp index 28309fbf8477..02f368b4c319 100644 --- a/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp +++ b/src/modules/mavlink/streams/OPTICAL_FLOW_RAD.hpp @@ -54,19 +54,7 @@ class MavlinkStreamOpticalFlowRad : public MavlinkStream } private: - explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamOpticalFlowRad() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::vehicle_optical_flow - }; + explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_optical_flow_sub{ORB_ID(vehicle_optical_flow)}; diff --git a/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp b/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp index 6ee25d44fe25..5aa3da2475f3 100644 --- a/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp +++ b/src/modules/mavlink/streams/ORBIT_EXECUTION_STATUS.hpp @@ -53,19 +53,7 @@ class MavlinkStreamOrbitStatus : public MavlinkStream } private: - explicit MavlinkStreamOrbitStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamOrbitStatus() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::orbit_status - }; + explicit MavlinkStreamOrbitStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::SubscriptionMultiArray _orbit_status_subs{ORB_ID::orbit_status}; diff --git a/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp b/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp index fcbb50e8c95c..f568e2aadc60 100644 --- a/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp +++ b/src/modules/mavlink/streams/POSITION_TARGET_GLOBAL_INT.hpp @@ -56,21 +56,7 @@ class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream } private: - explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamPositionTargetGlobalInt() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[3] { - ORB_ID::vehicle_control_mode, - ORB_ID::vehicle_local_position_setpoint, - ORB_ID::position_setpoint_triplet - }; + explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _lpos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; diff --git a/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp b/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp index 70358a386a99..69f3fba111ac 100644 --- a/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp +++ b/src/modules/mavlink/streams/POSITION_TARGET_LOCAL_NED.hpp @@ -53,19 +53,7 @@ class MavlinkStreamPositionTargetLocalNed : public MavlinkStream } private: - explicit MavlinkStreamPositionTargetLocalNed(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamPositionTargetLocalNed() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::vehicle_local_position_setpoint - }; + explicit MavlinkStreamPositionTargetLocalNed(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)}; diff --git a/src/modules/mavlink/streams/RAW_RPM.hpp b/src/modules/mavlink/streams/RAW_RPM.hpp index d653122db876..9d2088123ff5 100644 --- a/src/modules/mavlink/streams/RAW_RPM.hpp +++ b/src/modules/mavlink/streams/RAW_RPM.hpp @@ -53,19 +53,7 @@ class MavlinkStreamRawRpm : public MavlinkStream } private: - explicit MavlinkStreamRawRpm(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamRawRpm() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::rpm - }; + explicit MavlinkStreamRawRpm(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::SubscriptionMultiArray _rpm_subs{ORB_ID::rpm}; diff --git a/src/modules/mavlink/streams/RC_CHANNELS.hpp b/src/modules/mavlink/streams/RC_CHANNELS.hpp index a5180da42ef4..868908155cee 100644 --- a/src/modules/mavlink/streams/RC_CHANNELS.hpp +++ b/src/modules/mavlink/streams/RC_CHANNELS.hpp @@ -53,19 +53,7 @@ class MavlinkStreamRCChannels : public MavlinkStream } private: - explicit MavlinkStreamRCChannels(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamRCChannels() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::input_rc - }; + explicit MavlinkStreamRCChannels(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _input_rc_sub{ORB_ID(input_rc)}; diff --git a/src/modules/mavlink/streams/SCALED_IMU.hpp b/src/modules/mavlink/streams/SCALED_IMU.hpp index d46d96415d6f..95d0b48e4b36 100644 --- a/src/modules/mavlink/streams/SCALED_IMU.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU.hpp @@ -62,21 +62,7 @@ class MavlinkStreamScaledIMU : public MavlinkStream } private: - explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamScaledIMU() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[3] { - ORB_ID::vehicle_imu, - ORB_ID::vehicle_imu_status, - ORB_ID::sensor_mag - }; + explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 0}; uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status), 0}; diff --git a/src/modules/mavlink/streams/SCALED_IMU2.hpp b/src/modules/mavlink/streams/SCALED_IMU2.hpp index 283f3e324f8b..3f65189056d2 100644 --- a/src/modules/mavlink/streams/SCALED_IMU2.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU2.hpp @@ -62,21 +62,7 @@ class MavlinkStreamScaledIMU2 : public MavlinkStream } private: - explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamScaledIMU2() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[3] { - ORB_ID::vehicle_imu, - ORB_ID::vehicle_imu_status, - ORB_ID::sensor_mag - }; + explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 1}; uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status), 1}; diff --git a/src/modules/mavlink/streams/SCALED_IMU3.hpp b/src/modules/mavlink/streams/SCALED_IMU3.hpp index 60d9df91aa42..cb180c7b8b2b 100644 --- a/src/modules/mavlink/streams/SCALED_IMU3.hpp +++ b/src/modules/mavlink/streams/SCALED_IMU3.hpp @@ -62,21 +62,7 @@ class MavlinkStreamScaledIMU3 : public MavlinkStream } private: - explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamScaledIMU3() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[3] { - ORB_ID::vehicle_imu, - ORB_ID::vehicle_imu_status, - ORB_ID::sensor_mag - }; + explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 2}; uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status), 2}; diff --git a/src/modules/mavlink/streams/SCALED_PRESSURE.hpp b/src/modules/mavlink/streams/SCALED_PRESSURE.hpp index 81386a60bfea..7ae99083168d 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE.hpp @@ -58,20 +58,7 @@ class MavlinkStreamScaledPressure : public MavlinkStream } private: - explicit MavlinkStreamScaledPressure(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamScaledPressure() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::differential_pressure, - ORB_ID::sensor_baro - }; + explicit MavlinkStreamScaledPressure(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure), 0}; uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), 0}; diff --git a/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp b/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp index 941fbe69a39f..f453c4ff184d 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE2.hpp @@ -58,20 +58,7 @@ class MavlinkStreamScaledPressure2 : public MavlinkStream } private: - explicit MavlinkStreamScaledPressure2(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamScaledPressure2() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::differential_pressure, - ORB_ID::sensor_baro - }; + explicit MavlinkStreamScaledPressure2(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure), 1}; uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), 1}; diff --git a/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp b/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp index 76400699f29b..7b3011bb66e8 100644 --- a/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp +++ b/src/modules/mavlink/streams/SCALED_PRESSURE3.hpp @@ -58,20 +58,7 @@ class MavlinkStreamScaledPressure3 : public MavlinkStream } private: - explicit MavlinkStreamScaledPressure3(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamScaledPressure3() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::differential_pressure, - ORB_ID::sensor_baro - }; + explicit MavlinkStreamScaledPressure3(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure), 2}; uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), 2}; diff --git a/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp b/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp index 81759232089e..77640368ff25 100644 --- a/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp +++ b/src/modules/mavlink/streams/SERVO_OUTPUT_RAW.hpp @@ -66,19 +66,7 @@ class MavlinkStreamServoOutputRaw : public MavlinkStream } private: - explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamServoOutputRaw() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::actuator_outputs - }; + explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _act_sub{ORB_ID(actuator_outputs), N}; diff --git a/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp b/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp index cc77aee598f9..3476db3a02b2 100644 --- a/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp +++ b/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp @@ -54,19 +54,7 @@ class MavlinkStreamSmartBatteryInfo : public MavlinkStream } private: - explicit MavlinkStreamSmartBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamSmartBatteryInfo() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::battery_status - }; + explicit MavlinkStreamSmartBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; diff --git a/src/modules/mavlink/streams/STATUSTEXT.hpp b/src/modules/mavlink/streams/STATUSTEXT.hpp index f1aa5c8d0885..3409a27ab3fd 100644 --- a/src/modules/mavlink/streams/STATUSTEXT.hpp +++ b/src/modules/mavlink/streams/STATUSTEXT.hpp @@ -55,21 +55,13 @@ class MavlinkStreamStatustext : public MavlinkStream } private: - explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } + explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) {} ~MavlinkStreamStatustext() { - _mavlink->unregister_orb_poll(get_id_static()); perf_free(_missed_msg_count_perf); } - ORB_ID _orbs[1] { - ORB_ID::mavlink_log - }; - uORB::Subscription _mavlink_log_sub{ORB_ID(mavlink_log)}; perf_counter_t _missed_msg_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": STATUSTEXT missed messages")}; uint16_t _id{0}; diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index 0e75ead5d896..6859ea7f4be1 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -57,22 +57,7 @@ class MavlinkStreamSysStatus : public MavlinkStream } private: - explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamSysStatus() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[4] { - ORB_ID::vehicle_status, - ORB_ID::cpuload, - ORB_ID::health_report, - ORB_ID::battery_status - }; + explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _cpuload_sub{ORB_ID(cpuload)}; diff --git a/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp b/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp index 2fce063ab1b7..b28a8bd4dd0f 100644 --- a/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp +++ b/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp @@ -53,19 +53,7 @@ class MavlinkStreamTimeEstimateToTarget : public MavlinkStream } private: - explicit MavlinkStreamTimeEstimateToTarget(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamTimeEstimateToTarget() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::rtl_time_estimate - }; + explicit MavlinkStreamTimeEstimateToTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _rtl_estimate_sub{ORB_ID(rtl_time_estimate)}; diff --git a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp b/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp index f1b678a47d14..174c633fd2fc 100644 --- a/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp +++ b/src/modules/mavlink/streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp @@ -57,19 +57,7 @@ class MavlinkStreamTrajectoryRepresentationWaypoints: public MavlinkStream } private: - explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamTrajectoryRepresentationWaypoints() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[1] { - ORB_ID::vehicle_trajectory_waypoint_desired - }; + explicit MavlinkStreamTrajectoryRepresentationWaypoints(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _traj_wp_avoidance_sub{ORB_ID(vehicle_trajectory_waypoint_desired)}; diff --git a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp index f506ddfaca40..fdc8ae1ecc74 100644 --- a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp +++ b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp @@ -58,21 +58,8 @@ class MavlinkStreamUavionixADSBOutDynamic : public ModuleParams, public MavlinkS } private: - explicit MavlinkStreamUavionixADSBOutDynamic(Mavlink *mavlink) : ModuleParams(nullptr), MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamUavionixADSBOutDynamic() - { - _mavlink->unregister_orb_poll(get_id_static()); - } + explicit MavlinkStreamUavionixADSBOutDynamic(Mavlink *mavlink) : ModuleParams(nullptr), MavlinkStream(mavlink) {} - ORB_ID _orbs[3] { - ORB_ID::vehicle_gps_position, - ORB_ID::vehicle_air_data, - ORB_ID::vehicle_status - }; uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; diff --git a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp index 25f0c4122816..2cf17b69f2eb 100644 --- a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp +++ b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp @@ -59,23 +59,7 @@ class MavlinkStreamUTMGlobalPosition : public MavlinkStream } private: - explicit MavlinkStreamUTMGlobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamUTMGlobalPosition() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[5] { - ORB_ID::vehicle_local_position, - ORB_ID::vehicle_global_position, - ORB_ID::position_setpoint_triplet, - ORB_ID::vehicle_status, - ORB_ID::vehicle_land_detected - }; + explicit MavlinkStreamUTMGlobalPosition(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; diff --git a/src/modules/mavlink/streams/VFR_HUD.hpp b/src/modules/mavlink/streams/VFR_HUD.hpp index 68a50d1f2ad1..bbdfe0d3c862 100644 --- a/src/modules/mavlink/streams/VFR_HUD.hpp +++ b/src/modules/mavlink/streams/VFR_HUD.hpp @@ -61,23 +61,7 @@ class MavlinkStreamVFRHUD : public MavlinkStream } private: - explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamVFRHUD() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[5] { - ORB_ID::vehicle_local_position, - ORB_ID::actuator_armed, - ORB_ID::vehicle_thrust_setpoint, - ORB_ID::airspeed_validated, - ORB_ID::vehicle_air_data - }; + explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _lpos_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; diff --git a/src/modules/mavlink/streams/VIBRATION.hpp b/src/modules/mavlink/streams/VIBRATION.hpp index 75da44f93f4e..661c9528ac46 100644 --- a/src/modules/mavlink/streams/VIBRATION.hpp +++ b/src/modules/mavlink/streams/VIBRATION.hpp @@ -58,20 +58,7 @@ class MavlinkStreamVibration : public MavlinkStream } private: - explicit MavlinkStreamVibration(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamVibration() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::sensor_selection, - ORB_ID::vehicle_imu_status - }; + explicit MavlinkStreamVibration(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; diff --git a/src/modules/mavlink/streams/WIND_COV.hpp b/src/modules/mavlink/streams/WIND_COV.hpp index 3d24b373e25f..8cd5550db523 100644 --- a/src/modules/mavlink/streams/WIND_COV.hpp +++ b/src/modules/mavlink/streams/WIND_COV.hpp @@ -54,20 +54,7 @@ class MavlinkStreamWindCov : public MavlinkStream } private: - explicit MavlinkStreamWindCov(Mavlink *mavlink) : MavlinkStream(mavlink) - { - mavlink->register_orb_poll(get_id_static(), _orbs, arraySize(_orbs)); - } - - ~MavlinkStreamWindCov() - { - _mavlink->unregister_orb_poll(get_id_static()); - } - - ORB_ID _orbs[2] { - ORB_ID::wind, - ORB_ID::vehicle_local_position - }; + explicit MavlinkStreamWindCov(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};