/**
* 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;
}
}
/**
* Clear an info structure.
* Resets the time to now, sets up the signal as BAD, the FIX as BAD, and
* signals presence of these fields.
* Resets all other fields to 0.
*
* @param info a pointer to the structure
*/
void nmea_zero_INFO(nmeaINFO *info) {
if (!info) {
return;
}
memset(info, 0, sizeof(nmeaINFO));
nmea_time_now(&info->utc, &info->present);
info->sig = NMEA_SIG_BAD;
nmea_INFO_set_present(&info->present, SIG);
info->fix = NMEA_FIX_BAD;
nmea_INFO_set_present(&info->present, FIX);
}
/**
* 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 */
}
/**
* 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 */
}
/**
* 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;
}
}
/**
* Reset the time to now
*
* @param utc a pointer to the time structure
* @param present a pointer to a present field. when non-NULL then the UTCDATE
* and UTCTIME flags are set in it.
*/
void nmea_time_now(nmeaTIME *utc, uint32_t * present) {
struct timeval tp;
struct tm tt;
assert(utc);
gettimeofday(&tp, NULL );
gmtime_r(&tp.tv_sec, &tt);
utc->year = tt.tm_year;
utc->mon = tt.tm_mon;
utc->day = tt.tm_mday;
utc->hour = tt.tm_hour;
utc->min = tt.tm_min;
utc->sec = tt.tm_sec;
utc->hsec = (tp.tv_usec / 10000);
if (present) {
nmea_INFO_set_present(present, UTCDATE | UTCTIME);
}
}
/**
* Reset the time to now
*
* @param utc a pointer to the time structure
* @param present a pointer to a present field. when non-NULL then the UTCDATE
* and UTCTIME flags are set in it.
*/
void nmea_time_now(nmeaTIME *utc, uint32_t * present) {
struct tm tt;
RTCDateTime timespec;
uint32_t tv_msec;
NMEA_ASSERT(utc);
rtcGetTime(&RTCD1, ×pec);
rtcConvertDateTimeToStructTm(×pec, &tt, &tv_msec);
utc->year = tt.tm_year;
utc->mon = tt.tm_mon;
utc->day = tt.tm_mday;
utc->hour = tt.tm_hour;
utc->min = tt.tm_min;
utc->sec = tt.tm_sec;
utc->hsec = (tv_msec / 10);
if (present) {
nmea_INFO_set_present(present, UTCDATE | UTCTIME);
}
}
/**
* 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;
}
}
/**
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;
}
}
}
}
/**
* Parse a GPVTG sentence from a string
*
* @param s the string
* @param len the length of the string
* @param pack a pointer to the result structure
* @return 1 (true) - if parsed successfully or 0 (false) otherwise.
*/
int nmea_parse_GPVTG(const char *s, const int len, nmeaGPVTG *pack) {
int token_count;
assert(s);
assert(pack);
nmea_trace_buff(s, len);
/*
* Clear before parsing, to be able to detect absent fields
*/
pack->present = 0;
pack->track = NAN;
pack->track_t = 0;
pack->mtrack = NAN;
pack->mtrack_m = 0;
pack->spn = NAN;
pack->spn_n = 0;
pack->spk = NAN;
pack->spk_k = 0;
/* parse */
token_count = nmea_scanf(s, len, "$GPVTG,%f,%c,%f,%c,%f,%c,%f,%c*", &pack->track, &pack->track_t, &pack->mtrack,
&pack->mtrack_m, &pack->spn, &pack->spn_n, &pack->spk, &pack->spk_k);
/* see that we have enough tokens */
if (token_count != 8) {
nmea_error("GPVTG parse error: need 8 tokens, got %d in %s", token_count, s);
return 0;
}
/* determine which fields are present and validate them */
if (!isnan(pack->track) && (pack->track_t)) {
pack->track_t = toupper(pack->track_t);
if (pack->track_t != 'T') {
nmea_error("GPVTG parse error: invalid track unit, got %c, expected T", pack->track_t);
return 0;
}
nmea_INFO_set_present(&pack->present, TRACK);
}
if (!isnan(pack->mtrack) && (pack->mtrack_m)) {
pack->mtrack_m = toupper(pack->mtrack_m);
if (pack->mtrack_m != 'M') {
nmea_error("GPVTG parse error: invalid mtrack unit, got %c, expected M", pack->mtrack_m);
return 0;
}
nmea_INFO_set_present(&pack->present, MTRACK);
}
if (!isnan(pack->spn) && (pack->spn_n)) {
pack->spn_n = toupper(pack->spn_n);
if (pack->spn_n != 'N') {
nmea_error("GPVTG parse error: invalid knots speed unit, got %c, expected N", pack->spn_n);
return 0;
}
nmea_INFO_set_present(&pack->present, SPEED);
if (isnan(pack->spk)) {
pack->spk = pack->spn * NMEA_TUD_KNOTS;
pack->spk_k = 'K';
}
}
if (!isnan(pack->spk) && (pack->spk_k)) {
pack->spk_k = toupper(pack->spk_k);
if (pack->spk_k != 'K') {
nmea_error("GPVTG parse error: invalid kph speed unit, got %c, expected K", pack->spk_k);
return 0;
}
nmea_INFO_set_present(&pack->present, SPEED);
if (isnan(pack->spn)) {
pack->spn = pack->spk / NMEA_TUD_KNOTS;
pack->spn_n = 'N';
}
}
return 1;
}
/**
* Parse a GPRMC sentence from a string
*
* @param s the string
* @param len the length of the string
* @param pack a pointer to the result structure
* @return 1 (true) - if parsed successfully or 0 (false) otherwise.
*/
int nmea_parse_GPRMC(const char *s, const int len, nmeaGPRMC *pack) {
int token_count;
char time_buff[NMEA_TIMEPARSE_BUF];
int date;
size_t time_buff_len = 0;
assert(s);
assert(pack);
nmea_trace_buff(s, len);
/*
* Clear before parsing, to be able to detect absent fields
*/
time_buff[0] = '\0';
date = -1;
pack->present = 0;
pack->utc.year = -1;
pack->utc.mon = -1;
pack->utc.day = -1;
pack->utc.hour = -1;
pack->utc.min = -1;
pack->utc.sec = -1;
pack->utc.hsec = -1;
pack->status = 0;
pack->lat = NAN;
pack->ns = 0;
pack->lon = NAN;
pack->ew = 0;
pack->speed = NAN;
pack->track = NAN;
pack->magvar = NAN;
pack->magvar_ew = 0;
pack->mode = 0;
/* parse */
token_count = nmea_scanf(s, len, "$GPRMC,%s,%c,%f,%c,%f,%c,%f,%f,%d,%f,%c,%c*", &time_buff[0], &pack->status,
&pack->lat, &pack->ns, &pack->lon, &pack->ew, &pack->speed, &pack->track, &date,
&pack->magvar, &pack->magvar_ew, &pack->mode);
/* see that we have enough tokens */
if ((token_count != 11) && (token_count != 12)) {
nmea_error("GPRMC parse error: need 11 or 12 tokens, got %d in %s", token_count, s);
return 0;
}
/* determine which fields are present and validate them */
time_buff_len = strlen(&time_buff[0]);
if (time_buff_len) {
if (!_nmea_parse_time(&time_buff[0], time_buff_len, &pack->utc)) {
return 0;
}
if (!validateTime(&pack->utc)) {
return 0;
}
nmea_INFO_set_present(&pack->present, UTCTIME);
}
if (!pack->status) {
pack->status = 'V';
} else {
pack->status = toupper(pack->status);
if (!((pack->status == 'A') || (pack->status == 'V'))) {
nmea_error("GPRMC parse error: invalid status (%c)", pack->status);
return 0;
}
}
if (!isnan(pack->lat) && (pack->ns)) {
if (!validateNSEW(&pack->ns, true)) {
return 0;
}
nmea_INFO_set_present(&pack->present, LAT);
}
if (!isnan(pack->lon) && (pack->ew)) {
if (!validateNSEW(&pack->ew, false)) {
return 0;
}
nmea_INFO_set_present(&pack->present, LON);
}
if (!isnan(pack->speed)) {
nmea_INFO_set_present(&pack->present, SPEED);
}
if (!isnan(pack->track)) {
nmea_INFO_set_present(&pack->present, TRACK);
}
if (date != -1) {
if (!_nmea_parse_date(date, &pack->utc)) {
return 0;
}
if (!validateDate(&pack->utc)) {
return 0;
}
nmea_INFO_set_present(&pack->present, UTCDATE);
}
if (!isnan(pack->magvar) && (pack->magvar_ew)) {
if (!validateNSEW(&pack->magvar_ew, false)) {
return 0;
}
nmea_INFO_set_present(&pack->present, MAGVAR);
}
if (token_count == 11) {
pack->mode = 'A';
} else {
if (!pack->mode) {
pack->mode = 'N';
} else {
if (!validateMode(&pack->mode)) {
return 0;
}
}
}
return 1;
}
/**
* Parse a GPGSV sentence from a string
*
* @param s the string
* @param len the length of the string
* @param pack a pointer to the result structure
* @return 1 (true) - if parsed successfully or 0 (false) otherwise.
*/
int nmea_parse_GPGSV(const char *s, const int len, nmeaGPGSV *pack) {
int token_count;
int token_count_expected;
int sat_count;
int sat_counted = 0;
assert(s);
assert(pack);
nmea_trace_buff(s, len);
/*
* Clear before parsing, to be able to detect absent fields
*/
memset(pack, 0, sizeof(nmeaGPGSV));
/* parse */
token_count = nmea_scanf(s, len, "$GPGSV,%d,%d,%d,"
"%d,%d,%d,%d,"
"%d,%d,%d,%d,"
"%d,%d,%d,%d,"
"%d,%d,%d,%d*", &pack->pack_count, &pack->pack_index, &pack->sat_count, &pack->sat_data[0].id,
&pack->sat_data[0].elv, &pack->sat_data[0].azimuth, &pack->sat_data[0].sig, &pack->sat_data[1].id,
&pack->sat_data[1].elv, &pack->sat_data[1].azimuth, &pack->sat_data[1].sig, &pack->sat_data[2].id,
&pack->sat_data[2].elv, &pack->sat_data[2].azimuth, &pack->sat_data[2].sig, &pack->sat_data[3].id,
&pack->sat_data[3].elv, &pack->sat_data[3].azimuth, &pack->sat_data[3].sig);
/* return if we have no sentences or sats */
if ((pack->pack_count < 1) || (pack->pack_count > NMEA_NSATPACKS) || (pack->pack_index < 1)
|| (pack->pack_index > pack->pack_count) || (pack->sat_count < 0) || (pack->sat_count > NMEA_MAXSAT)) {
nmea_error("GPGSV parse error: inconsistent pack (count/index/satcount = %d/%d/%d)", pack->pack_count,
pack->pack_index, pack->sat_count);
return 0;
}
/* validate all sat settings and count the number of sats in the sentence */
for (sat_count = 0; sat_count < NMEA_SATINPACK; sat_count++) {
if (pack->sat_data[sat_count].id != 0) {
if ((pack->sat_data[sat_count].id < 0)) {
nmea_error("GPGSV parse error: invalid sat %d id (%d)", sat_count + 1, pack->sat_data[sat_count].id);
return 0;
}
if ((pack->sat_data[sat_count].elv < -90) || (pack->sat_data[sat_count].elv > 90)) {
nmea_error("GPGSV parse error: invalid sat %d elevation (%d)", sat_count + 1, pack->sat_data[sat_count].elv);
return 0;
}
if ((pack->sat_data[sat_count].azimuth < 0) || (pack->sat_data[sat_count].azimuth >= 360)) {
nmea_error("GPGSV parse error: invalid sat %d azimuth (%d)", sat_count + 1, pack->sat_data[sat_count].azimuth);
return 0;
}
if ((pack->sat_data[sat_count].sig < 0) || (pack->sat_data[sat_count].sig > 99)) {
nmea_error("GPGSV parse error: invalid sat %d signal (%d)", sat_count + 1, pack->sat_data[sat_count].sig);
return 0;
}
sat_counted++;
}
}
/* see that we have enough tokens */
token_count_expected = (sat_counted * 4) + 3;
if ((token_count < token_count_expected) || (token_count > (NMEA_SATINPACK * 4 + 3))) {
nmea_error("GPGSV parse error: need %d tokens, got %d", token_count_expected, token_count);
return 0;
}
/* determine which fields are present and validate them */
if (pack->sat_count > 0) {
nmea_INFO_set_present(&pack->present, SATINVIEW);
}
return 1;
}
/**
* Parse a GPGSA sentence from a string
*
* @param s the string
* @param len the length of the string
* @param pack a pointer to the result structure
* @return 1 (true) - if parsed successfully or 0 (false) otherwise.
*/
int nmea_parse_GPGSA(const char *s, const int len, nmeaGPGSA *pack) {
int token_count;
int i;
assert(s);
assert(pack);
nmea_trace_buff(s, len);
/*
* Clear before parsing, to be able to detect absent fields
*/
pack->present = 0;
pack->fix_mode = 0;
pack->fix_type = -1;
for (i = 0; i < NMEA_MAXSAT; i++) {
pack->sat_prn[i] = 0;
}
pack->PDOP = NAN;
pack->HDOP = NAN;
pack->VDOP = NAN;
/* parse */
token_count = nmea_scanf(s, len, "$GPGSA,%c,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f*", &pack->fix_mode,
&pack->fix_type, &pack->sat_prn[0], &pack->sat_prn[1], &pack->sat_prn[2], &pack->sat_prn[3],
&pack->sat_prn[4], &pack->sat_prn[5], &pack->sat_prn[6], &pack->sat_prn[7], &pack->sat_prn[8],
&pack->sat_prn[9], &pack->sat_prn[10], &pack->sat_prn[11], &pack->PDOP, &pack->HDOP, &pack->VDOP);
/* see that we have enough tokens */
if (token_count != 17) {
nmea_error("GPGSA parse error: need 17 tokens, got %d in %s", token_count, s);
return 0;
}
/* determine which fields are present and validate them */
pack->fix_mode = toupper(pack->fix_mode);
if (!((pack->fix_mode == 'A') || (pack->fix_mode == 'M'))) {
nmea_error("GPGSA parse error: invalid fix mode (%c)", pack->fix_mode);
return 0;
}
if (pack->fix_type != -1) {
if (!((pack->fix_type >= NMEA_FIX_FIRST) && (pack->fix_type <= NMEA_FIX_LAST))) {
nmea_error("GPGSA parse error: invalid fix type %d, expected [%d, %d]", pack->fix_type, NMEA_FIX_FIRST, NMEA_FIX_LAST);
return 0;
}
nmea_INFO_set_present(&pack->present, FIX);
}
for (i = 0; i < NMEA_MAXSAT; i++) {
if (pack->sat_prn[i] != 0) {
nmea_INFO_set_present(&pack->present, SATINUSE);
break;
}
}
if (!isnan(pack->PDOP)) {
nmea_INFO_set_present(&pack->present, PDOP);
}
if (!isnan(pack->HDOP)) {
nmea_INFO_set_present(&pack->present, HDOP);
}
if (!isnan(pack->VDOP)) {
nmea_INFO_set_present(&pack->present, VDOP);
}
return 1;
}
/**
* Parse a GPGGA sentence from a string
*
* @param s the string
* @param len the length of the string
* @param pack a pointer to the result structure
* @return 1 (true) - if parsed successfully or 0 (false) otherwise.
*/
int nmea_parse_GPGGA(const char *s, const int len, nmeaGPGGA *pack) {
int token_count;
char time_buff[NMEA_TIMEPARSE_BUF];
size_t time_buff_len = 0;
assert(s);
assert(pack);
nmea_trace_buff(s, len);
/*
* Clear before parsing, to be able to detect absent fields
*/
time_buff[0] = '\0';
pack->present = 0;
pack->utc.hour = -1;
pack->utc.min = -1;
pack->utc.sec = -1;
pack->utc.hsec = -1;
pack->lat = NAN;
pack->ns = 0;
pack->lon = NAN;
pack->ew = 0;
pack->sig = -1;
pack->satinuse = -1;
pack->HDOP = NAN;
pack->elv = NAN;
pack->elv_units = 0;
pack->diff = 0; /* ignored */
pack->diff_units = 0; /* ignored */
pack->dgps_age = 0; /* ignored */
pack->dgps_sid = 0; /* ignored */
/* parse */
token_count = nmea_scanf(s, len, "$GPGGA,%s,%f,%c,%f,%c,%d,%d,%f,%f,%c,%f,%c,%f,%d*", &time_buff[0], &pack->lat,
&pack->ns, &pack->lon, &pack->ew, &pack->sig, &pack->satinuse, &pack->HDOP, &pack->elv, &pack->elv_units,
&pack->diff, &pack->diff_units, &pack->dgps_age, &pack->dgps_sid);
/* see that we have enough tokens */
if (token_count != 14) {
nmea_error("GPGGA parse error: need 14 tokens, got %d in %s", token_count, s);
return 0;
}
/* determine which fields are present and validate them */
time_buff_len = strlen(&time_buff[0]);
if (time_buff_len > (NMEA_TIMEPARSE_BUF - 1))
time_buff_len = NMEA_TIMEPARSE_BUF - 1;
if (time_buff_len) {
if (!_nmea_parse_time(&time_buff[0], time_buff_len, &pack->utc)) {
return 0;
}
if (!validateTime(&pack->utc)) {
return 0;
}
nmea_INFO_set_present(&pack->present, UTCTIME);
}
if (!isnan(pack->lat) && (pack->ns)) {
if (!validateNSEW(&pack->ns, true)) {
return 0;
}
nmea_INFO_set_present(&pack->present, LAT);
}
if (!isnan(pack->lon) && (pack->ew)) {
if (!validateNSEW(&pack->ew, false)) {
return 0;
}
nmea_INFO_set_present(&pack->present, LON);
}
if (pack->sig != -1) {
if (!((pack->sig >= NMEA_SIG_FIRST) && (pack->sig <= NMEA_SIG_LAST))) {
nmea_error("GPGGA parse error: invalid signal %d, expected [%d, %d]", pack->sig, NMEA_SIG_FIRST, NMEA_SIG_LAST);
return 0;
}
nmea_INFO_set_present(&pack->present, SIG);
}
if (pack->satinuse != -1) {
nmea_INFO_set_present(&pack->present, SATINUSECOUNT);
}
if (!isnan(pack->HDOP)) {
nmea_INFO_set_present(&pack->present, HDOP);
}
if (!isnan(pack->elv) && (pack->elv_units)) {
if (pack->elv_units != 'M') {
nmea_error("GPGGA parse error: invalid elevation unit (%c)", pack->elv_units);
return 0;
}
nmea_INFO_set_present(&pack->present, ELV);
}
/* ignore diff and diff_units */
/* ignore dgps_age and dgps_sid */
return 1;
}
/**
* Read the position file
* @param fileName the filename
* @param nmeaInfo the NMEA data
*/
bool readPositionFile(char * fileName, nmeaINFO * nmeaInfo) {
bool retval = false;
struct stat statBuf;
nmeaINFO result;
FILE * fd = NULL;
unsigned int lineNumber = 0;
char * name = NULL;
char * value = NULL;
if (stat(fileName, &statBuf)) {
/* could not access the file */
goto out;
}
if (!memcmp(&cachedStat.timeStamp, &statBuf.st_mtime, sizeof(cachedStat.timeStamp))) {
/* file did not change since last read */
goto out;
}
fd = fopen(fileName, "r");
if (!fd) {
goto out;
}
nmea_zero_INFO(&result);
result.sig = POSFILE_DEFAULT_SIG;
result.fix = POSFILE_DEFAULT_FIX;
result.HDOP = POSFILE_DEFAULT_HDOP;
result.VDOP = POSFILE_CALCULATED_VDOP(result.HDOP);
result.PDOP = POSFILE_CALCULATED_PDOP(result.HDOP);
result.lat = POSFILE_DEFAULT_LAT;
result.lon = POSFILE_DEFAULT_LON;
result.elv = POSFILE_DEFAULT_ELV;
result.speed = POSFILE_DEFAULT_SPEED;
result.track = POSFILE_DEFAULT_TRACK;
result.mtrack = POSFILE_DEFAULT_MTRACK;
result.magvar = POSFILE_DEFAULT_MAGVAR;
memcpy(&cachedStat.timeStamp, &statBuf.st_mtime, sizeof(cachedStat.timeStamp));
while (fgets(line, LINE_LENGTH, fd)) {
regmatch_t pmatch[regexNameValuematchCount];
lineNumber++;
if (regexMatch(®exComment, line, 0, NULL)) {
continue;
}
if (!regexMatch(®exNameValue, line, regexNameValuematchCount, pmatch)) {
pudError(false, "Position file \"%s\", line %d uses invalid syntax: %s", fileName, lineNumber, line);
goto out;
}
/* determine name/value */
name = &line[pmatch[1].rm_so];
line[pmatch[1].rm_eo] = '\0';
value = &line[pmatch[2].rm_so];
line[pmatch[2].rm_eo] = '\0';
if (!strncasecmp(POSFILE_NAME_SIG, name, sizeof(line))) {
if (!strncasecmp(POSFILE_VALUE_SIG_BAD, value, sizeof(line))) {
result.sig = NMEA_SIG_BAD;
} else if (!strncasecmp(POSFILE_VALUE_SIG_LOW, value, sizeof(line))) {
result.sig = NMEA_SIG_LOW;
} else if (!strncasecmp(POSFILE_VALUE_SIG_MID, value, sizeof(line))) {
result.sig = NMEA_SIG_MID;
} else if (!strncasecmp(POSFILE_VALUE_SIG_HIGH, value, sizeof(line))) {
result.sig = NMEA_SIG_HIGH;
} else {
pudError(false, "Position file \"%s\", line %d uses an invalid value for \"%s\","
" valid values are [%s|%s\%s|%s]", fileName, lineNumber, POSFILE_NAME_SIG,
POSFILE_VALUE_SIG_BAD, POSFILE_VALUE_SIG_LOW, POSFILE_VALUE_SIG_MID, POSFILE_VALUE_SIG_HIGH);
goto out;
}
nmea_INFO_set_present(&result.present, SIG);
} else if (!strncasecmp(POSFILE_NAME_FIX, name, sizeof(line))) {
if (!strncasecmp(POSFILE_VALUE_FIX_BAD, value, sizeof(line))) {
result.fix = NMEA_FIX_BAD;
} else if (!strncasecmp(POSFILE_VALUE_FIX_2D, value, sizeof(line))) {
result.fix = NMEA_FIX_2D;
} else if (!strncasecmp(POSFILE_VALUE_FIX_3D, value, sizeof(line))) {
result.fix = NMEA_FIX_3D;
} else {
pudError(false, "Position file \"%s\", line %d uses an invalid value for \"%s\","
" valid values are [%s\%s|%s]", fileName, lineNumber, POSFILE_NAME_FIX, POSFILE_VALUE_FIX_BAD,
POSFILE_VALUE_FIX_2D, POSFILE_VALUE_FIX_3D);
goto out;
}
nmea_INFO_set_present(&result.present, FIX);
} else if (!strncasecmp(POSFILE_NAME_HDOP, name, sizeof(line))) {
