From 3354cb37d0a87f9dcba6b7734bc1a925f721de71 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Dec 2014 18:57:17 +1100 Subject: [PATCH] AP_GPS: fixed vertical velocity in Replay fixed NavEKF use of velocity velocity --- libraries/AP_GPS/AP_GPS.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 00ccd2b6ac..3a8eb513e1 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -411,6 +411,7 @@ AP_GPS::setHIL(uint8_t instance, GPS_Status _status, uint64_t time_epoch_ms, istate.location = _location; istate.location.options = 0; istate.velocity = _velocity; + istate.have_vertical_velocity = true; istate.ground_speed = pythagorous2(istate.velocity.x, istate.velocity.y); istate.ground_course_cd = degrees(atan2f(istate.velocity.y, istate.velocity.x)) * 100UL; istate.hdop = hdop;