From 1e99b04d9cce0206cec47f2cad3640faf3963a74 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Dec 2015 12:32:22 +0100 Subject: [PATCH] Fixes in simulator interface --- src/modules/simulator/simulator_mavlink.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 57b7d167f8..31292a8b6b 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -500,7 +500,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) PX4_INFO("Waiting for initial data on UDP. Please start the flight simulator to proceed.."); while (pret <= 0) { - pret = ::poll(&fds[0], (sizeof(fds[0]) / sizeof(fds[0])), 100); + pret = ::poll(&fds[0], fd_count, 100); } PX4_INFO("Found initial message, pret = %d", pret); @@ -537,6 +537,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) //timed out if (pret == 0) { + PX4_WARN("mavlink sim timeout for 100 ms"); continue; } @@ -556,7 +557,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) mavlink_message_t msg; mavlink_status_t status; - for (int i = 0; i < len; ++i) { + for (int i = 0; i < len; i++) { if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) { // have a message, handle it handle_message(&msg, publish); @@ -566,7 +567,7 @@ void Simulator::pollForMAVLinkMessages(bool publish) } // got data from PIXHAWK - if (fds[1].revents & POLLIN) { + if (fd_count > 1 && fds[1].revents & POLLIN) { len = ::read(serial_fd, serial_buf, sizeof(serial_buf)); if (len > 0) {