diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-09 23:16:53 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-07-09 23:16:53 +0200 |
commit | 71ca3ec449fe530ff5aa24f58d858b2186054c42 (patch) | |
tree | 417c2fab0b6af82a03c8e197fdd7ba81e8ffaae8 /src/drivers/gps/ubx.cpp | |
parent | c3d07030dd1882c626ed027cfc5870f42b1cd33e (diff) | |
parent | 2990a7384bb0f28462cf0389940ef7388b9cfc6f (diff) | |
download | px4-firmware-71ca3ec449fe530ff5aa24f58d858b2186054c42.tar.gz px4-firmware-71ca3ec449fe530ff5aa24f58d858b2186054c42.tar.bz2 px4-firmware-71ca3ec449fe530ff5aa24f58d858b2186054c42.zip |
Merged GPS info fixes from pigeonhunter
Diffstat (limited to 'src/drivers/gps/ubx.cpp')
-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 |