dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
This commit is contained in:
Julian Oes 2019-09-25 14:37:08 +02:00 committed by GitHub
parent b6db872303
commit 0b368d043f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 76 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

143
src/modules/mavlink/mavlink_main.cpp
View File

@ -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

2
src/modules/mavlink/mavlink_main.h
View File

@ -720,6 +720,8 @@ private:
void publish_telemetry_status();
void check_requested_subscriptions();
/**
* Check the configuration of a connected radio
*