Skip to content

Commit

Permalink
Revert "mavlink_main: use orb polling"
Browse files Browse the repository at this point in the history 
This reverts commit 090bb19.
  • Loading branch information
@jlaitine
jlaitine committed Jun 7, 2024
1 parent 71ada80 commit 2251ae4
Show file tree
Hide file tree
Showing 5 changed files with 3 additions and 416 deletions.
8 changes: 0 additions & 8 deletions src/modules/mavlink/Kconfig
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
73 changes: 2 additions & 71 deletions src/modules/mavlink/mavlink_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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 */
Expand All @@ -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;
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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 */
Expand Down
12 changes: 0 additions & 12 deletions src/modules/mavlink/mavlink_main.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down
229 changes: 0 additions & 229 deletions src/modules/mavlink/mavlink_stream.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Loading

0 comments on commit 2251ae4

Please sign in to comment.