diff options
author | unknown <jcyr@jcyr-pc.sharon.lan> | 2013-07-09 20:14:34 -0400 |
---|---|---|
committer | unknown <jcyr@jcyr-pc.sharon.lan> | 2013-07-09 20:14:34 -0400 |
commit | 328f4f8c872a0e33857fd9a1112963438a87165b (patch) | |
tree | 8305020bbda268454c71763e6da669a948846bfe | |
parent | c2642678141d03a8c6d643e20b2d8aef4be5712d (diff) | |
parent | 71ca3ec449fe530ff5aa24f58d858b2186054c42 (diff) | |
download | px4-firmware-328f4f8c872a0e33857fd9a1112963438a87165b.tar.gz px4-firmware-328f4f8c872a0e33857fd9a1112963438a87165b.tar.bz2 px4-firmware-328f4f8c872a0e33857fd9a1112963438a87165b.zip |
Merge remote-tracking branch 'upstream/master'
-rw-r--r-- | src/drivers/gps/ubx.cpp | 41 |
1 files changed, 14 insertions, 27 deletions
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index ad219cd25..762c257aa 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -549,7 +549,7 @@ UBX::handle_message() } case NAV_SVINFO: { -// printf("GOT NAV_SVINFO MESSAGE\n"); + // printf("GOT NAV_SVINFO MESSAGE\n"); if (!_waiting_for_ack) { //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message @@ -570,40 +570,27 @@ UBX::handle_message() uint8_t satellites_used = 0; int i; - + // printf("Number of Channels: %d\n", packet_part1->numCh); for (i = 0; i < packet_part1->numCh; i++) { //for each channel /* Get satellite information from the buffer */ memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; + /* Write satellite information to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; - /* Write satellite information in the global storage */ - _gps_position->satellite_prn[i] = packet_part2->svid; - - //if satellite information is healthy store the data - uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield - - if (!unhealthy) { - if ((packet_part2->flags) & 1) { //flags is a bitfield - _gps_position->satellite_used[i] = 1; - satellites_used++; - - } else { - _gps_position->satellite_used[i] = 0; - } - - _gps_position->satellite_snr[i] = packet_part2->cno; - _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - - } else { - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; + if ( sv_used ) { + // Count SVs used for NAV. + satellites_used++; } - + + // Record info for all channels, whether or not the SV is used for NAV. + _gps_position->satellite_used[i] = sv_used; + _gps_position->satellite_snr[i] = packet_part2->cno; + _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + _gps_position->satellite_prn[i] = packet_part2->svid; } for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused |