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

Merge remote-tracking branch 'upstream/master'

This commit is contained in:
unknown 2013-07-09 20:14:34 -04:00
parent c264267814 71ca3ec449
commit 328f4f8c87
1 changed files with 14 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;
if ( sv_used ) {
// Count SVs used for NAV.
satellites_used++;
} else {
_gps_position->satellite_used[i] = 0;
}
// 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);
} 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;
}
_gps_position->satellite_prn[i] = packet_part2->svid;
}
for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused