/**
* Fill nmeaINFO structure from GSA packet structure
*
* @param pack a pointer to the packet structure
* @param info a pointer to the nmeaINFO structure
*/
void nmea_GPGSA2info(const nmeaGPGSA *pack, nmeaINFO *info) {
int i = 0;
assert(pack);
assert(info);
info->present |= pack->present;
nmea_INFO_set_present(&info->present, SMASK);
info->smask |= GPGSA;
if (nmea_INFO_is_present(pack->present, FIX)) {
/* fix_mode is ignored */
info->fix = pack->fix_type;
}
if (nmea_INFO_is_present(pack->present, SATINUSE)) {
info->satinfo.inuse = 0;
for (i = 0; i < NMEA_MAXSAT; i++) {
info->satinfo.in_use[i] = pack->sat_prn[i];
if (pack->sat_prn[i]) {
info->satinfo.inuse++;
}
}
nmea_INFO_set_present(&info->present, SATINUSECOUNT);
}
if (nmea_INFO_is_present(pack->present, PDOP)) {
info->PDOP = pack->PDOP;
}
if (nmea_INFO_is_present(pack->present, HDOP)) {
info->HDOP = pack->HDOP;
}
if (nmea_INFO_is_present(pack->present, VDOP)) {
info->VDOP = pack->VDOP;
}
}
/**
* Convert an nmeaINFO structure into an nmeaGPGSA structure
*
* @param info a pointer to the nmeaINFO structure
* @param pack a pointer to the nmeaGPGSA structure
*/
void nmea_info2GPGSA(const nmeaINFO *info, nmeaGPGSA *pack) {
assert(pack);
assert(info);
nmea_zero_GPGSA(pack);
pack->present = info->present;
nmea_INFO_unset_present(&pack->present, SMASK);
if (nmea_INFO_is_present(info->present, FIX)) {
pack->fix_mode = 'A';
pack->fix_type = info->fix;
}
if (nmea_INFO_is_present(info->present, SATINUSE)) {
memcpy(pack->sat_prn, info->satinfo.in_use, sizeof(pack->sat_prn));
}
if (nmea_INFO_is_present(info->present, PDOP)) {
pack->PDOP = info->PDOP;
}
if (nmea_INFO_is_present(info->present, HDOP)) {
pack->HDOP = info->HDOP;
}
if (nmea_INFO_is_present(info->present, VDOP)) {
pack->VDOP = info->VDOP;
}
}
/**
* Generate a GPGSA sentence from an nmeaGPGSA structure
*
* @param s a pointer to the buffer to generate the string in
* @param len the size of the buffer
* @param pack the structure
* @return the length of the generated sentence
*/
int nmea_gen_GPGSA(char *s, const int len, const nmeaGPGSA *pack) {
int i;
char sFixMode[2];
char sFixType[2];
char sSatPrn[64];
char sPdop[16];
char sHdop[16];
char sVdop[16];
char * psSatPrn = &sSatPrn[0];
int ssSatPrn = sizeof(sSatPrn);
bool satinuse = nmea_INFO_is_present(pack->present, SATINUSE);
sFixMode[0] = sFixMode[1] = 0;
sFixType[0] = sFixType[1] = 0;
sSatPrn[0] = 0;
sPdop[0] = 0;
sHdop[0] = 0;
sVdop[0] = 0;
if (nmea_INFO_is_present(pack->present, FIX)) {
sFixMode[0] = pack->fix_mode;
snprintf(&sFixType[0], sizeof(sFixType), "%1d", pack->fix_type);
}
for (i = 0; i < NMEA_MAXSAT; i++) {
if (satinuse && pack->sat_prn[i]) {
int cnt = snprintf(psSatPrn, ssSatPrn, "%d", pack->sat_prn[i]);
if (cnt >= ssSatPrn) {
ssSatPrn = 0;
psSatPrn = &sSatPrn[sizeof(sSatPrn) - 1];
*psSatPrn = '\0';
break;
} else {
ssSatPrn -= cnt;
psSatPrn += cnt;
}
}
if (i < (NMEA_MAXSAT - 1)) {
*psSatPrn = ',';
psSatPrn++;
ssSatPrn--;
*psSatPrn = '\0';
}
}
if (nmea_INFO_is_present(pack->present, PDOP)) {
snprintf(&sPdop[0], sizeof(sPdop), "%03.1f", pack->PDOP);
}
if (nmea_INFO_is_present(pack->present, HDOP)) {
snprintf(&sHdop[0], sizeof(sHdop), "%03.1f", pack->HDOP);
}
if (nmea_INFO_is_present(pack->present, VDOP)) {
snprintf(&sVdop[0], sizeof(sVdop), "%03.1f", pack->VDOP);
}
return nmea_printf(s, len, "$GPGSA,%s,%s,%s,%s,%s,%s", &sFixMode[0], &sFixType[0], &sSatPrn[0], &sPdop[0],
&sHdop[0], &sVdop[0]);
}
/**
Add a new (incoming) position update to the position average list
@param positionAverageList
The position average list
@param newEntry
The new (incoming) position update (must be the same as the one returned from
the function getPositionAverageEntryForIncoming:INCOMING)
*/
void addNewPositionToAverage(PositionAverageList * positionAverageList,
PositionUpdateEntry * newEntry) {
assert (positionAverageList != NULL);
assert (newEntry == getPositionAverageEntry(positionAverageList, INCOMING));
if (positionAverageList->entriesCount
>= positionAverageList->entriesMaxCount) {
/* list is full, so first remove the oldest from the average */
addOrRemoveEntryToFromCumulativeAverage(positionAverageList,
getPositionAverageEntry(positionAverageList, OLDEST), false);
}
/* calculate the angle components */
calculateAngleComponents(&newEntry->track, nmea_INFO_is_present(newEntry->nmeaInfo.present, TRACK) ? &newEntry->nmeaInfo.track : NULL);
calculateAngleComponents(&newEntry->mtrack, nmea_INFO_is_present(newEntry->nmeaInfo.present, MTRACK) ? &newEntry->nmeaInfo.mtrack : NULL);
calculateAngleComponents(&newEntry->magvar, nmea_INFO_is_present(newEntry->nmeaInfo.present, MAGVAR) ? &newEntry->nmeaInfo.magvar : NULL);
/* now just add the new position */
addOrRemoveEntryToFromCumulativeAverage(positionAverageList, newEntry, true);
/* update the place where the new entry is stored */
positionAverageList->newestEntryIndex
= WRAPINDEX(positionAverageList, NEWESTINDEX(positionAverageList) + 1);
/* update average position */
updatePositionAverageFromCumulative(positionAverageList);
}
/**
* Fill nmeaINFO structure from VTG packet structure
*
* @param pack a pointer to the packet structure
* @param info a pointer to the nmeaINFO structure
*/
void nmea_GPVTG2info(const nmeaGPVTG *pack, nmeaINFO *info) {
assert(pack);
assert(info);
info->present |= pack->present;
nmea_INFO_set_present(&info->present, SMASK);
info->smask |= GPVTG;
if (nmea_INFO_is_present(pack->present, SPEED)) {
info->speed = pack->spk;
}
if (nmea_INFO_is_present(pack->present, TRACK)) {
info->track = pack->track;
}
if (nmea_INFO_is_present(pack->present, MTRACK)) {
info->mtrack = pack->mtrack;
}
}
/**
* Fill nmeaINFO structure from GGA packet structure
*
* @param pack a pointer to the packet structure
* @param info a pointer to the nmeaINFO structure
*/
void nmea_GPGGA2info(const nmeaGPGGA *pack, nmeaINFO *info) {
assert(pack);
assert(info);
info->present |= pack->present;
nmea_INFO_set_present(&info->present, SMASK);
info->smask |= GPGGA;
if (nmea_INFO_is_present(pack->present, UTCTIME)) {
info->utc.hour = pack->utc.hour;
info->utc.min = pack->utc.min;
info->utc.sec = pack->utc.sec;
info->utc.hsec = pack->utc.hsec;
}
if (nmea_INFO_is_present(pack->present, LAT)) {
info->lat = ((pack->ns == 'N') ? pack->lat : -pack->lat);
}
if (nmea_INFO_is_present(pack->present, LON)) {
info->lon = ((pack->ew == 'E') ? pack->lon : -pack->lon);
}
if (nmea_INFO_is_present(pack->present, SIG)) {
info->sig = pack->sig;
}
if (nmea_INFO_is_present(pack->present, SATINUSECOUNT)) {
info->satinfo.inuse = pack->satinuse;
}
if (nmea_INFO_is_present(pack->present, HDOP)) {
info->HDOP = pack->HDOP;
}
if (nmea_INFO_is_present(pack->present, ELV)) {
info->elv = pack->elv;
}
/* ignore diff and diff_units */
/* ignore dgps_age and dgps_sid */
}
/**
* Convert an nmeaINFO structure into an nmeaGPRMC structure
*
* @param info a pointer to the nmeaINFO structure
* @param pack a pointer to the nmeaGPRMC structure
*/
void nmea_info2GPRMC(const nmeaINFO *info, nmeaGPRMC *pack) {
assert(pack);
assert(info);
nmea_zero_GPRMC(pack);
pack->present = info->present;
nmea_INFO_unset_present(&pack->present, SMASK);
if (nmea_INFO_is_present(info->present, UTCDATE)) {
pack->utc.year = info->utc.year;
pack->utc.mon = info->utc.mon;
pack->utc.day = info->utc.day;
}
if (nmea_INFO_is_present(info->present, UTCTIME)) {
pack->utc.hour = info->utc.hour;
pack->utc.min = info->utc.min;
pack->utc.sec = info->utc.sec;
pack->utc.hsec = info->utc.hsec;
}
if (nmea_INFO_is_present(info->present, SIG)) {
pack->status = ((info->sig != NMEA_SIG_BAD) ? 'A' : 'V');
} else {
pack->status = 'V';
}
if (nmea_INFO_is_present(info->present, LAT)) {
pack->lat = fabs(info->lat);
pack->ns = ((info->lat > 0) ? 'N' : 'S');
}
if (nmea_INFO_is_present(info->present, LON)) {
pack->lon = fabs(info->lon);
pack->ew = ((info->lon > 0) ? 'E' : 'W');
}
if (nmea_INFO_is_present(info->present, SPEED)) {
pack->speed = info->speed / NMEA_TUD_KNOTS;
}
if (nmea_INFO_is_present(info->present, TRACK)) {
pack->track = info->track;
}
if (nmea_INFO_is_present(info->present, MAGVAR)) {
pack->magvar = fabs(info->magvar);
pack->magvar_ew = ((info->magvar > 0) ? 'E' : 'W');
}
if (nmea_INFO_is_present(info->present, SIG)) {
pack->mode = ((info->sig != NMEA_SIG_BAD) ? 'A' : 'N');
} else {
pack->mode = 'N';
}
}
/**
* Convert an nmeaINFO structure into an nmeaGPVTG structure
*
* @param info a pointer to the nmeaINFO structure
* @param pack a pointer to the nmeaGPRMC structure
*/
void nmea_info2GPVTG(const nmeaINFO *info, nmeaGPVTG *pack) {
assert(pack);
assert(info);
nmea_zero_GPVTG(pack); /* also sets up units */
pack->present = info->present;
nmea_INFO_unset_present(&pack->present, SMASK);
if (nmea_INFO_is_present(info->present, TRACK)) {
pack->track = info->track;
}
if (nmea_INFO_is_present(info->present, MTRACK)) {
pack->mtrack = info->mtrack;
}
if (nmea_INFO_is_present(info->present, SPEED)) {
pack->spn = info->speed / NMEA_TUD_KNOTS;
pack->spk = info->speed;
}
}
/**
* Converts the position fields to degrees and DOP fields to meters so that
* all fields use normal metric units.
*
* @param nmeaInfo
* the nmeaINFO
*/
void nmea_INFO_unit_conversion(nmeaINFO * nmeaInfo) {
if (!nmeaInfo) {
return;
}
/* smask (already in correct format) */
/* utc (already in correct format) */
/* sig (already in correct format) */
/* fix (already in correct format) */
if (nmea_INFO_is_present(nmeaInfo->present, PDOP)) {
nmeaInfo->PDOP = nmea_dop2meters(nmeaInfo->PDOP);
}
if (nmea_INFO_is_present(nmeaInfo->present, HDOP)) {
nmeaInfo->HDOP = nmea_dop2meters(nmeaInfo->HDOP);
}
if (nmea_INFO_is_present(nmeaInfo->present, VDOP)) {
nmeaInfo->VDOP = nmea_dop2meters(nmeaInfo->VDOP);
}
if (nmea_INFO_is_present(nmeaInfo->present, LAT)) {
nmeaInfo->lat = nmea_ndeg2degree(nmeaInfo->lat);
}
if (nmea_INFO_is_present(nmeaInfo->present, LON)) {
nmeaInfo->lon = nmea_ndeg2degree(nmeaInfo->lon);
}
/* elv (already in correct format) */
/* speed (already in correct format) */
/* track (already in correct format) */
/* mtrack (already in correct format) */
/* magvar (already in correct format) */
/* satinfo (already in correct format) */
}
/**
* Generate a GPRMC sentence from an nmeaGPRMC structure
*
* @param s a pointer to the buffer to generate the string in
* @param len the size of the buffer
* @param pack the structure
* @return the length of the generated sentence
*/
int nmea_gen_GPRMC(char *s, const int len, const nmeaGPRMC *pack) {
char sTime[16];
char sDate[16];
char sLat[16];
char sNs[2];
char sLon[16];
char sEw[2];
char sSpeed[16];
char sTrack[16];
char sMagvar[16];
char sMagvar_ew[2];
sTime[0] = 0;
sDate[0] = 0;
sLat[0] = 0;
sNs[0] = sNs[1] = 0;
sLon[0] = 0;
sEw[0] = sEw[1] = 0;
sSpeed[0] = 0;
sTrack[0] = 0;
sMagvar[0] = 0;
sMagvar_ew[0] = sMagvar_ew[1] = 0;
if (nmea_INFO_is_present(pack->present, UTCDATE)) {
snprintf(&sDate[0], sizeof(sDate), "%02d%02d%02d", pack->utc.day, pack->utc.mon + 1, pack->utc.year - 100);
}
if (nmea_INFO_is_present(pack->present, UTCTIME)) {
snprintf(&sTime[0], sizeof(sTime), "%02d%02d%02d.%02d", pack->utc.hour, pack->utc.min, pack->utc.sec,
pack->utc.hsec);
}
if (nmea_INFO_is_present(pack->present, LAT)) {
snprintf(&sLat[0], sizeof(sLat), "%09.4f", pack->lat);
sNs[0] = pack->ns;
}
if (nmea_INFO_is_present(pack->present, LON)) {
snprintf(&sLon[0], sizeof(sLon), "%010.4f", pack->lon);
sEw[0] = pack->ew;
}
if (nmea_INFO_is_present(pack->present, SPEED)) {
snprintf(&sSpeed[0], sizeof(sSpeed), "%03.1f", pack->speed);
}
if (nmea_INFO_is_present(pack->present, TRACK)) {
snprintf(&sTrack[0], sizeof(sTrack), "%03.1f", pack->track);
}
if (nmea_INFO_is_present(pack->present, MAGVAR)) {
snprintf(&sMagvar[0], sizeof(sMagvar), "%03.1f", pack->magvar);
sMagvar_ew[0] = pack->magvar_ew;
}
return nmea_printf(s, len, "$GPRMC,%s,%C,%s,%s,%s,%s,%s,%s,%s,%s,%s,%C", &sTime[0], pack->status, &sLat[0], &sNs[0],
&sLon[0], &sEw[0], &sSpeed[0], &sTrack[0], &sDate[0], &sMagvar[0], &sMagvar_ew[0], pack->mode);
}
/**
* Generate a GPGGA sentence from an nmeaGPGGA structure
*
* @param s a pointer to the buffer to generate the string in
* @param len the size of the buffer
* @param pack the structure
* @return the length of the generated sentence
*/
int nmea_gen_GPGGA(char *s, const int len, const nmeaGPGGA *pack) {
char sTime[16];
char sLat[16];
char sNs[2];
char sLon[16];
char sEw[2];
char sSig[4];
char sSatInUse[4];
char sHdop[16];
char sElv[16];
char sElvUnit[2];
sTime[0] = 0;
sLat[0] = 0;
sNs[0] = sNs[1] = 0;
sLon[0] = 0;
sEw[0] = sEw[1] = 0;
sSig[0] = 0;
sSatInUse[0] = 0;
sHdop[0] = 0;
sElv[0] = 0;
sElvUnit[0] = sElvUnit[1] = 0;
if (nmea_INFO_is_present(pack->present, UTCTIME)) {
snprintf(&sTime[0], sizeof(sTime), "%02d%02d%02d.%02d", pack->utc.hour, pack->utc.min, pack->utc.sec,
pack->utc.hsec);
}
if (nmea_INFO_is_present(pack->present, LAT)) {
snprintf(&sLat[0], sizeof(sLat), "%09.4f", pack->lat);
sNs[0] = pack->ns;
}
if (nmea_INFO_is_present(pack->present, LON)) {
snprintf(&sLon[0], sizeof(sLon), "%010.4f", pack->lon);
sEw[0] = pack->ew;
}
if (nmea_INFO_is_present(pack->present, SIG)) {
snprintf(&sSig[0], sizeof(sSig), "%1d", pack->sig);
}
if (nmea_INFO_is_present(pack->present, SATINUSECOUNT)) {
snprintf(&sSatInUse[0], sizeof(sSatInUse), "%02d", pack->satinuse);
}
if (nmea_INFO_is_present(pack->present, HDOP)) {
snprintf(&sHdop[0], sizeof(sHdop), "%03.1f", pack->HDOP);
}
if (nmea_INFO_is_present(pack->present, ELV)) {
snprintf(&sElv[0], sizeof(sElv), "%03.1f", pack->elv);
sElvUnit[0] = pack->elv_units;
}
return nmea_printf(s, len, "$GPGGA,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,,,,", &sTime[0], &sLat[0], &sNs[0],
&sLon[0], &sEw[0], &sSig[0], &sSatInUse[0], &sHdop[0], &sElv[0], &sElvUnit[0]);
}
/**
* Generate a GPVTG sentence from an nmeaGPVTG structure
*
* @param s a pointer to the buffer to generate the string in
* @param len the size of the buffer
* @param pack the structure
* @return the length of the generated sentence
*/
int nmea_gen_GPVTG(char *s, const int len, const nmeaGPVTG *pack) {
char sTrackT[16];
char sTrackM[16];
char sSpeedN[16];
char sSpeedK[16];
char sUnitT[2];
char sUnitM[2];
char sUnitN[2];
char sUnitK[2];
sTrackT[0] = 0;
sTrackM[0] = 0;
sSpeedN[0] = 0;
sSpeedK[0] = 0;
sUnitT[0] = sUnitT[1] = 0;
sUnitM[0] = sUnitM[1] = 0;
sUnitN[0] = sUnitN[1] = 0;
sUnitK[0] = sUnitK[1] = 0;
if (nmea_INFO_is_present(pack->present, TRACK)) {
snprintf(&sTrackT[0], sizeof(sTrackT), "%03.1f", pack->track);
sUnitT[0] = 'T';
}
if (nmea_INFO_is_present(pack->present, MTRACK)) {
snprintf(&sTrackM[0], sizeof(sTrackM), "%03.1f", pack->mtrack);
sUnitM[0] = 'M';
}
if (nmea_INFO_is_present(pack->present, SPEED)) {
snprintf(&sSpeedN[0], sizeof(sSpeedN), "%03.1f", pack->spn);
sUnitN[0] = 'N';
snprintf(&sSpeedK[0], sizeof(sSpeedK), "%03.1f", pack->spk);
sUnitK[0] = 'K';
}
return nmea_printf(s, len, "$GPVTG,%s,%s,%s,%s,%s,%s,%s,%s", &sTrackT[0], &sUnitT[0], &sTrackM[0],
&sUnitM[0], &sSpeedN[0], &sUnitN[0], &sSpeedK[0], &sUnitK[0]);
}
/**
* Generate a GPGSV sentence from an nmeaGPGSV structure
*
* @param s a pointer to the buffer to generate the string in
* @param len the size of the buffer
* @param pack the structure
* @return the length of the generated sentence
*/
int nmea_gen_GPGSV(char *s, const int len, const nmeaGPGSV *pack) {
char sCount[2];
char sIndex[2];
char sSatCount[4];
char sSatInfo[64];
char * psSatInfo = &sSatInfo[0];
int ssSatInfo = sizeof(sSatInfo);
bool satinview = nmea_INFO_is_present(pack->present, SATINVIEW);
int i;
sCount[0] = 0;
sIndex[0] = 0;
sSatCount[0] = 0;
sSatInfo[0] = 0;
if (satinview) {
snprintf(&sCount[0], sizeof(sCount), "%1d", pack->pack_count);
snprintf(&sIndex[0], sizeof(sIndex), "%1d", pack->pack_index);
snprintf(&sSatCount[0], sizeof(sSatCount), "%02d", pack->sat_count);
}
for (i = 0; i < NMEA_SATINPACK; i++) {
int cnt = 0;
if (satinview && pack->sat_data[i].id) {
cnt = snprintf(psSatInfo, ssSatInfo, "%02d,%02d,%03d,%02d", pack->sat_data[i].id, pack->sat_data[i].elv,
pack->sat_data[i].azimuth, pack->sat_data[i].sig);
} else {
cnt = snprintf(psSatInfo, ssSatInfo, ",,,");
}
if (cnt >= ssSatInfo) {
ssSatInfo = 0;
psSatInfo = &sSatInfo[sizeof(sSatInfo) - 1];
*psSatInfo = '\0';
break;
} else {
ssSatInfo -= cnt;
psSatInfo += cnt;
}
if (i < (NMEA_SATINPACK - 1)) {
*psSatInfo = ',';
psSatInfo++;
ssSatInfo--;
*psSatInfo = '\0';
}
}
return nmea_printf(s, len, "$GPGSV,%s,%s,%s,%s", &sCount[0], &sIndex[0], &sSatCount[0], &sSatInfo[0]);
}
/**
* Fill nmeaINFO structure from RMC packet structure
*
* @param pack a pointer to the packet structure
* @param info a pointer to the nmeaINFO structure
*/
void nmea_GPRMC2info(const nmeaGPRMC *pack, nmeaINFO *info) {
assert(pack);
assert(info);
info->present |= pack->present;
nmea_INFO_set_present(&info->present, SMASK);
info->smask |= GPRMC;
if (nmea_INFO_is_present(pack->present, UTCDATE)) {
info->utc.year = pack->utc.year;
info->utc.mon = pack->utc.mon;
info->utc.day = pack->utc.day;
}
if (nmea_INFO_is_present(pack->present, UTCTIME)) {
info->utc.hour = pack->utc.hour;
info->utc.min = pack->utc.min;
info->utc.sec = pack->utc.sec;
info->utc.hsec = pack->utc.hsec;
}
nmea_INFO_set_present(&info->present, SIG);
nmea_INFO_set_present(&info->present, FIX);
if (pack->status == 'A') {
if (info->sig == NMEA_SIG_BAD) {
info->sig = NMEA_SIG_MID;
}
if (info->fix == NMEA_FIX_BAD) {
info->fix = NMEA_FIX_2D;
}
} else {
info->sig = NMEA_SIG_BAD;
info->fix = NMEA_FIX_BAD;
}
if (nmea_INFO_is_present(pack->present, LAT)) {
info->lat = ((pack->ns == 'N') ? pack->lat : -pack->lat);
}
if (nmea_INFO_is_present(pack->present, LON)) {
info->lon = ((pack->ew == 'E') ? pack->lon : -pack->lon);
}
if (nmea_INFO_is_present(pack->present, SPEED)) {
info->speed = pack->speed * NMEA_TUD_KNOTS;
}
if (nmea_INFO_is_present(pack->present, TRACK)) {
info->track = pack->track;
}
if (nmea_INFO_is_present(pack->present, MAGVAR)) {
info->magvar = ((pack->magvar_ew == 'E') ? pack->magvar : -pack->magvar);
}
/* mode is ignored */
}
/**
* Convert an nmeaINFO structure into an nmeaGPGSV structure
*
* @param info a pointer to the nmeaINFO structure
* @param pack a pointer to the nmeaGPGSV structure
* @param pack_idx pack index (zero based)
*/
void nmea_info2GPGSV(const nmeaINFO *info, nmeaGPGSV *pack, int pack_idx) {
assert(pack);
assert(info);
nmea_zero_GPGSV(pack);
pack->present = info->present;
nmea_INFO_unset_present(&pack->present, SMASK);
if (nmea_INFO_is_present(info->present, SATINVIEW)) {
int sit;
int pit;
int toskip;
pack->sat_count = (info->satinfo.inview < NMEA_MAXSAT) ? info->satinfo.inview : NMEA_MAXSAT;
pack->pack_count = nmea_gsv_npack(pack->sat_count);
if (pack_idx >= pack->pack_count)
pack->pack_index = pack->pack_count;
else
pack->pack_index = pack_idx + 1;
/* now skip the first ((pack->pack_index - 1) * NMEA_SATINPACK) in view sats */
toskip = ((pack->pack_index - 1) * NMEA_SATINPACK);
sit = 0;
while ((toskip > 0) && (sit < NMEA_MAXSAT)) {
if (info->satinfo.sat[sit].id) {
toskip--;
}
sit++;
}
for (pit = 0; pit < NMEA_SATINPACK; sit++) {
if (sit < NMEA_MAXSAT) {
if (info->satinfo.sat[sit].id) {
pack->sat_data[pit] = info->satinfo.sat[sit];
pit++;
}
} else {
memset(&pack->sat_data[pit], 0, sizeof(pack->sat_data[pit]));
pit++;
}
}
}
}
/**
* Convert an nmeaINFO structure into an nmeaGPGGA structure
*
* @param info a pointer to the nmeaINFO structure
* @param pack a pointer to the nmeaGPGGA structure
*/
void nmea_info2GPGGA(const nmeaINFO *info, nmeaGPGGA *pack) {
assert(pack);
assert(info);
nmea_zero_GPGGA(pack);
pack->present = info->present;
nmea_INFO_unset_present(&pack->present, SMASK);
if (nmea_INFO_is_present(info->present, UTCTIME)) {
pack->utc.hour = info->utc.hour;
pack->utc.min = info->utc.min;
pack->utc.sec = info->utc.sec;
pack->utc.hsec = info->utc.hsec;
}
if (nmea_INFO_is_present(info->present, LAT)) {
pack->lat = fabs(info->lat);
pack->ns = ((info->lat > 0) ? 'N' : 'S');
}
if (nmea_INFO_is_present(info->present, LON)) {
pack->lon = fabs(info->lon);
pack->ew = ((info->lon > 0) ? 'E' : 'W');
}
if (nmea_INFO_is_present(info->present, SIG)) {
pack->sig = info->sig;
}
if (nmea_INFO_is_present(info->present, SATINUSECOUNT)) {
pack->satinuse = info->satinfo.inuse;
}
if (nmea_INFO_is_present(info->present, HDOP)) {
pack->HDOP = info->HDOP;
}
if (nmea_INFO_is_present(info->present, ELV)) {
pack->elv = info->elv;
pack->elv_units = 'M';
}
/* defaults for (ignored) diff and diff_units */
pack->diff = 0;
pack->diff_units = 'M';
/* defaults for (ignored) dgps_age and dgps_sid */
pack->dgps_age = 0;
pack->dgps_sid = 0;
}
/**
* Fill nmeaINFO structure from GSV packet structure
*
* @param pack a pointer to the packet structure
* @param info a pointer to the nmeaINFO structure
*/
void nmea_GPGSV2info(const nmeaGPGSV *pack, nmeaINFO *info) {
int pack_index;
assert(pack);
assert(info);
pack_index = pack->pack_index;
if (pack_index < 1)
pack_index = 1;
if (pack_index > pack->pack_count)
pack_index = pack->pack_count;
if ((pack_index * NMEA_SATINPACK) > NMEA_MAXSAT)
pack_index = NMEA_NSATPACKS;
info->present |= pack->present;
nmea_INFO_set_present(&info->present, SMASK);
info->smask |= GPGSV;
if (nmea_INFO_is_present(pack->present, SATINVIEW)) {
int sat_index;
/* index of 1st sat in pack */
int sat_offset = (pack_index - 1) * NMEA_SATINPACK;
/* the number of sats in this sentence */
int sat_count = ((sat_offset + NMEA_SATINPACK) > pack->sat_count) ? (pack->sat_count - sat_offset) : NMEA_SATINPACK;
for (sat_index = 0; sat_index < sat_count; sat_index++) {
info->satinfo.sat[sat_offset + sat_index].id = pack->sat_data[sat_index].id;
info->satinfo.sat[sat_offset + sat_index].elv = pack->sat_data[sat_index].elv;
info->satinfo.sat[sat_offset + sat_index].azimuth = pack->sat_data[sat_index].azimuth;
info->satinfo.sat[sat_offset + sat_index].sig = pack->sat_data[sat_index].sig;
}
info->satinfo.inview = pack->sat_count;
}
}
/**
* Sanitise the NMEA info, make sure that:
* - sig is in the range [0, 8],
* - fix is in the range [1, 3],
* - DOPs are positive,
* - latitude is in the range [-9000, 9000],
* - longitude is in the range [-18000, 18000],
* - speed is positive,
* - track is in the range [0, 360>.
* - mtrack is in the range [0, 360>.
* - magvar is in the range [0, 360>.
* - satinfo:
* - inuse and in_use are consistent (w.r.t. count)
* - inview and sat are consistent (w.r.t. count/id)
* - in_use and sat are consistent (w.r.t. count/id)
* - elv is in the range [0, 90]
* - azimuth is in the range [0, 359]
* - sig is in the range [0, 99]
*
* Time is set to the current time when not present.
* Fields are reset to their defaults (0) when not signaled as being present.
*
* @param nmeaInfo
* the NMEA info structure to sanitise
*/
void nmea_INFO_sanitise(nmeaINFO *nmeaInfo) {
double lat = 0;
double lon = 0;
double speed = 0;
double track = 0;
double mtrack = 0;
double magvar = 0;
bool latAdjusted = false;
bool lonAdjusted = false;
bool speedAdjusted = false;
bool trackAdjusted = false;
bool mtrackAdjusted = false;
bool magvarAdjusted = false;
nmeaTIME utc;
int inuseIndex;
int inviewIndex;
if (!nmeaInfo) {
return;
}
nmeaInfo->present = nmeaInfo->present & NMEA_INFO_PRESENT_MASK;
if (!nmea_INFO_is_present(nmeaInfo->present, SMASK)) {
nmeaInfo->smask = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, UTCDATE) || !nmea_INFO_is_present(nmeaInfo->present, UTCTIME)) {
nmea_time_now(&utc, NULL);
}
if (!nmea_INFO_is_present(nmeaInfo->present, UTCDATE)) {
nmeaInfo->utc.year = utc.year;
nmeaInfo->utc.mon = utc.mon;
nmeaInfo->utc.day = utc.day;
}
if (!nmea_INFO_is_present(nmeaInfo->present, UTCTIME)) {
nmeaInfo->utc.hour = utc.hour;
nmeaInfo->utc.min = utc.min;
nmeaInfo->utc.sec = utc.sec;
nmeaInfo->utc.hsec = utc.hsec;
}
if (!nmea_INFO_is_present(nmeaInfo->present, SIG)) {
nmeaInfo->sig = NMEA_SIG_BAD;
} else {
if ((nmeaInfo->sig < NMEA_SIG_BAD) || (nmeaInfo->sig > NMEA_SIG_SIM)) {
nmeaInfo->sig = NMEA_SIG_BAD;
}
}
if (!nmea_INFO_is_present(nmeaInfo->present, FIX)) {
nmeaInfo->fix = NMEA_FIX_BAD;
} else {
if ((nmeaInfo->fix < NMEA_FIX_BAD) || (nmeaInfo->fix > NMEA_FIX_3D)) {
nmeaInfo->fix = NMEA_FIX_BAD;
}
}
if (!nmea_INFO_is_present(nmeaInfo->present, PDOP)) {
nmeaInfo->PDOP = 0;
} else {
nmeaInfo->PDOP = fabs(nmeaInfo->PDOP);
}
if (!nmea_INFO_is_present(nmeaInfo->present, HDOP)) {
nmeaInfo->HDOP = 0;
} else {
nmeaInfo->HDOP = fabs(nmeaInfo->HDOP);
}
if (!nmea_INFO_is_present(nmeaInfo->present, VDOP)) {
nmeaInfo->VDOP = 0;
} else {
nmeaInfo->VDOP = fabs(nmeaInfo->VDOP);
}
if (!nmea_INFO_is_present(nmeaInfo->present, LAT)) {
nmeaInfo->lat = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, LON)) {
nmeaInfo->lon = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, ELV)) {
nmeaInfo->elv = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, SPEED)) {
nmeaInfo->speed = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, TRACK)) {
nmeaInfo->track = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, MTRACK)) {
nmeaInfo->mtrack = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, MAGVAR)) {
nmeaInfo->magvar = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, SATINUSECOUNT)) {
nmeaInfo->satinfo.inuse = 0;
}
if (!nmea_INFO_is_present(nmeaInfo->present, SATINUSE)) {
memset(&nmeaInfo->satinfo.in_use, 0, sizeof(nmeaInfo->satinfo.in_use));
}
if (!nmea_INFO_is_present(nmeaInfo->present, SATINVIEW)) {
nmeaInfo->satinfo.inview = 0;
memset(&nmeaInfo->satinfo.sat, 0, sizeof(nmeaInfo->satinfo.sat));
}
/*
* lat
*/
lat = nmeaInfo->lat;
lon = nmeaInfo->lon;
/* force lat in [-18000, 18000] */
while (lat < -18000.0) {
lat += 36000.0;
latAdjusted = true;
}
while (lat > 18000.0) {
lat -= 36000.0;
latAdjusted = true;
}
/* lat is now in [-18000, 18000] */
/* force lat from <9000, 18000] in [9000, 0] */
if (lat > 9000.0) {
lat = 18000.0 - lat;
lon += 18000.0;
latAdjusted = true;
lonAdjusted = true;
}
/* force lat from [-18000, -9000> in [0, -9000] */
if (lat < -9000.0) {
lat = -18000.0 - lat;
lon += 18000.0;
latAdjusted = true;
lonAdjusted = true;
}
/* lat is now in [-9000, 9000] */
if (latAdjusted) {
nmeaInfo->lat = lat;
}
/*
* lon
*/
/* force lon in [-18000, 18000] */
while (lon < -18000.0) {
lon += 36000.0;
lonAdjusted = true;
}
while (lon > 18000.0) {
lon -= 36000.0;
lonAdjusted = true;
}
/* lon is now in [-18000, 18000] */
if (lonAdjusted) {
nmeaInfo->lon = lon;
}
/*
* speed
*/
speed = nmeaInfo->speed;
track = nmeaInfo->track;
mtrack = nmeaInfo->mtrack;
if (speed < 0.0) {
speed = -speed;
track += 180.0;
mtrack += 180.0;
speedAdjusted = true;
trackAdjusted = true;
mtrackAdjusted = true;
}
/* speed is now in [0, max> */
if (speedAdjusted) {
nmeaInfo->speed = speed;
}
/*
* track
*/
/* force track in [0, 360> */
while (track < 0.0) {
track += 360.0;
trackAdjusted = true;
}
while (track >= 360.0) {
track -= 360.0;
trackAdjusted = true;
}
/* track is now in [0, 360> */
if (trackAdjusted) {
nmeaInfo->track = track;
}
/*
* mtrack
*/
/* force mtrack in [0, 360> */
while (mtrack < 0.0) {
mtrack += 360.0;
mtrackAdjusted = true;
}
while (mtrack >= 360.0) {
mtrack -= 360.0;
mtrackAdjusted = true;
}
/* mtrack is now in [0, 360> */
if (mtrackAdjusted) {
nmeaInfo->mtrack = mtrack;
}
/*
* magvar
*/
magvar = nmeaInfo->magvar;
/* force magvar in [0, 360> */
while (magvar < 0.0) {
magvar += 360.0;
magvarAdjusted = true;
}
while (magvar >= 360.0) {
magvar -= 360.0;
magvarAdjusted = true;
}
/* magvar is now in [0, 360> */
if (magvarAdjusted) {
nmeaInfo->magvar = magvar;
}
/*
* satinfo
*/
nmeaInfo->satinfo.inuse = 0;
for (inuseIndex = 0; inuseIndex < NMEA_MAXSAT; inuseIndex++) {
if (nmeaInfo->satinfo.in_use[inuseIndex])
nmeaInfo->satinfo.inuse++;
}
nmeaInfo->satinfo.inview = 0;
for (inviewIndex = 0; inviewIndex < NMEA_MAXSAT; inviewIndex++) {
if (nmeaInfo->satinfo.sat[inviewIndex].id) {
nmeaInfo->satinfo.inview++;
/* force elv in [-180, 180] */
while (nmeaInfo->satinfo.sat[inviewIndex].elv < -180) {
nmeaInfo->satinfo.sat[inviewIndex].elv += 360;
}
while (nmeaInfo->satinfo.sat[inviewIndex].elv > 180) {
nmeaInfo->satinfo.sat[inviewIndex].elv -= 360;
}
/* elv is now in [-180, 180] */
/* force elv from <90, 180] in [90, 0] */
if (nmeaInfo->satinfo.sat[inviewIndex].elv > 90) {
nmeaInfo->satinfo.sat[inviewIndex].elv = 180 - nmeaInfo->satinfo.sat[inviewIndex].elv;
}
/* force elv from [-180, -90> in [0, -90] */
if (nmeaInfo->satinfo.sat[inviewIndex].elv < -90) {
nmeaInfo->satinfo.sat[inviewIndex].elv = -180 - nmeaInfo->satinfo.sat[inviewIndex].elv;
}
/* elv is now in [-90, 90] */
if (nmeaInfo->satinfo.sat[inviewIndex].elv < 0) {
nmeaInfo->satinfo.sat[inviewIndex].elv = -nmeaInfo->satinfo.sat[inviewIndex].elv;
}
/* elv is now in [0, 90] */
/* force azimuth in [0, 360> */
while (nmeaInfo->satinfo.sat[inviewIndex].azimuth < 0) {
nmeaInfo->satinfo.sat[inviewIndex].azimuth += 360;
}
while (nmeaInfo->satinfo.sat[inviewIndex].azimuth >= 360) {
nmeaInfo->satinfo.sat[inviewIndex].azimuth -= 360;
}
/* azimuth is now in [0, 360> */
/* force sig in [0, 99] */
if (nmeaInfo->satinfo.sat[inviewIndex].sig < 0)
nmeaInfo->satinfo.sat[inviewIndex].sig = 0;
if (nmeaInfo->satinfo.sat[inviewIndex].sig > 99)
nmeaInfo->satinfo.sat[inviewIndex].sig = 99;
}
}
/* make sure the in_use IDs map to sat IDs */
for (inuseIndex = 0; inuseIndex < NMEA_MAXSAT; inuseIndex++) {
int inuseID = nmeaInfo->satinfo.in_use[inuseIndex];
if (inuseID) {
bool found = false;
for (inviewIndex = 0; inviewIndex < NMEA_MAXSAT; inviewIndex++) {
int inviewID = nmeaInfo->satinfo.sat[inviewIndex].id;
if (inuseID == inviewID) {
found = true;
break;
}
}
if (!found) {
/* clear the id, did not find it */
nmeaInfo->satinfo.in_use[inuseIndex] = 0;
if (nmeaInfo->satinfo.inuse)
nmeaInfo->satinfo.inuse--;
}
}
}
}
/**
Determine whether s position is valid.
@param position
a pointer to a position
@return
- true when valid
- false otherwise
*/
static bool positionValid(PositionUpdateEntry * position) {
return (nmea_INFO_is_present(position->nmeaInfo.present, FIX)
&& (position->nmeaInfo.fix != NMEA_FIX_BAD));
}
/**
Update position average present, smask and fix counters for a new entry or for
an entry that is/will be removed. Update the respective counters when the smask
of the entry has the corresponding flag set. The fix counters count the fix
values separately.
@param positionAverageList
The position average list
@param entry
The entry to update the counters from
@param add
True when updating the counters for a new entry, false for an entry that
is/will be removed
*/
static void updateCounters(PositionAverageList * positionAverageList,
PositionUpdateEntry * entry, bool add) {
PositionUpdateCounters * counters = &positionAverageList->counters;
uint32_t present = entry->nmeaInfo.present;
int smask = entry->nmeaInfo.smask;
#ifndef NDEBUG
unsigned long long maxCount = positionAverageList->entriesMaxCount;
#endif
int amount = (add ? 1 : -1);
/* present */
if (nmea_INFO_is_present(present, SMASK)) {
assert(add ? (counters->smask < maxCount):(counters->smask > 0));
counters->smask += amount;
}
if (nmea_INFO_is_present(present, UTCDATE)) {
assert(add ? (counters->utcdate < maxCount):(counters->utcdate > 0));
counters->utcdate += amount;
}
if (nmea_INFO_is_present(present, UTCTIME)) {
assert(add ? (counters->utctime < maxCount):(counters->utctime > 0));
counters->utctime += amount;
}
if (nmea_INFO_is_present(present, SIG)) {
assert(add ? (counters->sig < maxCount):(counters->sig > 0));
counters->sig += amount;
}
if (nmea_INFO_is_present(present, FIX)) {
assert(add ? (counters->fix < maxCount):(counters->fix > 0));
counters->fix += amount;
}
if (nmea_INFO_is_present(present, PDOP)) {
assert(add ? (counters->pdop < maxCount):(counters->pdop > 0));
counters->pdop += amount;
}
if (nmea_INFO_is_present(present, HDOP)) {
assert(add ? (counters->hdop < maxCount):(counters->hdop > 0));
counters->hdop += amount;
}
if (nmea_INFO_is_present(present, VDOP)) {
assert(add ? (counters->vdop < maxCount):(counters->vdop > 0));
counters->vdop += amount;
}
if (nmea_INFO_is_present(present, LAT)) {
assert(add ? (counters->lat < maxCount):(counters->lat > 0));
counters->lat += amount;
}
if (nmea_INFO_is_present(present, LON)) {
assert(add ? (counters->lon < maxCount):(counters->lon > 0));
counters->lon += amount;
}
if (nmea_INFO_is_present(present, ELV)) {
assert(add ? (counters->elv < maxCount):(counters->elv > 0));
counters->elv += amount;
}
if (nmea_INFO_is_present(present, SPEED)) {
assert(add ? (counters->speed < maxCount):(counters->speed > 0));
counters->speed += amount;
}
if (nmea_INFO_is_present(present, TRACK)) {
assert(add ? (counters->track < maxCount):(counters->track > 0));
counters->track += amount;
}
if (nmea_INFO_is_present(present, MTRACK)) {
assert(add ? (counters->mtrack < maxCount):(counters->mtrack > 0));
counters->mtrack += amount;
}
if (nmea_INFO_is_present(present, MAGVAR)) {
assert(add ? (counters->magvar < maxCount):(counters->magvar > 0));
counters->magvar += amount;
}
if (nmea_INFO_is_present(present, SATINUSECOUNT)) {
assert(add ? (counters->satinusecount < maxCount):(counters->satinusecount > 0));
counters->satinusecount += amount;
}
if (nmea_INFO_is_present(present, SATINUSE)) {
assert(add ? (counters->satinuse < maxCount):(counters->satinuse > 0));
counters->satinuse += amount;
}
if (nmea_INFO_is_present(present, SATINVIEW)) {
assert(add ? (counters->satinview < maxCount):(counters->satinview > 0));
counters->satinview += amount;
}
/* smask */
if ((smask & GPGGA) != 0) {
assert(add ? (counters->gpgga < maxCount):(counters->gpgga > 0));
counters->gpgga += amount;
}
if ((smask & GPGSA) != 0) {
assert(add ? (counters->gpgsa < maxCount):(counters->gpgsa > 0));
counters->gpgsa += amount;
}
if ((smask & GPGSV) != 0) {
assert(add ? (counters->gpgsv < maxCount):(counters->gpgsv > 0));
counters->gpgsv += amount;
}
if ((smask & GPRMC) != 0) {
assert(add ? (counters->gprmc < maxCount):(counters->gprmc > 0));
counters->gprmc += amount;
}
if ((smask & GPVTG) != 0) {
assert(add ? (counters->gpvtg < maxCount):(counters->gpvtg > 0));
counters->gpvtg += amount;
}
/* sig */
if (nmea_INFO_is_present(present, SIG)) {
if (entry->nmeaInfo.sig == NMEA_SIG_HIGH) {
assert(add ? (counters->sigHigh < maxCount):(counters->sigHigh > 0));
counters->sigHigh += amount;
} else if (entry->nmeaInfo.sig == NMEA_SIG_MID) {
assert(add ? (counters->sigMid < maxCount):(counters->sigMid > 0));
counters->sigMid += amount;
} else if (entry->nmeaInfo.sig == NMEA_SIG_LOW) {
assert(add ? (counters->sigLow < maxCount):(counters->sigLow > 0));
counters->sigLow += amount;
} else {
assert(add ? (counters->sigBad < maxCount):(counters->sigBad > 0));
counters->sigBad += amount;
}
}
/* fix */
if (nmea_INFO_is_present(present, FIX)) {
if (entry->nmeaInfo.fix == NMEA_FIX_3D) {
assert(add ? (counters->fix3d < maxCount):(counters->fix3d > 0));
counters->fix3d += amount;
} else if (entry->nmeaInfo.fix == NMEA_FIX_2D) {
assert(add ? (counters->fix2d < maxCount):(counters->fix2d > 0));
counters->fix2d += amount;
} else {
assert(add ? (counters->fixBad < maxCount):(counters->fixBad > 0));
counters->fixBad += amount;
}
}
}
/**
Determine the new smask, sig and fix of the average position based on the
counters. The relevant smask bits (like GPGGA) are only set when all entries
in the average list have that bit set. The sig and fix will be set to the
lowest/worst value of all entries and will only be set to the highest/best
value when all entries in the average list are set to the highest/best value.
@param positionAverageList
The position average list
*/
static void determineCumulativePresentSmaskSigFix(
PositionAverageList * positionAverageList) {
PositionUpdateEntry * cumulative =
&positionAverageList->positionAverageCumulative;
PositionUpdateCounters * counters = &positionAverageList->counters;
unsigned long long count = positionAverageList->entriesCount;
/* present */
cumulative->nmeaInfo.present = 0;
if (counters->smask >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, SMASK);
}
if (counters->utcdate >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, UTCDATE);
}
if (counters->utctime >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, UTCTIME);
}
if (counters->sig >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, SIG);
}
if (counters->fix >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, FIX);
}
if (counters->pdop >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, PDOP);
}
if (counters->hdop >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, HDOP);
}
if (counters->vdop >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, VDOP);
}
if (counters->lat >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, LAT);
}
if (counters->lon >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, LON);
}
if (counters->elv >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, ELV);
}
if (counters->speed >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, SPEED);
}
if (counters->track >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, TRACK);
}
if (counters->mtrack >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, MTRACK);
}
if (counters->magvar >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, MAGVAR);
}
if (counters->satinusecount >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, SATINUSECOUNT);
}
if (counters->satinuse >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, SATINUSE);
}
if (counters->satinview >= count) {
nmea_INFO_set_present(&cumulative->nmeaInfo.present, SATINVIEW);
}
/* smask */
cumulative->nmeaInfo.smask = 0;
if (counters->gpgga >= count) {
cumulative->nmeaInfo.smask |= GPGGA;
}
if (counters->gpgsa >= count) {
cumulative->nmeaInfo.smask |= GPGSA;
}
if (counters->gpgsv >= count) {
cumulative->nmeaInfo.smask |= GPGSV;
}
if (counters->gprmc >= count) {
cumulative->nmeaInfo.smask |= GPRMC;
}
if (counters->gpvtg >= count) {
cumulative->nmeaInfo.smask |= GPVTG;
}
/* sig */
cumulative->nmeaInfo.sig = NMEA_SIG_BAD;
if (nmea_INFO_is_present(cumulative->nmeaInfo.present, SIG)) {
if (counters->sigBad == 0) {
if (counters->sigHigh >= count) {
cumulative->nmeaInfo.sig = NMEA_SIG_HIGH;
} else if (counters->sigMid > 0) {
cumulative->nmeaInfo.sig = NMEA_SIG_MID;
} else if (counters->sigLow > 0) {
cumulative->nmeaInfo.sig = NMEA_SIG_LOW;
}
}
}
/* fix */
cumulative->nmeaInfo.fix = NMEA_FIX_BAD;
if (nmea_INFO_is_present(cumulative->nmeaInfo.present, FIX)) {
if (counters->fixBad == 0) {
if (counters->fix3d >= count) {
cumulative->nmeaInfo.fix = NMEA_FIX_3D;
} else if (counters->fix2d > 0) {
cumulative->nmeaInfo.fix = NMEA_FIX_2D;
}
}
}
}
/**
Convert a nmeaINFO structure into an OLSR message.
@param nmeaInfo
A pointer to a nmeaINFO structure
@param olsrMessage
A pointer to an OLSR message in which to place the converted information
@param olsrMessageSize
The maximum number of bytes available for the olsrMessage
@param validityTime
the validity time of the message in seconds
@return
- the aligned size of the converted information
- 0 (zero) in case of an error
*/
unsigned int gpsToOlsr(nmeaINFO *nmeaInfo, union olsr_message *olsrMessage,
unsigned int olsrMessageSize, unsigned long long validityTime) {
unsigned int aligned_size;
unsigned int aligned_size_remainder;
size_t nodeLength;
nodeIdBinaryType * nodeIdBinary = NULL;
PudOlsrPositionUpdate * olsrGpsMessage =
getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
/*
* Compose message contents
*/
memset(olsrGpsMessage, 0, sizeof (PudOlsrPositionUpdate));
setPositionUpdateVersion(olsrGpsMessage, PUD_WIRE_FORMAT_VERSION);
setValidityTime(&olsrGpsMessage->validityTime, validityTime);
setPositionUpdatePresent(olsrGpsMessage, nmeaInfo->present & ~PUD_PRESENT_GATEWAY);
/* utc is always present, we make sure of that elsewhere, so just use it */
setPositionUpdateTime(olsrGpsMessage, nmeaInfo->utc.hour, nmeaInfo->utc.min,
nmeaInfo->utc.sec);
if (likely(nmea_INFO_is_present(nmeaInfo->present, LAT))) {
setPositionUpdateLatitude(olsrGpsMessage, nmeaInfo->lat);
} else {
setPositionUpdateLatitude(olsrGpsMessage, 0.0);
}
if (likely(nmea_INFO_is_present(nmeaInfo->present, LON))) {
setPositionUpdateLongitude(olsrGpsMessage, nmeaInfo->lon);
} else {
setPositionUpdateLongitude(olsrGpsMessage, 0.0);
}
if (likely(nmea_INFO_is_present(nmeaInfo->present, ELV))) {
setPositionUpdateAltitude(olsrGpsMessage, nmeaInfo->elv);
} else {
setPositionUpdateAltitude(olsrGpsMessage, 0.0);
}
if (likely(nmea_INFO_is_present(nmeaInfo->present, SPEED))) {
setPositionUpdateSpeed(olsrGpsMessage, nmeaInfo->speed);
} else {
setPositionUpdateSpeed(olsrGpsMessage, 0.0);
}
if (likely(nmea_INFO_is_present(nmeaInfo->present, TRACK))) {
setPositionUpdateTrack(olsrGpsMessage, nmeaInfo->track);
} else {
setPositionUpdateTrack(olsrGpsMessage, 0);
}
if (likely(nmea_INFO_is_present(nmeaInfo->present, HDOP))) {
setPositionUpdateHdop(olsrGpsMessage, nmeaInfo->HDOP);
} else {
setPositionUpdateHdop(olsrGpsMessage, PUD_HDOP_MAX);
}
nodeIdBinary = getNodeIdBinary();
nodeLength = setPositionUpdateNodeInfo(olsr_cnf->ip_version, olsrGpsMessage,
olsrMessageSize, getNodeIdTypeNumber(),
(unsigned char *) &nodeIdBinary->buffer, nodeIdBinary->length);
/*
* Messages in OLSR are 4-byte aligned: align
*/
/* size = type, string, string terminator */
aligned_size = PUD_OLSRWIREFORMATSIZE + nodeLength;
aligned_size_remainder = (aligned_size % 4);
if (aligned_size_remainder != 0) {
aligned_size += (4 - aligned_size_remainder);
}
/*
* Fill message headers (fill ALL fields, except message)
* Note: olsr_vtime is currently unused, we use it for our validity time.
*/
if (olsr_cnf->ip_version == AF_INET) {
/* IPv4 */
olsrMessage->v4.olsr_msgtype = PUD_OLSR_MSG_TYPE;
olsrMessage->v4.olsr_vtime = reltime_to_me(validityTime * 1000);
/* message->v4.olsr_msgsize at the end */
olsrMessage->v4.originator = olsr_cnf->main_addr.v4.s_addr;
olsrMessage->v4.ttl = getOlsrTtl();
olsrMessage->v4.hopcnt = 0;
olsrMessage->v4.seqno = htons(get_msg_seqno());
/* add length of message->v4 fields */
aligned_size += (sizeof(olsrMessage->v4)
- sizeof(olsrMessage->v4.message));
olsrMessage->v4.olsr_msgsize = htons(aligned_size);
} else {
/* IPv6 */
olsrMessage->v6.olsr_msgtype = PUD_OLSR_MSG_TYPE;
olsrMessage->v6.olsr_vtime = reltime_to_me(validityTime * 1000);
/* message->v6.olsr_msgsize at the end */
olsrMessage->v6.originator = olsr_cnf->main_addr.v6;
olsrMessage->v6.ttl = getOlsrTtl();
olsrMessage->v6.hopcnt = 0;
olsrMessage->v6.seqno = htons(get_msg_seqno());
/* add length of message->v6 fields */
aligned_size += (sizeof(olsrMessage->v6)
- sizeof(olsrMessage->v6.message));
olsrMessage->v6.olsr_msgsize = htons(aligned_size);
}
/* pad with zeroes */
if (aligned_size_remainder != 0) {
memset(&(((char *) &olsrGpsMessage->nodeInfo.nodeIdType)[nodeLength]),
0, (4 - aligned_size_remainder));
}
return aligned_size;
}
/**
Convert an OLSR message into a string to multicast on the LAN
@param olsrMessage
A pointer to the OLSR message
@param txGpsBuffer
A pointer to the buffer in which the transmit string can be written
@param txGpsBufferSize
The size of the txGpsBuffer
@return
- the length of the transmit string placed in the txGpsBuffer
- 0 (zero) in case of an error
*/
unsigned int gpsFromOlsr(union olsr_message *olsrMessage,
unsigned char * txGpsBuffer, unsigned int txGpsBufferSize) {
unsigned long validityTime;
struct tm timeStruct;
char latitudeString[PUD_TX_LATITUDE_DIGITS];
const char * latitudeHemisphere;
char longitudeString[PUD_TX_LONGITUDE_DIGITS];
const char * longitudeHemisphere;
char altitudeString[PUD_TX_ALTITUDE_DIGITS];
char speedString[PUD_TX_SPEED_DIGITS];
char trackString[PUD_TX_TRACK_DIGITS];
char hdopString[PUD_TX_HDOP_DIGITS];
uint32_t present;
char gateway[2] = { '0', '\0' };
char nodeIdTypeString[PUD_TX_NODEIDTYPE_DIGITS];
char nodeIdString[PUD_TX_NODEID_BUFFERSIZE];
const char * nodeId;
const void * ipAddr;
char originatorBuffer[64];
const char * originator;
unsigned int transmitStringLength;
PudOlsrPositionUpdate * olsrGpsMessage =
getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
if (unlikely(getPositionUpdateVersion(olsrGpsMessage) != PUD_WIRE_FORMAT_VERSION)) {
/* currently we can only handle our own version */
pudError(false, "Can not handle version %u OLSR PUD messages"
" (only version %u): message ignored",
getPositionUpdateVersion(olsrGpsMessage), PUD_WIRE_FORMAT_VERSION);
return 0;
}
ipAddr = (olsr_cnf->ip_version == AF_INET) ?
(void *) &olsrMessage->v4.originator :
(void *) &olsrMessage->v6.originator;
originator = inet_ntop(olsr_cnf->ip_version, ipAddr, &originatorBuffer[0],
sizeof(originatorBuffer));
validityTime = getValidityTime(&olsrGpsMessage->validityTime);
present = getPositionUpdatePresent(olsrGpsMessage);
if (present & PUD_PRESENT_GATEWAY) {
gateway[0] = '1';
}
/* time is ALWAYS present so we can just use it */
getPositionUpdateTime(olsrGpsMessage, time(NULL), &timeStruct);
if (likely(nmea_INFO_is_present(present, LAT))) {
double latitude = getPositionUpdateLatitude(olsrGpsMessage);
if (latitude >= 0) {
latitudeHemisphere = "N";
} else {
latitudeHemisphere = "S";
latitude = -latitude;
}
latitude = nmea_degree2ndeg(latitude);
snprintf(&latitudeString[0], PUD_TX_LATITUDE_DIGITS, "%." PUD_TX_LATITUDE_DECIMALS "f", latitude);
} else {
latitudeHemisphere = "";
latitudeString[0] = '\0';
}
if (likely(nmea_INFO_is_present(present, LON))) {
double longitude = getPositionUpdateLongitude(olsrGpsMessage);
if (longitude >= 0) {
longitudeHemisphere = "E";
} else {
longitudeHemisphere = "W";
longitude = -longitude;
}
longitude = nmea_degree2ndeg(longitude);
snprintf(&longitudeString[0], PUD_TX_LONGITUDE_DIGITS, "%." PUD_TX_LONGITUDE_DECIMALS "f", longitude);
} else {
longitudeHemisphere = "";
longitudeString[0] = '\0';
}
if (likely(nmea_INFO_is_present(present, ELV))) {
snprintf(&altitudeString[0], PUD_TX_ALTITUDE_DIGITS, "%ld", getPositionUpdateAltitude(olsrGpsMessage));
} else {
altitudeString[0] = '\0';
}
if (likely(nmea_INFO_is_present(present, SPEED))) {
snprintf(&speedString[0], PUD_TX_SPEED_DIGITS, "%lu", getPositionUpdateSpeed(olsrGpsMessage));
} else {
speedString[0] = '\0';
}
if (likely(nmea_INFO_is_present(present, TRACK))) {
snprintf(&trackString[0], PUD_TX_TRACK_DIGITS, "%lu", getPositionUpdateTrack(olsrGpsMessage));
} else {
trackString[0] = '\0';
}
if (likely(nmea_INFO_is_present(present, HDOP))) {
snprintf(&hdopString[0], PUD_TX_HDOP_DIGITS, "%." PUD_TX_HDOP_DECIMALS "f",
nmea_meters2dop(getPositionUpdateHdop(olsrGpsMessage)));
} else {
hdopString[0] = '\0';
}
getNodeTypeStringFromOlsr(olsr_cnf->ip_version, olsrGpsMessage,
&nodeIdTypeString[0], sizeof(nodeIdTypeString));
getNodeIdStringFromOlsr(olsr_cnf->ip_version, olsrMessage, &nodeId,
&nodeIdString[0], sizeof(nodeIdString));
transmitStringLength = nmea_printf((char *) txGpsBuffer, txGpsBufferSize
- 1, "$P%s," /* prefix (always) */
"%u," /* sentence version (always) */
"%s," /* gateway flag (always) */
"%s," /* OLSR originator (always) */
"%s,%s," /* nodeIdType/nodeId (always) */
"%02u%02u%02u," /* date (always) */
"%02u%02u%02u," /* time (always) */
"%lu," /* validity time (always) */
"%s,%s," /* latitude (optional) */
"%s,%s," /* longitude (optional) */
"%s," /* altitude (optional) */
"%s," /* speed (optional) */
"%s," /* track (optional) */
"%s" /* hdop (optional) */
, getTxNmeaMessagePrefix(), PUD_TX_SENTENCE_VERSION, &gateway[0],
originator , &nodeIdTypeString[0],
nodeId, timeStruct.tm_mday, timeStruct.tm_mon + 1, (timeStruct.tm_year
% 100), timeStruct.tm_hour, timeStruct.tm_min,
timeStruct.tm_sec, validityTime, &latitudeString[0],
latitudeHemisphere, &longitudeString[0], longitudeHemisphere,
&altitudeString[0], &speedString[0], &trackString[0],
&hdopString[0]);
if (unlikely(transmitStringLength > (txGpsBufferSize - 1))) {
pudError(false, "String to transmit on non-OLSR is too large, need"
" at least %u bytes, skipped", transmitStringLength);
return 0;
}
if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
txGpsBuffer[txGpsBufferSize - 1] = '\0';
} else {
txGpsBuffer[transmitStringLength] = '\0';
}
return transmitStringLength;
}
