From 0b368d043f0248399616b80555ac25cd371669b2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 25 Sep 2019 14:37:08 +0200 Subject: [PATCH] mavlink: always check stream subscriptions (#13018) This fixes the regression where we saw this in SITL: ERROR [mavlink] system boot did not complete in 20 seconds The reason was that the stream subscription requests were not checked because the while loop was not running yet because mavlink was not booted completely. By moving the stream subscription requests into a function we can run them even if we don't run the rest of the loop. --- src/modules/mavlink/mavlink_main.cpp | 143 ++++++++++++++------------- src/modules/mavlink/mavlink_main.h | 2 + 2 files changed, 76 insertions(+), 69 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 8f4e240357..1235966c6d 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2217,6 +2217,7 @@ Mavlink::task_main(int argc, char *argv[]) px4_usleep(_main_loop_delay); if (!should_transmit()) { + check_requested_subscriptions(); continue; } @@ -2370,75 +2371,7 @@ Mavlink::task_main(int argc, char *argv[]) } } - /* check for requested subscriptions */ - if (_subscribe_to_stream != nullptr) { - if (_subscribe_to_stream_rate < -1.5f) { - if (configure_streams_to_default(_subscribe_to_stream) == 0) { - if (get_protocol() == Protocol::SERIAL) { - PX4_DEBUG("stream %s on device %s set to default rate", _subscribe_to_stream, _device_name); - } - -#if defined(MAVLINK_UDP) - - else if (get_protocol() == Protocol::UDP) { - PX4_DEBUG("stream %s on UDP port %d set to default rate", _subscribe_to_stream, _network_port); - } - -#endif // MAVLINK_UDP - - } else { - PX4_ERR("setting stream %s to default failed", _subscribe_to_stream); - } - - } else if (configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate) == 0) { - if (fabsf(_subscribe_to_stream_rate) > 0.00001f) { - if (get_protocol() == Protocol::SERIAL) { - PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, - (double)_subscribe_to_stream_rate); - - } - -#if defined(MAVLINK_UDP) - - else if (get_protocol() == Protocol::UDP) { - PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port, - (double)_subscribe_to_stream_rate); - } - -#endif // MAVLINK_UDP - - } else { - if (get_protocol() == Protocol::SERIAL) { - PX4_DEBUG("stream %s on device %s disabled", _subscribe_to_stream, _device_name); - - } - -#if defined(MAVLINK_UDP) - - else if (get_protocol() == Protocol::UDP) { - PX4_DEBUG("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port); - } - -#endif // MAVLINK_UDP - } - - } else { - if (get_protocol() == Protocol::SERIAL) { - PX4_ERR("stream %s on device %s not found", _subscribe_to_stream, _device_name); - - } - -#if defined(MAVLINK_UDP) - - else if (get_protocol() == Protocol::UDP) { - PX4_ERR("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port); - } - -#endif // MAVLINK_UDP - } - - _subscribe_to_stream = nullptr; - } + check_requested_subscriptions(); /* update streams */ for (const auto &stream : _streams) { @@ -2571,6 +2504,78 @@ Mavlink::task_main(int argc, char *argv[]) return OK; } +void Mavlink::check_requested_subscriptions() +{ + if (_subscribe_to_stream != nullptr) { + if (_subscribe_to_stream_rate < -1.5f) { + if (configure_streams_to_default(_subscribe_to_stream) == 0) { + if (get_protocol() == Protocol::SERIAL) { + PX4_DEBUG("stream %s on device %s set to default rate", _subscribe_to_stream, _device_name); + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { + PX4_DEBUG("stream %s on UDP port %d set to default rate", _subscribe_to_stream, _network_port); + } + +#endif // MAVLINK_UDP + + } else { + PX4_ERR("setting stream %s to default failed", _subscribe_to_stream); + } + + } else if (configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate) == 0) { + if (fabsf(_subscribe_to_stream_rate) > 0.00001f) { + if (get_protocol() == Protocol::SERIAL) { + PX4_DEBUG("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, + (double)_subscribe_to_stream_rate); + + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { + PX4_DEBUG("stream %s on UDP port %d enabled with rate %.1f Hz", _subscribe_to_stream, _network_port, + (double)_subscribe_to_stream_rate); + } + +#endif // MAVLINK_UDP + + } else { + if (get_protocol() == Protocol::SERIAL) { + PX4_DEBUG("stream %s on device %s disabled", _subscribe_to_stream, _device_name); + + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { + PX4_DEBUG("stream %s on UDP port %d disabled", _subscribe_to_stream, _network_port); + } + +#endif // MAVLINK_UDP + } + + } else { + if (get_protocol() == Protocol::SERIAL) { + PX4_ERR("stream %s on device %s not found", _subscribe_to_stream, _device_name); + + } + +#if defined(MAVLINK_UDP) + + else if (get_protocol() == Protocol::UDP) { + PX4_ERR("stream %s on UDP port %d not found", _subscribe_to_stream, _network_port); + } + +#endif // MAVLINK_UDP + } + + _subscribe_to_stream = nullptr; + } +} + void Mavlink::publish_telemetry_status() { // many fields are populated in place diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 04ced7f353..12d1c07573 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -720,6 +720,8 @@ private: void publish_telemetry_status(); + void check_requested_subscriptions(); + /** * Check the configuration of a connected radio *