dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

UBlox: Only use fix and velocity if flags are valid

This commit is contained in:
Johan Jansen 2015-04-07 17:15:17 +02:00
parent 1219ef8d43
commit a121f6101f
1 changed files with 12 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/drivers/gps/ubx.cpp
View File

@ -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;