diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 14 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 13 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 2 |
3 files changed, 15 insertions, 14 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 38f753c39..42504dea9 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -52,7 +52,12 @@ #include <poll.h> #include <termios.h> #include <time.h> +#include <math.h> /* isinf / isnan checks */ + #include <sys/ioctl.h> +#include <sys/types.h> +#include <sys/stat.h> + #include <drivers/device/device.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> @@ -64,6 +69,7 @@ #include <uORB/topics/parameter_update.h> #include <uORB/topics/mission.h> #include <uORB/topics/mission_result.h> + #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/perf_counter.h> @@ -72,12 +78,10 @@ #include <dataman/dataman.h> #include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> -#include <sys/types.h> -#include <sys/stat.h> + #include <commander/px4_custom_mode.h> #include "mavlink_bridge_header.h" -#include "math.h" /* isinf / isnan checks */ #include "mavlink_main.h" #include "mavlink_orb_listener.h" #include "mavlink_receiver.h" @@ -156,9 +160,9 @@ namespace mavlink Mavlink::Mavlink() : device_name("/dev/ttyS1"), - _mavlink_fd(-1), - _next(nullptr), _task_should_exit(false), + _next(nullptr), + _mavlink_fd(-1), thread_running(false), _mavlink_task(-1), _mavlink_incoming_fd(-1), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 371f945c4..d07de0f22 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,7 +106,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : telemetry_status_pub(-1), lat0(0), lon0(0), - alt0(0) + alt0(0.0) { } @@ -605,8 +605,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } - uint64_t timestamp = hrt_absolute_time(); - // publish global position if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); @@ -628,7 +626,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) if (pub_hil_local_pos > 0) { float x; float y; - bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve? + bool landed = (float)(hil_state.alt)/1000.0f < (alt0 + 0.1f); // XXX improve? double lat = hil_state.lat*1e-7; double lon = hil_state.lon*1e-7; map_projection_project(lat, lon, &x, &y); @@ -811,14 +809,13 @@ MavlinkReceiver::receive_thread(void *arg) while (!_mavlink->_task_should_exit) { if (poll(fds, 1, timeout) > 0) { - if (nread < sizeof(buf)) { + + /* non-blocking read. read may return negative values */ + if ((nread = read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { /* to avoid reading very small chunks wait for data before reading */ usleep(1000); } - /* non-blocking read. read may return negative values */ - nread = read(uart_fd, buf, sizeof(buf)); - /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { if (mavlink_parse_char(_mavlink->get_chan(), buf[i], &msg, &status)) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8e139e5e4..fca5de917 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -140,6 +140,6 @@ private: orb_advert_t telemetry_status_pub; int32_t lat0; int32_t lon0; - double alt0; + float alt0; }; |