do not run mavlink receiver before app is fully booted when using sockets

This commit is contained in:
tumbili
2015-09-02 10:24:31 +02:00
parent b1850a316b
commit 95af5fc3d0
2 changed files with 12 additions and 6 deletions

View File

@@ -1543,11 +1543,6 @@ Mavlink::task_main(int argc, char *argv[])
/* flush stdout in case MAVLink is about to take it over */
fflush(stdout);
/* init socket if necessary */
if (get_protocol() == UDP) {
init_udp();
}
#ifndef __PX4_POSIX
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 */
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 (get_protocol() == SERIAL) {
send_autopilot_capabilites();

View File

@@ -1761,10 +1761,17 @@ MavlinkReceiver::receive_thread(void *arg)
#ifdef __PX4_POSIX
struct sockaddr_in srcaddr;
socklen_t addrlen = sizeof(srcaddr);
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].events = POLLIN;
}
#endif
ssize_t nread = 0;
@@ -1779,7 +1786,6 @@ MavlinkReceiver::receive_thread(void *arg)
}
#ifdef __PX4_POSIX
if (_mavlink->get_protocol() == UDP) {
if (fds[0].revents & POLLIN) {
nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen);
}