void System::Publish::position(void *arg) {
gpsCustom _mode(_gps, "GPGSA", 1);
gpsCustom _fixtype(_gps, "GPGSA", 2);
gpsCustom _pdop(_gps, "GPGSA", 15);
gpsCustom _vdop(_gps, "GPGSA", 17);
gpsCustom _nummessages(_gps, "GPGSV", 1);
gpsCustom _messagenum(_gps, "GPGSV", 2);
gpsCustom _numsatsinview(_gps, "GPGSV", 3);
gpsCustom _prn(_gps, "GPGSV", 4);
gpsCustom _elevation(_gps, "GPGSV", 5);
gpsCustom _azimuth(_gps, "GPGSV", 6);
gpsCustom _snr(_gps, "GPGSV", 7);
gpsCustom _cog(_gps, "GPRMC", 8);
gpsCustom _timestamp(_gps, "GPRMC", 1);
for (;; ) {
unsigned long start = millis();
do
{
while (Serial2.available())
_gps.encode(Serial2.read());
} while (millis() - start < 100);
positionBox.latitude = _gps.location.lat();
positionBox.longitude = _gps.location.lng();
positionBox.cog = _cog.value();
gpsInfoBox.mode = _mode.value();
gpsInfoBox.fixtype = _fixtype.value();
gpsInfoBox.pdop = _pdop.value();
gpsInfoBox.hdop = _gps.hdop.value();
gpsInfoBox.vdop = _vdop.value();
gpsInfoBox.numbers_of_gpgsv_messages = _nummessages.value();
gpsInfoBox.index_number_of_gpgsv_message = _messagenum.value();
gpsInfoBox.numbers_of_sats_inview = _numsatsinview.value();
gpsInfoBox.prn = _prn.value();
gpsInfoBox.snr = _snr.value();
gpsInfoBox.elevation = _elevation.value();
gpsInfoBox.azimuth = _azimuth.value();
gpsInfoBox.timestamp = _timestamp.value();
gpsInfoBox.number_of_satellites = _gps.satellites.value();
}
}
// GNGGA
void Ublox::read_gga()
{
int counter = 0;
char token[20];
Tokeniser tok(buf, ',');
while(tok.next(token, 20))
{
switch(counter)
{
case 1: //time
{
float time = atof(token);
int hms = int(time);
datetime.millis = time - hms;
datetime.seconds = fmod(hms, 100);
hms /= 100;
datetime.minutes = fmod(hms, 100);
hms /= 100;
datetime.hours = hms;
time_age = millis();
}
break;
case 2: //latitude
{
float llat = atof(token);
int ilat = llat/100;
double mins = fmod(llat, 100);
latitude = ilat + (mins/60);
}
break;
case 3: //north/south
{
if(token[0] == 'S')
latitude = -latitude;
}
break;
case 4: //longitude
{
float llong = atof(token);
int ilat = llong/100;
double mins = fmod(llong, 100);
longitude = ilat + (mins/60);
}
break;
case 5: //east/west
{
if(token[0] == 'W')
longitude = -longitude;
latlng_age = millis();
}
break;
case 6:
{
fixtype = _fixtype(atoi(token));
}
break;
case 7:
{
sats_in_use = atoi(token);
}
break;
case 8:
{
hdop = atoi(token);
}
break;
case 9:
{
float new_alt = atof(token);
vert_speed = (new_alt - altitude)/((millis()-alt_age)/1000.0);
altitude = atof(token);
alt_age = millis();
}
break;
}
counter++;
}
}
