From 362fb470c4aee49ca0115f5aa910339a0af13fb5 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 4 Apr 2018 12:36:57 -0700 Subject: [PATCH] AP_GPS: Allow HIL_GPS to flag vertical velocity Closes #7997 --- libraries/AP_GPS/AP_GPS_MAV.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp index c7078b4f7c..92fb607948 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.cpp +++ b/libraries/AP_GPS/AP_GPS_MAV.cpp @@ -132,12 +132,15 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t *msg) } Vector3f vel(packet.vn/100.0f, packet.ve/100.0f, packet.vd/100.0f); state.velocity = vel; + if (packet.vd != 0) { + state.have_vertical_velocity = true; + } if (packet.cog < 36000) { state.ground_course = packet.cog / 100.0f; } state.have_speed_accuracy = false; - state.have_horizontal_accuracy = 0; - state.have_vertical_accuracy = 0; + state.have_horizontal_accuracy = false; + state.have_vertical_accuracy = false; if (packet.satellites_visible < 255) { state.num_sats = packet.satellites_visible; }