From a121f6101fb763ec61e5f447731ad0a155e2ac72 Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Tue, 7 Apr 2015 17:15:17 +0200 Subject: [PATCH] UBlox: Only use fix and velocity if flags are valid --- src/drivers/gps/ubx.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f42c968d37..96b26b6991 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -716,7 +716,18 @@ UBX::payload_rx_done(void) case UBX_MSG_NAV_PVT: UBX_TRACE_RXMSG("Rx NAV-PVT\n"); - _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; + //Check if position fix flag is good + if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) + { + _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; + _gps_position->vel_ned_valid = true; + } + else + { + _gps_position->fix_type = 0; + _gps_position->vel_ned_valid = false; + } + _gps_position->satellites_used = _buf.payload_rx_nav_pvt.numSV; _gps_position->lat = _buf.payload_rx_nav_pvt.lat; @@ -732,7 +743,6 @@ UBX::payload_rx_done(void) _gps_position->vel_n_m_s = (float)_buf.payload_rx_nav_pvt.velN * 1e-3f; _gps_position->vel_e_m_s = (float)_buf.payload_rx_nav_pvt.velE * 1e-3f; _gps_position->vel_d_m_s = (float)_buf.payload_rx_nav_pvt.velD * 1e-3f; - _gps_position->vel_ned_valid = true; _gps_position->cog_rad = (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f; _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f;