mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 02:55:07 +08:00
do not run mavlink receiver before app is fully booted when using sockets
This commit is contained in:
@@ -1543,11 +1543,6 @@ Mavlink::task_main(int argc, char *argv[])
|
|||||||
/* flush stdout in case MAVLink is about to take it over */
|
/* flush stdout in case MAVLink is about to take it over */
|
||||||
fflush(stdout);
|
fflush(stdout);
|
||||||
|
|
||||||
/* init socket if necessary */
|
|
||||||
if (get_protocol() == UDP) {
|
|
||||||
init_udp();
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifndef __PX4_POSIX
|
#ifndef __PX4_POSIX
|
||||||
struct termios uart_config_original;
|
struct termios uart_config_original;
|
||||||
|
|
||||||
@@ -1724,6 +1719,11 @@ Mavlink::task_main(int argc, char *argv[])
|
|||||||
/* now the instance is fully initialized and we can bump the instance count */
|
/* now the instance is fully initialized and we can bump the instance count */
|
||||||
LL_APPEND(_mavlink_instances, this);
|
LL_APPEND(_mavlink_instances, this);
|
||||||
|
|
||||||
|
/* init socket if necessary */
|
||||||
|
if (get_protocol() == UDP) {
|
||||||
|
init_udp();
|
||||||
|
}
|
||||||
|
|
||||||
/* if the protocol is serial, we send the system version blindly */
|
/* if the protocol is serial, we send the system version blindly */
|
||||||
if (get_protocol() == SERIAL) {
|
if (get_protocol() == SERIAL) {
|
||||||
send_autopilot_capabilites();
|
send_autopilot_capabilites();
|
||||||
|
|||||||
@@ -1761,10 +1761,17 @@ MavlinkReceiver::receive_thread(void *arg)
|
|||||||
#ifdef __PX4_POSIX
|
#ifdef __PX4_POSIX
|
||||||
struct sockaddr_in srcaddr;
|
struct sockaddr_in srcaddr;
|
||||||
socklen_t addrlen = sizeof(srcaddr);
|
socklen_t addrlen = sizeof(srcaddr);
|
||||||
|
|
||||||
if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) {
|
if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) {
|
||||||
|
// make sure mavlink app has booted before we start using the socket
|
||||||
|
while (!_mavlink->boot_complete()) {
|
||||||
|
usleep(100000);
|
||||||
|
}
|
||||||
|
|
||||||
fds[0].fd = _mavlink->get_socket_fd();
|
fds[0].fd = _mavlink->get_socket_fd();
|
||||||
fds[0].events = POLLIN;
|
fds[0].events = POLLIN;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
ssize_t nread = 0;
|
ssize_t nread = 0;
|
||||||
|
|
||||||
@@ -1779,7 +1786,6 @@ MavlinkReceiver::receive_thread(void *arg)
|
|||||||
}
|
}
|
||||||
#ifdef __PX4_POSIX
|
#ifdef __PX4_POSIX
|
||||||
if (_mavlink->get_protocol() == UDP) {
|
if (_mavlink->get_protocol() == UDP) {
|
||||||
|
|
||||||
if (fds[0].revents & POLLIN) {
|
if (fds[0].revents & POLLIN) {
|
||||||
nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen);
|
nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen);
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user