static int
nmea_reader_update_date( NmeaReader* r, Token date, Token time )
{
Token tok = date;
int day, mon, year;
if (tok.p + 6 != tok.end) {
D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
return -1;
}
day = str2int(tok.p, tok.p+2);
mon = str2int(tok.p+2, tok.p+4);
year = str2int(tok.p+4, tok.p+6) + 2000;
if ((day|mon|year) < 0) {
D("date not properly formatted: '%.*s'", tok.end-tok.p, tok.p);
return -1;
}
r->utc_year = year;
r->utc_mon = mon;
r->utc_day = day;
return nmea_reader_update_time( r, time );
}
static int
nmea_reader_update_date( NmeaReader* r, Token date, Token mtime )
{
Token tok = date;
int day, mon, year;
if (tok.p + 6 != tok.end) {
/* no date info, will use host time in _update_time function
*/
}
/* normal case */
day = str2int(tok.p, tok.p+2);
mon = str2int(tok.p+2, tok.p+4);
year = str2int(tok.p+4, tok.p+6) + 2000;
if ((day|mon|year) < 0) {
return -1;
}
r->utc_year = year;
r->utc_mon = mon;
r->utc_day = day;
return nmea_reader_update_time( r, mtime );
}
static void
nmea_reader_parse( NmeaReader* r )
{
/* we received a complete sentence, now parse it to generate
* a new GPS fix...
*/
NmeaTokenizer tzer[1];
Token tok;
D("Received: '%.*s'", r->pos, r->in);
if (r->pos < 9) {
D("Too short. discarded.");
return;
}
nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
#if GPS_DEBUG
{
int n;
D("Found %d tokens", tzer->count);
for (n = 0; n < tzer->count; n++) {
Token tok = nmea_tokenizer_get(tzer,n);
D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);
}
}
#endif
tok = nmea_tokenizer_get(tzer, 0);
if (tok.p + 5 > tok.end) {
D("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p);
return;
}
// ignore first two characters.
tok.p += 2;
if ( !memcmp(tok.p, "GGA", 3) ) {
// GPS fix
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_latitude = nmea_tokenizer_get(tzer,2);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3);
Token tok_longitude = nmea_tokenizer_get(tzer,4);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
Token tok_altitude = nmea_tokenizer_get(tzer,9);
Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10);
nmea_reader_update_time(r, tok_time);
nmea_reader_update_latlong(r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0]);
nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
} else if ( !memcmp(tok.p, "GSA", 3) ) {
// do something ?
} else if ( !memcmp(tok.p, "RMC", 3) ) {
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_fixStatus = nmea_tokenizer_get(tzer,2);
Token tok_latitude = nmea_tokenizer_get(tzer,3);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4);
Token tok_longitude = nmea_tokenizer_get(tzer,5);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
Token tok_speed = nmea_tokenizer_get(tzer,7);
Token tok_bearing = nmea_tokenizer_get(tzer,8);
Token tok_date = nmea_tokenizer_get(tzer,9);
D("in RMC, fixStatus=%c", tok_fixStatus.p[0]);
if (tok_fixStatus.p[0] == 'A')
{
nmea_reader_update_date( r, tok_date, tok_time );
nmea_reader_update_latlong( r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0] );
nmea_reader_update_bearing( r, tok_bearing );
nmea_reader_update_speed ( r, tok_speed );
}
} else {
tok.p -= 2;
D("unknown sentence '%.*s", tok.end-tok.p, tok.p);
}
if (r->fix.flags != 0) {
#if GPS_DEBUG
char temp[256];
char* p = temp;
char* end = p + sizeof(temp);
struct tm utc;
p += snprintf( p, end-p, "sending fix" );
if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude);
}
if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) {
p += snprintf(p, end-p, " altitude=%g", r->fix.altitude);
}
if (r->fix.flags & GPS_LOCATION_HAS_SPEED) {
p += snprintf(p, end-p, " speed=%g", r->fix.speed);
}
if (r->fix.flags & GPS_LOCATION_HAS_BEARING) {
p += snprintf(p, end-p, " bearing=%g", r->fix.bearing);
}
if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy);
}
gmtime_r( (time_t*) &r->fix.timestamp, &utc );
p += snprintf(p, end-p, " time=%s", asctime( &utc ) );
//D(temp);
#endif
if (r->callback) {
r->callback( &r->fix );
r->fix.flags = 0;
}
else {
D("no callback, keeping data until needed !");
}
}
}
static void
nmea_reader_parse( NmeaReader* r )
{
/* we received a complete sentence, now parse it to generate
* a new GPS fix...
*/
NmeaTokenizer tzer[1];
Token tok;
struct timeval tv;
D("Received: '%.*s'", r->pos, r->in);
if (r->pos < 9) {
D("Too short. discarded.");
return;
}
gettimeofday(&tv, NULL);
if (_gps_state->init)
_gps_state->callbacks->nmea_cb(tv.tv_sec*1000+tv.tv_usec/1000, r->in, r->pos);
nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
#if GPS_DEBUG
{
int n;
D("Found %d tokens", tzer->count);
for (n = 0; n < tzer->count; n++) {
Token tok = nmea_tokenizer_get(tzer,n);
D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);
}
}
#endif
tok = nmea_tokenizer_get(tzer, 0);
if (tok.p + 5 > tok.end) {
D("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p);
return;
}
/*
$GPRMC,084537.345,V,,,,,,,270811,,,N*4F
$GPGGA,084538.353,,,,,0,00,,,M,0.0,M,,0000*51
$GPGSA,A,1,,,,,,,,,,,,,,,*1E
$GPGSV,3,1,11,09,80,303,,12,41,229,,15,40,178,,17,39,070,*70
$GPGSV,3,2,11,22,20,295,,18,18,256,,14,13,317,,26,10,154,*72
$GPGSV,3,3,11,25,08,234,,28,08,056,,24,04,011,*40
*/
// ignore first two characters.
tok.p += 2;
if ( !memcmp(tok.p, "GGA", 3) ) {
// GPS fix
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_latitude = nmea_tokenizer_get(tzer,2);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3);
Token tok_longitude = nmea_tokenizer_get(tzer,4);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
Token tok_altitude = nmea_tokenizer_get(tzer,9);
Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10);
nmea_reader_update_time(r, tok_time);
nmea_reader_update_latlong(r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0]);
nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
} else if ( !memcmp(tok.p, "GSA", 3) ) {
//Satellites are handled by RPC-side code.
/*
1 = Mode:
M=Manual, forced to operate in 2D or 3D
A=Automatic, 3D/2D
2 = Mode:
1=Fix not available
2=2D
3=3D
3-14 = IDs of SVs used in position fix (null for unused fields)
15 = PDOP
16 = HDOP
17 = VDOP
*/
Token tok_mode = nmea_tokenizer_get(tzer,1);
Token tok_fix = nmea_tokenizer_get(tzer,2);
Token tok_id = nmea_tokenizer_get(tzer,3);
Token tok_pdop = nmea_tokenizer_get(tzer,15);
Token tok_hdop = nmea_tokenizer_get(tzer,16);
Token tok_vdop = nmea_tokenizer_get(tzer,17);
int i;
for ( i=0; i<12; i++ ) {
Token tok_id = nmea_tokenizer_get(tzer,3+i);
if ( tok_id.end > tok_id.p )
id_in_fixed[i]=str2int(tok_id.end, tok_id.p);
}
} else if ( !memcmp(tok.p, "GSV", 3) ) {
//Satellites are handled by RPC-side code.
Token tok_num_messages = nmea_tokenizer_get(tzer,1);
Token tok_msg_number = nmea_tokenizer_get(tzer,2);
Token tok_svs_inview = nmea_tokenizer_get(tzer,3);
Token tok_sv1_prn_num = nmea_tokenizer_get(tzer,4);
Token tok_sv1_elevation = nmea_tokenizer_get(tzer,5);
Token tok_sv1_azimuth = nmea_tokenizer_get(tzer,6);
Token tok_sv1_snr = nmea_tokenizer_get(tzer,7);
Token tok_sv2_prn_num = nmea_tokenizer_get(tzer,8);
Token tok_sv2_elevation = nmea_tokenizer_get(tzer,9);
Token tok_sv2_azimuth = nmea_tokenizer_get(tzer,10);
Token tok_sv2_snr = nmea_tokenizer_get(tzer,11);
Token tok_sv3_prn_num = nmea_tokenizer_get(tzer,12);
Token tok_sv3_elevation = nmea_tokenizer_get(tzer,13);
Token tok_sv3_azimuth = nmea_tokenizer_get(tzer,14);
Token tok_sv3_snr = nmea_tokenizer_get(tzer,15);
Token tok_sv4_prn_num = nmea_tokenizer_get(tzer,16);
Token tok_sv4_elevation = nmea_tokenizer_get(tzer,17);
Token tok_sv4_azimuth = nmea_tokenizer_get(tzer,18);
Token tok_sv4_snr = nmea_tokenizer_get(tzer,19);
int num_messages = str2int(tok_num_messages.p,tok_num_messages.end);
int msg_number = str2int(tok_msg_number.p,tok_msg_number.end);
int svs_inview = str2int(tok_svs_inview.p,tok_svs_inview.end);
D("GSV %d %d %d", num_messages, msg_number, svs_inview );
if (msg_number==1)
r->sv_status.used_in_fix_mask=0;
nmea_reader_update_svs( r, svs_inview, msg_number, 0, tok_sv1_prn_num, tok_sv1_elevation, tok_sv1_azimuth, tok_sv1_snr );
nmea_reader_update_svs( r, svs_inview, msg_number, 1, tok_sv2_prn_num, tok_sv2_elevation, tok_sv2_azimuth, tok_sv2_snr );
nmea_reader_update_svs( r, svs_inview, msg_number, 2, tok_sv3_prn_num, tok_sv3_elevation, tok_sv3_azimuth, tok_sv3_snr );
nmea_reader_update_svs( r, svs_inview, msg_number, 3, tok_sv4_prn_num, tok_sv4_elevation, tok_sv4_azimuth, tok_sv4_snr );
r->sv_status.num_svs=svs_inview;
if (num_messages==msg_number)
update_gps_svstatus(&r->sv_status);
/*
1 = Total number of messages of this type in this cycle
2 = Message number
3 = Total number of SVs in view
4 = SV PRN number
5 = Elevation in degrees, 90 maximum
6 = Azimuth, degrees from true north, 000 to 359
7 = SNR, 00-99 dB (null when not tracking)
8-11 = Information about second SV, same as field 4-7
12-15= Information about third SV, same as field 4-7
16-19= Information about fourth SV, same as field 4-7
*/
/*
int i;
_gps_state.sv_status.num_svs=ntohl(data[82]) & 0x1F;
for(i=0;i<ret.num_svs;++i) {
_gps_state.sv_status.sv_list[i].prn=ntohl(data[83+3*i]);
_gps_state.sv_status.sv_list[i].elevation=ntohl(data[83+3*i+1]);
_gps_state.sv_status.sv_list[i].azimuth=ntohl(data[83+3*i+2])/100;
_gps_state.sv_status.sv_list[i].snr=ntohl(data[83+3*i+2])%100;
}
_gps_state.sv_status.used_in_fix_mask=ntohl(data[77]);
update_gps_svstatus(&_gps_state.
*/
} else if ( !memcmp(tok.p, "RMC", 3) ) {
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_fixStatus = nmea_tokenizer_get(tzer,2);
Token tok_latitude = nmea_tokenizer_get(tzer,3);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4);
Token tok_longitude = nmea_tokenizer_get(tzer,5);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
Token tok_speed = nmea_tokenizer_get(tzer,7);
Token tok_bearing = nmea_tokenizer_get(tzer,8);
Token tok_date = nmea_tokenizer_get(tzer,9);
D("in RMC, fixStatus=%c", tok_fixStatus.p[0]);
if (tok_fixStatus.p[0] == 'A')
{
nmea_reader_update_date( r, tok_date, tok_time );
nmea_reader_update_latlong( r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0] );
nmea_reader_update_bearing( r, tok_bearing );
nmea_reader_update_speed ( r, tok_speed );
}
} else {
tok.p -= 2;
D("unknown sentence '%.*s", tok.end-tok.p, tok.p);
}
if (r->fix.flags != 0) {
#if GPS_DEBUG
char temp[256];
char* p = temp;
char* end = p + sizeof(temp);
struct tm utc;
p += snprintf( p, end-p, "sending fix" );
if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude);
}
if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) {
p += snprintf(p, end-p, " altitude=%g", r->fix.altitude);
}
if (r->fix.flags & GPS_LOCATION_HAS_SPEED) {
p += snprintf(p, end-p, " speed=%g", r->fix.speed);
}
if (r->fix.flags & GPS_LOCATION_HAS_BEARING) {
p += snprintf(p, end-p, " bearing=%g", r->fix.bearing);
}
if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy);
}
gmtime_r( (time_t*) &r->fix.timestamp, &utc );
p += snprintf(p, end-p, " time=%s", asctime( &utc ) );
#endif
if (_gps_state->callbacks->location_cb) {
_gps_state->callbacks->location_cb( &r->fix );
r->fix.flags = 0;
} else {
D("no callback, keeping data until needed !");
}
/*
if (r->callback) {
r->callback( &r->fix );
r->fix.flags = 0;
}
else {
D("no callback, keeping data until needed !");
}
*/
}
}
static void
nmea_reader_parse( NmeaReader* r )
{
/* we received a complete sentence, now parse it to generate
* a new GPS fix...
*/
NmeaTokenizer tzer[1];
Token tok;
if (r->pos < 9) {
return;
}
if (gps_state->callbacks.nmea_cb) {
struct timeval tv;
unsigned long long mytimems;
gettimeofday(&tv,NULL);
mytimems = tv.tv_sec * 1000 + tv.tv_usec / 1000;
gps_state->callbacks.nmea_cb(mytimems, r->in, r->pos);
}
nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
#ifdef GPS_DEBUG_TOKEN
{
int n;
D("Found %d tokens", tzer->count);
for (n = 0; n < tzer->count; n++) {
Token tok = nmea_tokenizer_get(tzer,n);
D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);
}
}
#endif
tok = nmea_tokenizer_get(tzer, 0);
if (tok.p + 5 > tok.end) {
/* for $PUNV sentences */
if ( !memcmp(tok.p, "PUNV", 4) ) {
Token tok_cfg = nmea_tokenizer_get(tzer,1);
if (!memcmp(tok_cfg.p, "CFG_R", 5)) {
} else if ( !memcmp(tok_cfg.p, "QUAL", 4) ) {
Token tok_sigma_x = nmea_tokenizer_get(tzer, 3);
if (tok_sigma_x.p[0] != ',') {
Token tok_accuracy = nmea_tokenizer_get(tzer, 10);
nmea_reader_update_accuracy(r, tok_accuracy);
}
} else if (!memcmp(tok_cfg.p, "TIMEMAP", 7))
{
Token systime = nmea_tokenizer_get(tzer, 8); // system time token
Token timestamp = nmea_tokenizer_get(tzer, 2); // UTC time token
nmea_reader_update_timemap(r, systime, timestamp);
}
}else{
}
return;
}
if ( !memcmp(tok.p, "GPG", 3) ) //GPGSA,GPGGA,GPGSV
bGetFormalNMEA = 1;
// ignore first two characters.
tok.p += 2;
if ( !memcmp(tok.p, "GGA", 3) ) {
// GPS fix
Token tok_fixstaus = nmea_tokenizer_get(tzer,6);
if (tok_fixstaus.p[0] > '0') {
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_latitude = nmea_tokenizer_get(tzer,2);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3);
Token tok_longitude = nmea_tokenizer_get(tzer,4);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
Token tok_altitude = nmea_tokenizer_get(tzer,9);
Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10);
nmea_reader_update_time(r, tok_time);
nmea_reader_update_latlong(r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0]);
nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
}
} else if ( !memcmp(tok.p, "GLL", 3) ) {
Token tok_fixstaus = nmea_tokenizer_get(tzer,6);
if (tok_fixstaus.p[0] == 'A') {
Token tok_latitude = nmea_tokenizer_get(tzer,1);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,2);
Token tok_longitude = nmea_tokenizer_get(tzer,3);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,4);
Token tok_time = nmea_tokenizer_get(tzer,5);
nmea_reader_update_time(r, tok_time);
nmea_reader_update_latlong(r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0]);
}
} else if ( !memcmp(tok.p, "GSA", 3) ) {
Token tok_fixStatus = nmea_tokenizer_get(tzer, 2);
int i;
if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != '1') {
r->sv_status.used_in_fix_mask = 0ul;
for (i = 3; i <= 14; ++i){
Token tok_prn = nmea_tokenizer_get(tzer, i);
int prn = str2int(tok_prn.p, tok_prn.end);
/* only available for PRN 1-32 */
if ((prn > 0) && (prn < 33)){
r->sv_status.used_in_fix_mask |= (1ul << (prn-1));
r->sv_status_changed = 1;
/* mark this parameter to identify the GSA is in fixed state */
r->gsa_fixed = 1;
}
}
}else {
if (r->gsa_fixed == 1) {
r->sv_status.used_in_fix_mask = 0ul;
r->sv_status_changed = 1;
r->gsa_fixed = 0;
}
}
} else if ( !memcmp(tok.p, "GSV", 3) ) {
Token tok_noSatellites = nmea_tokenizer_get(tzer, 3);
int noSatellites = str2int(tok_noSatellites.p, tok_noSatellites.end);
if (noSatellites > 0) {
Token tok_noSentences = nmea_tokenizer_get(tzer, 1);
Token tok_sentence = nmea_tokenizer_get(tzer, 2);
int sentence = str2int(tok_sentence.p, tok_sentence.end);
int totalSentences = str2int(tok_noSentences.p, tok_noSentences.end);
int curr;
int i;
if (sentence == 1) {
r->sv_status_changed = 0;
r->sv_status.num_svs = 0;
}
curr = r->sv_status.num_svs;
i = 0;
while (i < 4 && r->sv_status.num_svs < noSatellites){
Token tok_prn = nmea_tokenizer_get(tzer, i * 4 + 4);
Token tok_elevation = nmea_tokenizer_get(tzer, i * 4 + 5);
Token tok_azimuth = nmea_tokenizer_get(tzer, i * 4 + 6);
Token tok_snr = nmea_tokenizer_get(tzer, i * 4 + 7);
r->sv_status.sv_list[curr].prn = str2int(tok_prn.p, tok_prn.end);
r->sv_status.sv_list[curr].elevation = str2float(tok_elevation.p, tok_elevation.end);
r->sv_status.sv_list[curr].azimuth = str2float(tok_azimuth.p, tok_azimuth.end);
r->sv_status.sv_list[curr].snr = str2float(tok_snr.p, tok_snr.end);
r->sv_status.num_svs += 1;
curr += 1;
i += 1;
}
if (sentence == totalSentences) {
r->sv_status_changed = 1;
}
}
} else if ( !memcmp(tok.p, "RMC", 3) ) {
Token tok_fixStatus = nmea_tokenizer_get(tzer,2);
if (tok_fixStatus.p[0] == 'A')
{
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_latitude = nmea_tokenizer_get(tzer,3);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4);
Token tok_longitude = nmea_tokenizer_get(tzer,5);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
Token tok_speed = nmea_tokenizer_get(tzer,7);
Token tok_bearing = nmea_tokenizer_get(tzer,8);
Token tok_date = nmea_tokenizer_get(tzer,9);
nmea_reader_update_date( r, tok_date, tok_time );
nmea_reader_update_latlong( r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0] );
nmea_reader_update_bearing( r, tok_bearing );
nmea_reader_update_speed ( r, tok_speed );
}
} else if ( !memcmp(tok.p, "VTG", 3) ) {
Token tok_fixStatus = nmea_tokenizer_get(tzer,9);
if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != 'N')
{
Token tok_bearing = nmea_tokenizer_get(tzer,1);
Token tok_speed = nmea_tokenizer_get(tzer,5);
nmea_reader_update_bearing( r, tok_bearing );
nmea_reader_update_speed ( r, tok_speed );
}
} else if ( !memcmp(tok.p, "ZDA", 3) ) {
Token tok_time;
Token tok_year = nmea_tokenizer_get(tzer,4);
if (tok_year.p[0] != '\0') {
Token tok_day = nmea_tokenizer_get(tzer,2);
Token tok_mon = nmea_tokenizer_get(tzer,3);
nmea_reader_update_cdate( r, tok_day, tok_mon, tok_year );
}
tok_time = nmea_tokenizer_get(tzer,1);
if (tok_time.p[0] != '\0') {
nmea_reader_update_time(r, tok_time);
}
} else {
tok.p -= 2;
}
if (!gps_state->first_fix &&
gps_state->init == STATE_INIT &&
r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
ath_send_ni_notification(r);
if (gps_state->callbacks.location_cb) {
gps_state->callbacks.location_cb( &r->fix );
r->fix.flags = 0;
}
gps_state->first_fix = 1;
}
}
static void
nmea_reader_parse( NmeaReader* r )
{
/* we received a complete sentence, now parse it to generate
* a new GPS fix...
*/
NmeaTokenizer tzer[1];
Token tok;
D("Received: '%.*s'", r->pos, r->in);
if (r->pos < 9) {
D("Too short. discarded.");
return;
}
nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
#if GPS_DEBUG
{
int n;
D("Found %d tokens", tzer->count);
for (n = 0; n < tzer->count; n++) {
Token tok = nmea_tokenizer_get(tzer,n);
D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);
}
}
#endif
tok = nmea_tokenizer_get(tzer, 0);
if (tok.p + 5 > tok.end) {
D("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p);
return;
}
// ignore first two characters.
tok.p += 2;
if ( !memcmp(tok.p, "GGA", 3) ) {
// GPS fix
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_latitude = nmea_tokenizer_get(tzer,2);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3);
Token tok_longitude = nmea_tokenizer_get(tzer,4);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
Token tok_altitude = nmea_tokenizer_get(tzer,9);
Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10);
nmea_reader_update_time(r, tok_time);
nmea_reader_update_latlong(r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0]);
nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
} else if ( !memcmp(tok.p, "GSA", 3) ) {
int i;
Token tok_fixStatus = nmea_tokenizer_get(tzer, 2);
if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != '1') {
Token tok_accuracy = nmea_tokenizer_get(tzer, 15);
nmea_reader_update_accuracy(r, tok_accuracy);
}
for(i = 3; i <= 14; i++){
Token tok_prn = nmea_tokenizer_get(tzer, i);
if(tok_prn.p >= tok_prn.end){
r->status.used_in_fix_mask &= ~(1 << (i-3));
}else{
r->status.used_in_fix_mask |= 1 << (i-3);
}
}
} else if ( !memcmp(tok.p, "GSV", 3) ) {
Token tok_numgsv = nmea_tokenizer_get(tzer,1);
Token tok_indexgsv = nmea_tokenizer_get(tzer,2);
Token tok_numsvs = nmea_tokenizer_get(tzer,3);
int numgsv = str2int(tok_numgsv.p, tok_numgsv.end);
int indexgsv = str2int(tok_indexgsv.p, tok_indexgsv.end);
int numsvs = str2int(tok_numsvs.p, tok_numsvs.end);
if((indexgsv <= numgsv) && (indexgsv >= 1)) //________
if (numsvs > 0){
int i, j = 4;
for(i = (indexgsv-1) << 2; i < indexgsv << 2; i++){
Token tok_prn = nmea_tokenizer_get(tzer,j++);
Token tok_elevation = nmea_tokenizer_get(tzer,j++);
Token tok_azimuth = nmea_tokenizer_get(tzer,j++);
Token tok_snr = nmea_tokenizer_get(tzer,j++);
nmea_reader_update_sv_list(&r->status.sv_list[i], tok_prn, tok_elevation, tok_azimuth, tok_snr);
}
if(indexgsv == numgsv){
r->status.num_svs = numsvs;
if(r->sv_status_cb)
r->sv_status_cb(&r->status);
}
}
} else if ( !memcmp(tok.p, "RMC", 3) ) {
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_fixStatus = nmea_tokenizer_get(tzer,2);
Token tok_latitude = nmea_tokenizer_get(tzer,3);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4);
Token tok_longitude = nmea_tokenizer_get(tzer,5);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
Token tok_speed = nmea_tokenizer_get(tzer,7);
Token tok_bearing = nmea_tokenizer_get(tzer,8);
Token tok_date = nmea_tokenizer_get(tzer,9);
D("in RMC, fixStatus=%c", tok_fixStatus.p[0]);
if (tok_fixStatus.p[0] == 'A')
{
nmea_reader_update_date( r, tok_date, tok_time );
nmea_reader_update_latlong( r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0] );
nmea_reader_update_bearing( r, tok_bearing );
nmea_reader_update_speed ( r, tok_speed );
}
} else {
tok.p -= 2;
D("unknown sentence '%.*s", tok.end-tok.p, tok.p);
}
if ((r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) &&
(r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) &&
(r->fix.flags & (GPS_LOCATION_HAS_SPEED | GPS_LOCATION_HAS_BEARING))) {
#if GPS_DEBUG
char temp[256];
char* p = temp;
char* end = p + sizeof(temp);
struct tm utc;
p += snprintf( p, end-p, "sending fix" );
if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
p += snprintf(p, end-p, " time=%llu", r->fix.timestamp);
p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude);
}
if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) {
p += snprintf(p, end-p, " altitude=%g", r->fix.altitude);
}
if (r->fix.flags & GPS_LOCATION_HAS_SPEED) {
p += snprintf(p, end-p, " speed=%g", r->fix.speed);
}
if (r->fix.flags & GPS_LOCATION_HAS_BEARING) {
p += snprintf(p, end-p, " bearing=%g", r->fix.bearing);
}
if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy);
}
D("%s",temp);
#endif
if (r->location_cb) {
r->location_cb( &r->fix );
r->fix.flags = 0;
}
else {
D("no location_cb, keeping data until needed !");
}
}
}
static void
nmea_reader_parse( NmeaReader* r )
{
/* we received a complete sentence, now parse it to generate
* a new GPS fix...
*/
NmeaTokenizer tzer[1];
Token tok;
D("Received: '%.*s'", r->pos, r->in);
if (r->pos < 9) {
D("Too short. discarded.");
return;
}
nmea_tokenizer_init(tzer, r->in, r->in + r->pos);
#if GPS_DEBUG
{
int n;
D("Found %d tokens", tzer->count);
for (n = 0; n < tzer->count; n++) {
Token tok = nmea_tokenizer_get(tzer,n);
D("%2d: '%.*s'", n, tok.end-tok.p, tok.p);
}
}
#endif
tok = nmea_tokenizer_get(tzer, 0);
if (tok.p + 5 > tok.end) {
D("sentence id '%.*s' too short, ignored.", tok.end-tok.p, tok.p);
return;
}
// ignore first two characters.
tok.p += 2;
if ( !memcmp(tok.p, "GGA", 3) ) {
// GPS fix
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_latitude = nmea_tokenizer_get(tzer,2);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,3);
Token tok_longitude = nmea_tokenizer_get(tzer,4);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,5);
Token tok_altitude = nmea_tokenizer_get(tzer,9);
Token tok_altitudeUnits = nmea_tokenizer_get(tzer,10);
nmea_reader_update_time(r, tok_time);
nmea_reader_update_latlong(r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0]);
nmea_reader_update_altitude(r, tok_altitude, tok_altitudeUnits);
} else if ( !memcmp(tok.p, "GSA", 3) ) {
#if GPS_SV_INCLUDE
Token tok_fixStatus = nmea_tokenizer_get(tzer, 2);
int i;
if (tok_fixStatus.p[0] != '\0' && tok_fixStatus.p[0] != '1') {
Token tok_accuracy = nmea_tokenizer_get(tzer, 15);
nmea_reader_update_accuracy(r, tok_accuracy);
r->sv_status.used_in_fix_mask = 0ul;
for (i = 3; i <= 14; ++i){
Token tok_prn = nmea_tokenizer_get(tzer, i);
int prn = str2int(tok_prn.p, tok_prn.end);
D("%s: prn is %d", __FUNCTION__, r->sv_status.used_in_fix_mask);
if (prn > 0){
//r->sv_status.used_in_fix_mask |= (1ul << (32 - prn));
r->sv_status.used_in_fix_mask |= (1ul << (prn-1));
r->sv_status_changed = 1;
D("%s: fix mask is 0x%08X", __FUNCTION__, r->sv_status.used_in_fix_mask);
}
}
}
#endif
// do something ?
} else if ( !memcmp(tok.p, "RMC", 3) ) {
Token tok_time = nmea_tokenizer_get(tzer,1);
Token tok_fixStatus = nmea_tokenizer_get(tzer,2);
Token tok_latitude = nmea_tokenizer_get(tzer,3);
Token tok_latitudeHemi = nmea_tokenizer_get(tzer,4);
Token tok_longitude = nmea_tokenizer_get(tzer,5);
Token tok_longitudeHemi = nmea_tokenizer_get(tzer,6);
Token tok_speed = nmea_tokenizer_get(tzer,7);
Token tok_bearing = nmea_tokenizer_get(tzer,8);
Token tok_date = nmea_tokenizer_get(tzer,9);
D("in RMC, fixStatus=%c", tok_fixStatus.p[0]);
if (tok_fixStatus.p[0] == 'A')
{
nmea_reader_update_date( r, tok_date, tok_time );
nmea_reader_update_latlong( r, tok_latitude,
tok_latitudeHemi.p[0],
tok_longitude,
tok_longitudeHemi.p[0] );
nmea_reader_update_bearing( r, tok_bearing );
nmea_reader_update_speed ( r, tok_speed );
}
} else if ( !memcmp(tok.p, "GSV", 3) ) {
#if GPS_SV_INCLUDE
Token tok_noSatellites = nmea_tokenizer_get(tzer, 3);
int noSatellites = str2int(tok_noSatellites.p, tok_noSatellites.end);
if (noSatellites > 0) {
Token tok_noSentences = nmea_tokenizer_get(tzer, 1);
Token tok_sentence = nmea_tokenizer_get(tzer, 2);
int sentence = str2int(tok_sentence.p, tok_sentence.end);
int totalSentences = str2int(tok_noSentences.p, tok_noSentences.end);
int curr;
int i;
if (sentence == 1) {
r->sv_status_changed = 0;
r->sv_status.num_svs = 0;
}
curr = r->sv_status.num_svs;
i = 0;
while (i < 4 && r->sv_status.num_svs < noSatellites){
Token tok_prn = nmea_tokenizer_get(tzer, i * 4 + 4);
Token tok_elevation = nmea_tokenizer_get(tzer, i * 4 + 5);
Token tok_azimuth = nmea_tokenizer_get(tzer, i * 4 + 6);
Token tok_snr = nmea_tokenizer_get(tzer, i * 4 + 7);
r->sv_status.sv_list[curr].prn = str2int(tok_prn.p, tok_prn.end);
r->sv_status.sv_list[curr].elevation = str2float(tok_elevation.p, tok_elevation.end);
r->sv_status.sv_list[curr].azimuth = str2float(tok_azimuth.p, tok_azimuth.end);
r->sv_status.sv_list[curr].snr = str2float(tok_snr.p, tok_snr.end);
r->sv_status.num_svs += 1;
curr += 1;
i += 1;
}
if (sentence == totalSentences) {
r->sv_status_changed = 1;
}
D("%s: GSV message with total satellites %d", __FUNCTION__, noSatellites);
}else if (noSatellites == 0) {
D("%s: GSV message, There are no satellites %d", __FUNCTION__, noSatellites);
//[SMIT] wwwen: While we can't get Satellites, we report a simular Satellites(10) to test UART
r->sv_status_changed = 1;
r->sv_status.num_svs = 0;
r->sv_status.sv_list[0].prn = 10;
r->sv_status.sv_list[0].elevation = 0;
r->sv_status.sv_list[0].azimuth = 0;
r->sv_status.sv_list[0].snr = -1;
r->sv_status.num_svs += 1;
}
#endif
} else {
tok.p -= 2;
D("unknown sentence '%.*s", tok.end-tok.p, tok.p);
}
if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
r->fix.flags |=GPS_LOCATION_HAS_SPEED;
r->fix.flags |=GPS_LOCATION_HAS_ACCURACY;
r->fix.flags |=GPS_LOCATION_HAS_BEARING;
r->fix.flags |=GPS_LOCATION_HAS_ALTITUDE;
#if GPS_DEBUG
char temp[256];
char* p = temp;
char* end = p + sizeof(temp);
struct tm utc;
p += snprintf( p, end-p, "sending fix" );
if (r->fix.flags & GPS_LOCATION_HAS_LAT_LONG) {
p += snprintf(p, end-p, " lat=%g lon=%g", r->fix.latitude, r->fix.longitude);
}
if (r->fix.flags & GPS_LOCATION_HAS_ALTITUDE) {
p += snprintf(p, end-p, " altitude=%g", r->fix.altitude);
}
if (r->fix.flags & GPS_LOCATION_HAS_SPEED) {
p += snprintf(p, end-p, " speed=%g", r->fix.speed);
}
if (r->fix.flags & GPS_LOCATION_HAS_BEARING) {
p += snprintf(p, end-p, " bearing=%g", r->fix.bearing);
}
if (r->fix.flags & GPS_LOCATION_HAS_ACCURACY) {
p += snprintf(p,end-p, " accuracy=%g", r->fix.accuracy);
}
gmtime_r( (time_t*) &r->fix.timestamp, &utc );
p += snprintf(p, end-p, " time=%s", asctime( &utc ) );
D("%s", temp);
#endif
if (r->callback) {
r->callback( &r->fix );
r->fix.flags = 0;
}
else {
D("no callback, keeping data until needed !");
}
#if GPS_SV_INCLUDE
if ((r->sv_status_changed == 1) && (r->sv_callback)){
r->sv_callback(&r->sv_status);
r->sv_status_changed = 0;
}
else{
D("no sv callback, keeping data until needed !");
}
#endif
}
}