rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1739 Fill all UAVO slots with SV data

Browse Source
This commit is contained in:
Erik Gustavsson 2015-03-01 16:44:30 +01:00
parent a806eb6f36
commit 60736ea82d
1 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
flight/modules/GPS/UBX.c
View File

@ -413,6 +413,8 @@ static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, __attribute__((unused))
struct UBX_NAV_SVINFO *svinfo = &ubx->payload.nav_svinfo; struct UBX_NAV_SVINFO *svinfo = &ubx->payload.nav_svinfo;
svdata.SatsInView = 0; svdata.SatsInView = 0;
// First, use slots for SVs actually being received
for (chan = 0; chan < svinfo->numCh; chan++) { for (chan = 0; chan < svinfo->numCh; chan++) {
if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM && svinfo->sv[chan].cno > 0) { if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM && svinfo->sv[chan].cno > 0) {
svdata.Azimuth[svdata.SatsInView] = svinfo->sv[chan].azim; svdata.Azimuth[svdata.SatsInView] = svinfo->sv[chan].azim;
@ -422,6 +424,18 @@ static void parse_ubx_nav_svinfo(struct UBXPacket *ubx, __attribute__((unused))
svdata.SatsInView++; svdata.SatsInView++;
} }
} }
// Now try to add the rest
for (chan = 0; chan < svinfo->numCh; chan++) {
if (svdata.SatsInView < GPSSATELLITES_PRN_NUMELEM && 0 == svinfo->sv[chan].cno) {
svdata.Azimuth[svdata.SatsInView] = svinfo->sv[chan].azim;
svdata.Elevation[svdata.SatsInView] = svinfo->sv[chan].elev;
svdata.PRN[svdata.SatsInView] = svinfo->sv[chan].svid;
svdata.SNR[svdata.SatsInView] = svinfo->sv[chan].cno;
svdata.SatsInView++;
}
}
// fill remaining slots (if any) // fill remaining slots (if any)
for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) { for (chan = svdata.SatsInView; chan < GPSSATELLITES_PRN_NUMELEM; chan++) {
svdata.Azimuth[chan] = 0; svdata.Azimuth[chan] = 0;