From 1b33445c7be545717bd49a015c9309a7c6ee7704 Mon Sep 17 00:00:00 2001 From: Daniel Agar <daniel@agar.ca> Date: Sat, 9 Jun 2018 16:47:13 -0400 Subject: [PATCH] simulator mavlink set lpos ground truth limits to infinity --- src/modules/simulator/simulator_mavlink.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index e90796a913..0214d00d35 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -47,6 +47,8 @@ #include <mathlib/mathlib.h> #include <uORB/topics/vehicle_local_position.h> +#include <limits> + extern "C" __EXPORT hrt_abstime hrt_reset(void); #define SEND_INTERVAL 20 @@ -524,10 +526,10 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) hil_lpos.ref_lon = _hil_ref_lon; hil_lpos.ref_alt = _hil_ref_alt; hil_lpos.ref_timestamp = _hil_ref_timestamp; - hil_lpos.vxy_max = 0.0f; - hil_lpos.vz_max = 0.0f; - hil_lpos.hagl_min = 0.0f; - hil_lpos.hagl_max = 0.0f; + hil_lpos.vxy_max = std::numeric_limits<float>::infinity(); + hil_lpos.vz_max = std::numeric_limits<float>::infinity(); + hil_lpos.hagl_min = std::numeric_limits<float>::infinity(); + hil_lpos.hagl_max = std::numeric_limits<float>::infinity(); // always publish ground truth attitude message int hil_lpos_multi; @@ -535,8 +537,6 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) ORB_PRIO_HIGH); } - - break; } -- GitLab