int main()
{
char bufa[BUFSZ] = {'\0'},
bufb[BUFSZ];
size_t lena = 0,
mem = BUFSZ;
nmeamsg_t nm;
nmeabuf_t nb;
nmea_ctor(&nb, bufb, mem);
nmea_concat(&nb, ",2*23\n$CAREV,INIT,0.1.2.3.4*45\n$CA", 34);
nmea_scan(&nb, &nm);
nmea_debug(stderr, &nb);
lena = nmea_parse(bufa, mem, &nm);
if (0 == lena) return -1;
if (0 != strncmp(bufa, "$CAREV,INIT,0.1.2.3.4*45\n", lena)) return -1;
bufa[lena-1]='.';
printf("len=%ld msg=\"%s\"\n", lena, bufa);
nmea_scan(&nb, &nm);
nmea_debug(stderr, &nb);
return 0;
}
int
main(void)
{
// Sentence string to be parsed
char sentence[] = "$GPGLL,4916.45,N,12311.12,W,225444,A\n\n";
printf("Parsing NMEA sentence: %s", sentence);
// Pointer to struct containing the parsed data. Should be freed manually.
nmea_s *data;
// Parse...
data = nmea_parse(sentence, strlen(sentence), 0);
if (NMEA_GPGLL == data->type) {
nmea_gpgll_s *gpgll = (nmea_gpgll_s *) data;
printf("GPGLL Sentence\n");
printf("Longitude:\n");
printf(" Degrees: %d\n", gpgll->longitude.degrees);
printf(" Minutes: %f\n", gpgll->longitude.minutes);
printf(" Cardinal: %c\n", (char) gpgll->longitude.cardinal);
printf("Latitude:\n");
printf(" Degrees: %d\n", gpgll->latitude.degrees);
printf(" Minutes: %f\n", gpgll->latitude.minutes);
printf(" Cardinal: %c\n", (char) gpgll->latitude.cardinal);
}
nmea_free(data);
return 0;
}
int main()
{
const char *buff =
"$GPGGA,073044.000,2235.8106,N,11359.9026,E,1,8,1.01,143.7,M,-2.4,M,,*41\r\n\
$GPRMC,070344.000,A,2235.8106,N,11359.9026,E,0.17,220.01,200916,,,A*6A\r\n";
int it;
nmeaINFO info;
nmeaPARSER parser;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
// for(it = 0; it < 1; ++it)
nmea_parse(&parser, buff, (int)strlen(buff), &info);
printf("gps time:\t%04d-%02d-%02d %02d:%02d:%02d\n",
info.utc.year + 1900,
info.utc.mon + 1,
info.utc.day,
info.utc.hour + 8,
info.utc.min,
info.utc.sec);
printf("gps latitude:\t%f\n", info.lat);
printf("gps longitude:\t%f\n", info.lon);
printf("gps altitude:\t%f\n", info.elv);
nmea_parser_destroy(&parser);
return 0;
}
int main()
{
const char *buff[] = {
"$GPRMC,173843,A,3349.896,N,11808.521,W,000.0,360.0,230108,013.4,E*69\r\n",
"$GPGGA,111609.14,5001.27,N,3613.06,E,3,08,0.0,10.2,M,0.0,M,0.0,0000*70\r\n",
"$GPGSV,2,1,08,01,05,005,80,02,05,050,80,03,05,095,80,04,05,140,80*7f\r\n",
"$GPGSV,2,2,08,05,05,185,80,06,05,230,80,07,05,275,80,08,05,320,80*71\r\n",
"$GPGSA,A,3,01,02,03,04,05,06,07,08,00,00,00,00,0.0,0.0,0.0*3a\r\n",
"$GPRMC,111609.14,A,5001.27,N,3613.06,E,11.2,0.0,261206,0.0,E*50\r\n",
"$GPVTG,217.5,T,208.8,M,000.00,N,000.01,K*4C\r\n"
};
int it;
nmeaINFO info;
nmeaPARSER parser;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
for(it = 0; it < 6; ++it)
nmea_parse(&parser, buff[it], (int)strlen(buff[it]), &info);
nmea_parser_destroy(&parser);
return 0;
}
int main(int argc, char const *argv[]) {
char strings[][73] = { "$GPGGA,152606.000,3732.7200,N,07726.9881,W,1,4,1.56,159.0,M,-33.6,M,,*64",
"$GPGSA,A,3,21,14,22,24,,,,,,,,,1.85,1.56,0.99*0C",
"$GPRMC,152606.000,A,3732.7200,N,07726.9881,W,0.29,81.28,110413,,,A*48",
"$GPVTG,81.28,T,,M,0.29,N,0.53,K,A*03",
"$PGTOP,11,2*6E" };
for (int i = 0; i < 5; i++) {
nmea_parse(strings[i], strlen(strings[i]));
}
// Validate state
within_range(get_latitude(), 37.545333, 0.00001f);
within_range(get_longitude(), -77.449802, 0.00001f);
within_range(get_altitude(), 159.0, 0.1f);
validate_int(get_sat_count(), 4);
validate_int(get_fix_type(), 3);
nmea_parser_t *parser = nmea_parser_new();
for (int i = 0; i < 5; i++) {
nmea_parse_r(strings[i], strlen(strings[i]), parser);
}
within_range(get_latitude_r(parser), 37.545333, 0.00001f);
within_range(get_longitude_r(parser), -77.449802, 0.00001f);
within_range(get_altitude_r(parser), 159.0, 0.1f);
validate_int(get_sat_count_r(parser), 4);
validate_int(get_fix_type_r(parser), 3);
nmea_parser_free(&parser);
assert(NULL == parser);
return error;
}
/*
portTASK_FUNCTION(gps, pvParameters ) {
vTaskDelay(500);
awal_gps();
//ser2_putstring("Masuk serial 2\r\n");
vTaskDelay(100);
int hasilGPS=0;
for(;;) {
vTaskDelay(2);
//if (len=serPollGPS())
{
if (jgps<2) {
hasilGPS = proses_gps();
#ifdef DEBUG_GPSs
info_gps(hasilGPS);
#endif
}
}
#ifdef DEBUG_GPSs
vTaskDelay(3000);
#endif
}
deinit_gps();
printf("akhir GPS !!\r\n");
}
//*/
int proses_gps() {
int parsenya=0;
parsenya = nmea_parse(&parserGPS, pesanGPS, (int)strlen(pesanGPS), &infoGPS);
//printf("serPoll : %d :%3d: %s\r\n", parsenya, (int)strlen(pesanGPS), pesanGPS);
nmea_info2pos(&infoGPS, &dposGPS);
//info_gps(parsenya);
return parsenya;
}
static void parse_gps(enum sys_message msg)
{
nmea_parse((char *)uart0_rx_buf, uart0_p);
uart0_p = 0;
uart0_rx_enable = 1;
LED_OFF;
}
/**
* static int Gps_set(void * _self, va_list *app) - Perform a new gps acquisition.
* \param *_self - pointer to the device
* \param *app - none
* \return:
<LI><Breturn = 0:</B>when the GPS device is connected on a Grove nest DIG port, with or without a valid connection </LI>
<LI><Breturn = -1:</B>when the GPS device is not connected on a Grove nest DIG port. </LI>
</UL>
*/
static int Gps_set(void *_self,va_list *app)
{
struct Gps *self = _self;
char gps_sentences[70];
unsigned int timer = 65000;
flag = 1;
nmea_zero_INFO(&self->info);
nmea_parser_init(&self->parser);
UARTOn(self->uart_module);
UARTFlush(self->uart_module);
vTaskDelay(50);
while((UARTBufferSize(self->uart_module)<3) && timer)
timer--;
if(!timer)
return -1;
timer = 500;
while((gps_sentences[0] != '$') && timer)
{
timer--;
while(UARTBufferSize(self->uart_module)<1);
UARTRead(self->uart_module,gps_sentences,1);
if(gps_sentences[0] == '$')
{
while(UARTBufferSize(self->uart_module) < 5);
UARTRead(self->uart_module,(gps_sentences+1),5); //Reads all the chars in the
if((gps_sentences[4] == 'M'))
{
while(UARTBufferSize(self->uart_module) < 65);
UARTRead(self->uart_module,(gps_sentences+6),64); //Reads all the chars in the
gps_sentences[70] = '\0';
int i = 0;
i = nmea_parse(&self->parser, gps_sentences, (int)strlen(gps_sentences), &self->info);
if(i)
{
flag = 0;
self->lat = nmea_ndeg2degree (self->info.lat);
self->lon = -nmea_ndeg2degree (self->info.lon);
self->year = self->info.utc.year+1900;
self->mon = self->info.utc.mon;
self->day = self->info.utc.day;
self->hour = self->info.utc.hour;
self->min = self->info.utc.min;
self->sec = self->info.utc.sec;
self->speed = self->info.speed;
break;
}
}
}
gps_sentences[0]= 0;
}
nmea_parser_destroy(&self->parser);
UARTOff(self->uart_module);
return 0;
}
static gps_mask_t italk_parse_input(struct gps_device_t *session)
{
if (session->lexer.type == ITALK_PACKET) {
return italk_parse(session, session->lexer.outbuffer,
session->lexer.outbuflen);;
#ifdef NMEA0183_ENABLE
} else if (session->lexer.type == NMEA_PACKET) {
return nmea_parse((char *)session->lexer.outbuffer, session);
#endif /* NMEA0183_ENABLE */
} else
return 0;
}
int main()
{
static const char * filename = "../../samples/parse_file/gpslog.txt";
nmeaINFO info;
nmeaPARSER parser;
FILE *file;
char buff[2048];
int size, it = 0;
nmeaPOS dpos;
file = fopen(filename, "rb");
if(!file) {
printf("Could not open file %s\n", filename);
return -1;
}
nmea_property()->trace_func = &trace;
nmea_property()->error_func = &error;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
/*
while(1)
{
*/
while(!feof(file))
{
size = (int)fread(&buff[0], 1, 100, file);
nmea_parse(&parser, &buff[0], size, &info);
nmea_info2pos(&info, &dpos);
printf(
"%03d, Lat: %f, Lon: %f, Sig: %d, Fix: %d\n",
it++, dpos.lat, dpos.lon, info.sig, info.fix
);
}
fseek(file, 0, SEEK_SET);
/*
}
*/
nmea_parser_destroy(&parser);
fclose(file);
return 0;
}
static gps_mask_t evermore_parse_input(struct gps_device_t *session)
{
gps_mask_t st;
if (session->packet.type == EVERMORE_PACKET){
st = evermore_parse(session, session->packet.outbuffer, session->packet.outbuflen);
return st;
#ifdef NMEA_ENABLE
} else if (session->packet.type == NMEA_PACKET) {
st = nmea_parse((char *)session->packet.outbuffer, session);
return st;
#endif /* NMEA_ENABLE */
} else
return 0;
}
gps_mask_t generic_parse_input(struct gps_device_t *session)
{
const struct gps_type_t **dp;
if (session->packet.type == COMMENT_PACKET) {
gpsd_set_century(session);
return 0;
#ifdef NMEA_ENABLE
} else if (session->packet.type == NMEA_PACKET) {
gps_mask_t st = 0;
char *sentence = (char *)session->packet.outbuffer;
if (sentence[strlen(sentence)-1] != '\n')
gpsd_report(LOG_IO, "<= GPS: %s\n", sentence);
else
gpsd_report(LOG_IO, "<= GPS: %s", sentence);
if ((st=nmea_parse(sentence, session)) == 0) {
gpsd_report(LOG_WARN, "unknown sentence: \"%s\"\n", sentence);
}
for (dp = gpsd_drivers; *dp; dp++) {
char *trigger = (*dp)->trigger;
if (trigger!=NULL && strncmp(sentence,trigger,strlen(trigger))==0) {
gpsd_report(LOG_PROG, "found trigger string %s.\n", trigger);
if (*dp != session->device_type) {
(void)gpsd_switch_driver(session, (*dp)->type_name);
if (session->device_type != NULL
&& session->device_type->event_hook != NULL)
session->device_type->event_hook(session,
event_triggermatch);
st |= DEVICEID_SET;
}
}
}
return st;
#endif /* NMEA_ENABLE */
} else {
for (dp = gpsd_drivers; *dp; dp++) {
if (session->packet.type == (*dp)->packet_type) {
(void)gpsd_switch_driver(session, (*dp)->type_name);
return (*dp)->parse_packet(session);
}
}
return 0;
}
}
void gps_rx_cb (void * arg, uint8_t *buf, int len, void *pxTaskWoken)
{
gps_cfg_t *gps_cfg = (gps_cfg_t *) arg;
DEBUG("GPS_RX: %c\n", *buf);
if (*buf == '\n' || *buf == '\r') {
gps_fix_t *fix = gps_incr_fix();
int len = gps_buf_cur + 1;
int result = nmea_parse(gps_buf, len, fix);
memset(gps_buf, '\0', len);
gps_buf_cur = 0;
if (result != NMEA_OK) {
DEBUG("error parsing NMEA: %d\n", result);
return;
}
// Send location fix
if (NULL != gps_cfg->conn) {
DEBUG("Parsed NMEA sentence, sending message\n");
DEBUG("NMEA %02d:%02d:%02d.%03d\n", fix->hour, fix->minute,
fix->seconds, fix->milliseconds);
csp_packet_t * msg;
msg = csp_buffer_get(sizeof(gps_fix_t));
memcpy(msg->data, fix, sizeof(gps_fix_t));
//*((gps_fix_t*)msg->data) = *fix;
msg->length = sizeof(gps_fix_t);
if (!csp_send(gps_cfg->conn, msg, 1000)) {
DEBUG("csp_send failed\n");
csp_buffer_free(msg);
}
}
return;
}
gps_buf[gps_buf_cur++] = *buf;
DEBUG("GPS_BUF: %.*s\n", gps_buf_cur, gps_buf);
if (gps_buf_cur > GPS_BUFSIZE - 1) {
DEBUG("Too much data for GPS buffer, discarding");
gps_buf_cur = 0;
}
}
int main()
{
const char *buff[] = {
"$GPGGA,000000,2508.644,N,12145.244,E,8,00,99.9,00000,M,00000,M,,*4A\r\n\r\n",
"$SDDBT,0008.2,f,0002.5,M,0001.4,F*3E\r\n\r\n"
};
const char *buff2[] = {
"$GPDTM,W84,,0.0000,N,0.0000,E,0.0000,W84*5F\r\n",
"$GPAPB,V,V,-0.00,L,N,V,V,090.2,T,0009,090.2,T,090.2,T,N*6F\r\n",
"$GPBOD,090.2,T,090.2,M,0009,0000*4E\r\n",
"$GPBWC,000000,2508.956,N,12146.687,E,090.2,T,090.2,M,000.27,N,N*74\r\n",
"$GPGGA,000000,2508.957,N,12146.385,E,0,00,99.9,00000,M,00000,M,,*40\r\n",
"$GPGBS,,,,,,,,*41\r\n",
"$GPRMC,000000,V,2508.957,N,12146.385,E,00.0,000.0,010105,,,N*57\r\n",
"$GPVTG,000.0,T,,,00.0,N,00.0,K,N*4F\r\n",
"$GPRMC,000000,V,2515.791,N,12145.337,E,00.0,251.7,010105,,,S*49\r\n"
};
int it;
for(it = 0; it < 2; ++it)
{
nmeaINFO info;
nmeaPARSER parser;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
nmea_parse(&parser, buff[it], (int)strlen(buff[it]), &info);
printf("parse: %s\n", buff[it]);
printf("declination: %f\n", info.declination);
printf("utc.min: %f\n", info.utc.min);
printf("HDOP: %f\n", info.HDOP);
printf("VDOP: %f\n", info.VDOP);
printf("lat: %f\n", info.lat);
printf("lon: %f\n", info.lon);
printf("elv: %f\n", info.elv);
printf("depth: %f\n", info.depthinfo.depth_M);
nmea_parser_destroy(&parser);
}
return 0;
}
static void tnt_update(void)
{
/*
* We have to do our own field parsing because the way this
* gets called, nmea_parse() is never called on the sentence.
*/
(void)nmea_parse((char *)session.packet.outbuffer, &session);
(void)mvwaddstr(thtmwin, 1, 19, session.driver.nmea.field[1]);
(void)mvwaddstr(thtmwin, 2, 19, session.driver.nmea.field[3]);
(void)mvwaddstr(thtmwin, 3, 19, session.driver.nmea.field[5]);
(void)mvwaddstr(thtmwin, 4, 19, session.driver.nmea.field[7]);
(void)mvwaddstr(thtmwin, 1, 61, session.driver.nmea.field[2]);
(void)mvwaddstr(thtmwin, 2, 61, session.driver.nmea.field[4]);
(void)mvwaddstr(thtmwin, 3, 61, session.driver.nmea.field[6]);
(void)mvwaddstr(thtmwin, 4, 61, session.driver.nmea.field[8]);
}
static gps_mask_t italk_parse_input(struct gps_device_t *session)
{
gps_mask_t st;
if (session->packet.type == ITALK_PACKET) {
st = italk_parse(session, session->packet.outbuffer,
session->packet.outbuflen);
session->gpsdata.dev.driver_mode = MODE_BINARY; /* binary */
return st;
#ifdef NMEA_ENABLE
} else if (session->packet.type == NMEA_PACKET) {
st = nmea_parse((char *)session->packet.outbuffer, session);
session->gpsdata.dev.driver_mode = MODE_NMEA; /* NMEA */
return st;
#endif /* NMEA_ENABLE */
} else
return 0;
}
void GpsParser::parse(const std::string& packet, GpsInfo& info)
{
// Create a new GPS info
GpsInfo last;
// Reset the info
last.reset();
// Append return and end of line character
std::string message = (packet.substr(0, packet.length() - 1)).append("\r\n");
// Parse the NMEA packet
nmea_parse(&_parser, message.c_str(), message.length(), &last.info());
// Check if the packet is valid
if (last.signal() >= 2)
{
// Push the last to become the current GPS info
info = last;
}
}
void ReceiverThread::run()
{
//qWarning() << "Starting thread loop (1)";
int numBytes = 0;
int l = 512;
#ifndef GPS_FILE
if(!port ||(port && !port->isOpen()))
{
qWarning()<<"anomaly 1";
if(port)
{
delete port;
port=NULL;
}
/*retry to open the port*/
if(!this->initPort())
{
qWarning() << "Port not open => not starting GPS thread";
if(port) port->close();
delete port;
port=NULL;
exit();
return;
}
}
#else
QFile fakeGPS("fakeGPS.data");
if(!fakeGPS.open(QIODevice::ReadOnly | QIODevice::Text))
{
qWarning()<<"failed to open fakeGPS.data";
return;
}
#endif
//qWarning() << "Starting thread loop (2)";
/*clear port*/
//port->readAll();
while (!parent->getPause() && port && port->isOpen())
{
#ifndef GPS_FILE
numBytes = port->bytesAvailable();
#else
numBytes=513;
#endif
if(numBytes > l)
{
QByteArray buffer;
#ifndef GPS_FILE
while(port->bytesAvailable()>0)
{
buffer=buffer+port->read(512);
}
#else
buffer.clear();
for(int pass=0;pass<50;++pass)
{
if(fakeGPS.atEnd())
{
fakeGPS.seek(0);
qWarning()<<"restarting fakeGPS file";
}
buffer=fakeGPS.readLine();
#endif
QList<QByteArray> lines=buffer.split('\n');
for (int n=0;n<lines.count();++n)
{
if(lines.at(n).count()>1)
{
#ifndef GPS_FILE
QString temp=QString(lines.at(n).left(lines.at(n).length()-1));
#else
QString temp=lines.at(n);
#endif
// if(temp.contains("RMC"))
// qWarning()<<temp;
#ifndef GPS_FILE
QByteArray data=lines.at(n).left(lines.at(n).length()-1);
#else
QByteArray data=lines.at(n);
#endif
//data.replace("$II","$GP"); would have been good but... checksum
data.push_back('\r' );
data.push_back('\n');
char * record=data.data();
nmea_parse(&parser, record, data.size(), &info);
if(listNMEA)
{
listNMEA->addItem(temp);
}
}
}
emit updateBoat(info);
#ifdef GPS_FILE
}
#endif
}
this->msleep(3000);
}
if(port)
{
if(port->isOpen())
port->close();
//qWarning()<<"deleting port";
delete port;
port=NULL;
}
//qWarning() << "Exiting thread loop";
this->exit();
}
int main(int argc, char** argv) {
/* Parameters (can be set using -options) */
char* port_name = NULL;
int delay = 10;
int baudrate = 9600;
/* Handle -options */
int c;
while ((c = getopt(argc, argv, "hb:d:")) != -1) {
switch (c) {
case 'b':
sscanf(optarg, "%d", &baudrate);
break;
case 'd':
sscanf(optarg, "%d", &delay);
break;
case 'h':
printf("Usage: %s [-b <BAUDRATE>] [-d <DELAY>] PORT\n", argv[0]);
printf("Saves time from a GPS receiver connected to PORT as local time.\n");
printf("Waits specified time until valid $GPRMC NMEA sentence is received. If no valid time information is received (i.e. the signal is poor), the local time remains unchanged.\n");
printf("This program has to be run with root privilegies in order to configure local time.\n");
printf("\n");
printf(" -b BAUDRATE Sets specified baudrate. Only limited baudrate set {2400, 4800, ..., 230400} is supported (Default %d bd).\n", baudrate);
printf(" -d DELAY Sets the maximum time in seconds or -1 for infinite loop (Default %d s).\n", delay);
printf(" -h Displays this help.\n");
printf("\n");
printf("(c) 2014 Adam Heinrich, www.adamheinrich.com\n");
return 0;
break;
case '?':
if (optopt == 'b' || optopt == 'd') {
fprintf(stderr, "Option -%c requires an argument.\nUse %s -h for help.\n", optopt, argv[0]);
} else {
fprintf(stderr, "Unknown option character '%c'.\nUse %s -h for help.\n", optopt, argv[0]);
}
return 1;
default:
fprintf(stderr, "Usage: %s [-b <BAUDRATE>] [-d <DELAY>] PORT_NAME\n", argv[0]);
return 1;
}
}
/* Save port name */
if (optind < argc) {
port_name = argv[optind];
} else {
fprintf(stderr, "Port name must be specified.\nUse %s -h for help.\n", argv[0]);
return 1;
}
/* Check baudrate */
if (baudrate_constant(baudrate) == -1) {
fprintf(stderr, "Unsupported baudrate: %d.\nUse %s -h for help.\n", baudrate, argv[0]);
return 1;
}
/* Open port */
int fd = open_port(port_name, baudrate);
if (fd == -1) {
fprintf(stderr, "Can't open port %s\n", port_name);
return 1;
}
/* Real gentlemen clean up after finishing the job! */
catch_signals();
int nread;
char buffer[32];
time_t first_time;
time_t curr_time;
time(&first_time);
read_gps = 1;
while (read_gps) {
nread = read(fd, buffer, 32);
if (nread > 0) {
nmea_parse(nread, buffer, &message_complete);
}
time(&curr_time);
if (read_gps && delay != -1 && (curr_time - first_time) >= delay) {
printf("No valid time information from GPS in last %d seconds.\n", delay);
read_gps = 0;
}
}
close(fd);
return 0;
}
int UartNMEAGPS::read(devices::gps_data *data)
{
int got = uart->read(buffer+buffer_count, sizeof(buffer) - buffer_count);
if (got > 0)
m_healthy = true;
else
return 1;
buffer_count += got;
if (!m_healthy)
return -1;
int return_pos = -1;
for(int i = buffer_count - 1; i>0; i--)
{
if (buffer[i] == '\n')
{
return_pos = i+1;
break;
}
}
int return_value = 1;
if (return_pos > 0)
{
int parse_result = nmea_parse(&parser, buffer, return_pos, &info);
memmove(buffer, buffer+return_pos, buffer_count - return_pos);
buffer_count -= return_pos;
// do we have GPGGA and GPRMC packets?
if (parse_result > 0 && (info.smask & GPGGA) && (info.smask & GPRMC))
{
return_value = 0;
info.smask = 0;
}
else
{
return_value = 1;
}
}
// copy from info
data->longitude = NDEG2DEG(info.lon); // longitude in degree
data->latitude = NDEG2DEG(info.lat); // latitude in degree
data->speed = info.speed / 3.6f; // unit: meter/s
data->altitude = info.elv; // meter
data->direction = info.direction; // Track angle in degrees True, 0-360 degree, 0: north, 90: east, 180: south, 270: west, 359:almost north
data->DOP[0] = info.PDOP*100; // DOP[3]: PDOP, HDOP, VOP, unit base: 0.01
data->DOP[1] = info.HDOP*100; // DOP[3]: PDOP, HDOP, VOP, unit base: 0.01
data->DOP[2] = info.VDOP*100; // DOP[3]: PDOP, HDOP, VOP, unit base: 0.01
data->satelite_in_view = info.satinfo.inview;
data->satelite_in_use = info.satinfo.inuse;
data->sig = info.sig; // GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive)
data->fix = info.fix; // Operating mode, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D)
data->declination = info.declination * 100.0f;// Magnetic variation in 0.01 degrees (Easterly var. subtracts from true course)
struct tm _tm;
_tm.tm_sec = info.utc2.sec;
_tm.tm_min = info.utc2.min;
_tm.tm_hour = info.utc2.hour;
_tm.tm_mday = info.utc2.day;
_tm.tm_mon = info.utc2.mon;
_tm.tm_year = info.utc2.year;
data->timestamp = mktime(&_tm);
data->timestamp += info.utc2.zone_hour * 3600;
return return_value;
}
void lip_gps_proc(vam_envar_t *p_vam, uint8_t *databuf, uint32_t len)
{
nmea_parse(databuf, len);
}
void _main(int argc, char *argv[])
{
(void)argc;
(void)argv;
if (scl_init("gpsp") != 0)
{
syslog(LOG_CRIT, "could not init scl module");
exit(EXIT_FAILURE);
}
gps_socket = scl_get_socket("data");
if (gps_socket == NULL)
{
syslog(LOG_CRIT, "could not get scl gate");
exit(EXIT_FAILURE);
}
int64_t hwm = 1;
zmq_setsockopt(gps_socket, ZMQ_SNDHWM, &hwm, sizeof(hwm));
opcd_param_t params[] =
{
{"serial_path", &serial_path},
{"serial_speed", &serial_speed},
OPCD_PARAMS_END
};
opcd_params_init("sensors.gps.", 0);
opcd_params_apply("", params);
int status = serial_open(&port, serial_path, tsint_get(&serial_speed), 0, 0, 0);
if (status < 0)
{
syslog(LOG_CRIT, "could not open serial port: %s", serial_path);
exit(EXIT_FAILURE);
}
nmeaPARSER parser;
nmea_parser_init(&parser);
nmeaINFO info;
nmea_zero_INFO(&info);
int time_set = 0;
int smask = 0; /* global smask collects all sentences and is never reset,
in contrast to info.smask */
while (running)
{
int c = serial_read_char(&port);
if (c < 0)
continue;
char b = c;
/* parse NMEA frame: */
if (nmea_parse(&parser, &b, 1, &info) == 1)
{
smask |= info.smask;
if ( (info.smask & GPGGA) /* check for new position update */
&& (smask & (GPGSA | GPRMC))) /* go sure that we collect all sentences for first output*/
{
GpsData gps_data = GPS_DATA__INIT;
/* set general data: */
char time_str[TIME_STR_LEN];
generate_time_str(time_str, &info.utc);
gps_data.fix = 0;
gps_data.time = time_str;
/* set system time to gps time once: */
if (!time_set && info.fix >= 2)
{
char shell_date_cmd[TIME_STR_LEN + 8];
linux_sys_set_timezone(convert(info.lat), convert(info.lon));
sprintf(shell_date_cmd, "date -s \"%s\"", time_str);
time_set = system(shell_date_cmd) == 0;
}
/* set position data if a minimum of satellites is seen: */
if (info.fix >= 2)
{
gps_data.fix = 2;
PB_SET(gps_data, hdop, info.HDOP);
PB_SET(gps_data, lat, convert(info.lat));
PB_SET(gps_data, lon, convert(info.lon));
PB_SET(gps_data, sats, info.satinfo.inuse);
PB_SET(gps_data, course, info.track);
PB_SET(gps_data, speed, info.speed);
}
/* set data for 3d fix: */
if (info.fix == 3)
{
gps_data.fix = 3;
PB_SET(gps_data, vdop, info.VDOP);
PB_SET(gps_data, alt, info.elv);
}
/* add satellit info: */
unsigned int i;
gps_data.n_satinfo = info.satinfo.inview;
SatInfo **satinfo = malloc(gps_data.n_satinfo * sizeof(SatInfo *));
for (i = 0; i < gps_data.n_satinfo; i++)
{
/* fill SatInfo structure: */
nmeaSATELLITE *nmea_satinfo = &info.satinfo.sat[i];
satinfo[i] = malloc(gps_data.n_satinfo * sizeof(SatInfo));
sat_info__init(satinfo[i]);
satinfo[i]->id = nmea_satinfo->id;
satinfo[i]->in_use = info.satinfo.in_use[i] ? 1 : 0;
satinfo[i]->elv = nmea_satinfo->elv;
satinfo[i]->azimuth = nmea_satinfo->azimuth;
satinfo[i]->sig = nmea_satinfo->sig;
}
gps_data.satinfo = satinfo;
/* send the data: */
SCL_PACK_AND_SEND_DYNAMIC(gps_socket, gps_data, gps_data);
/* free allocated memory: */
for (i = 0; i < gps_data.n_satinfo; i++)
{
free(satinfo[i]);
}
free(satinfo);
}
nmea_zero_INFO(&info);
}
}
}
int nmea_decode_test(void){
int tempOut=0;
s16 AccelGyro[7]={0};
s16 TempBuffer[7]={0};
nmeaINFO info; //GPS parsed info
nmeaPARSER parser; //struct used for decoding
uint8_t new_parse=0; //new or not, have history?
nmeaTIME beiJingTime;
nmea_property()->trace_func = &trace;
nmea_property()->error_func = &error;
//GPS initialization
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
while(1)
{
if(GPS_HalfTransferEnd) /* received half the buffer size*/
{
/* parse using nmea format */
nmea_parse(&parser, (const char*)&gps_rbuff[0], HALF_GPS_RBUFF_SIZE, &info);
GPS_HalfTransferEnd = 0; //Clear flag
new_parse = 1; //new info
}
else if(GPS_TransferEnd) /* receiving the other half */
{
nmea_parse(&parser, (const char*)&gps_rbuff[HALF_GPS_RBUFF_SIZE], HALF_GPS_RBUFF_SIZE, &info);
GPS_TransferEnd = 0;
new_parse =1;
}
if(new_parse ) //if have new info
{
//Converts time to GMT
GMTconvert(&info.utc,&beiJingTime,8,1);
/* Output data*/
printf("\r\n Time:%d,%d,%d,%d,%d,%d\r\n", beiJingTime.year+1900, beiJingTime.mon+1,beiJingTime.day,beiJingTime.hour,beiJingTime.min,beiJingTime.sec);
printf("\r\n Latitude:%f,Longtitude:%f\r\n",info.lat,info.lon);
printf("\r\n Numbers of Sat in use:%d, Numbers of Sat in view:%d",info.satinfo.inuse,info.satinfo.inview);
printf("\r\n Numbers of meters above horizon: %f", info.elv);
printf("\r\n Speed: %f km/h ", info.speed);
printf("\r\n Direction: %f degree", info.direction);
new_parse = 0;
}
//--------------------------actual loop------------------------
//------------------------this is imu------------------------
printf("\r\nMPU Readings:");
MPU6050_GetRawAccelGyro(AccelGyro);
printf("\r\nIMU[0]: %10d",AccelGyro[0]);
printf("\t IMU[1]: %10d",AccelGyro[1]);
printf("\t IMU[2]: %10d",AccelGyro[2]);
printf("\t IMU[3]: %10d",AccelGyro[3]);
printf("\t IMU[4]: %10d",AccelGyro[4]);
printf("\t IMU[5]: %10d",AccelGyro[5]);
//--------------------------temp loop--------------------------
//tempOut=USART3_getTemp();
//printf("\r\n temp is: %d\n",tempOut);
//-------------------------------------------------------------
delay_ms(1000);
}
}
int main()
{
const char *buff[] = {
"$GPRMC,213916.199,A,4221.0377,N,07102.9778,W,0.00,,010207,,,A*6A\r\n",
"$GPRMC,213917.199,A,4221.0510,N,07102.9549,W,0.23,175.43,010207,,,A*77\r\n",
"$GPRMC,213925.000,A,4221.1129,N,07102.9146,W,0.00,,010207,,,A*68\r\n",
"$GPRMC,111609.14,A,5001.27,N,3613.06,E,11.2,0.0,261206,0.0,E*50\r\n"
};
nmeaPOS pos[NUM_POINTS], pos_moved[NUM_POINTS][2];
double dist[NUM_POINTS][2];
double azimuth[NUM_POINTS][2], azimuth_moved[NUM_POINTS];
int result[2];
int it = 0;
nmeaPARSER parser;
nmea_parser_init(&parser);
for(it = 0; it < NUM_POINTS; ++it)
{
nmeaINFO info;
nmea_zero_INFO(&info);
(void)nmea_parse(&parser, buff[it], (int)strlen(buff[it]), &info);
nmea_info2pos(&info, &pos[it]);
}
nmea_parser_destroy(&parser);
for(it = 0; it < NUM_POINTS; ++it)
{
dist[it][0] = nmea_distance(&pos[0], &pos[it]);
dist[it][1] = nmea_distance_ellipsoid(
&pos[0], &pos[it], &azimuth[it][0], &azimuth[it][1]
);
}
for(it = 0; it < NUM_POINTS; ++it)
{
result[0] = nmea_move_horz(&pos[0], &pos_moved[it][0], azimuth[it][0], dist[it][0]);
result[1] = nmea_move_horz_ellipsoid(
&pos[0], &pos_moved[it][1], azimuth[it][0], dist[it][0], &azimuth_moved[it]
);
}
/* Output of results */
printf("Coordinate points:\n");
for(it = 0; it < NUM_POINTS; ++it)
{
printf(
"P%d in radians: lat:%9.6lf lon:%9.6lf \tin degree: lat:%+010.6lf° lon:%+011.6lf°\n",
it, pos[it].lat, pos[it].lon, nmea_radian2degree(pos[it].lat), nmea_radian2degree(pos[it].lon)
);
}
printf("\nCalculation results:\n");
for(it = 0; it < NUM_POINTS; ++it)
{
printf("\n");
printf("Distance P0 to P%d\ton spheroid: %14.3lf m\n", it, dist[it][0]);
printf("Distance P0 to P%d\ton ellipsoid: %14.3lf m\n", it, dist[it][1]);
printf("Azimuth P0 to P%d\tat start: %8.3lf°\tat end: %8.3lf°\n", it, nmea_radian2degree(azimuth[it][0]), nmea_radian2degree(azimuth[it][1]));
printf("Move P0 to P%d\t \tAzimuth at end: %8.3lf°\n", it, nmea_radian2degree(azimuth_moved[it]));
printf("Move P0 to P%d\ton spheroid: %3s lat:%+010.6lf° lon:%+011.6lf°\n", it, result[0] == 1 ? "OK" : "nOK", nmea_radian2degree(pos_moved[it][0].lat), nmea_radian2degree(pos_moved[it][0].lon));
printf("Move P0 to P%d\ton ellipsoid: %3s lat:%+010.6lf° lon:%+011.6lf°\n", it, result[0] == 1 ? "OK" : "nOK", nmea_radian2degree(pos_moved[it][1].lat), nmea_radian2degree(pos_moved[it][1].lon));
printf("Move P0 to P%d\toriginal: lat:%+010.6lf° lon:%+011.6lf°\n", it, nmea_radian2degree(pos[it].lat), nmea_radian2degree(pos[it].lon));
}
return 0;
}
int
main(void)
{
char buffer[4096];
int gps_fd = 0; // stdin
int read_bytes = 0;
int total_bytes = 0;
char *start, *end;
sigset_t block_mask;
while (1) {
char buf[255];
nmea_s *data;
read_bytes = read(gps_fd, buffer + total_bytes, 20);
if (-1 == read_bytes) {
perror("read stdin");
exit(EXIT_FAILURE);
}
if (0 == read_bytes) {
break;
}
total_bytes += read_bytes;
/* find start (a dollar $ign) */
start = memchr(buffer, '$', total_bytes);
if (NULL == start) {
total_bytes = 0;
continue;
}
/* find end of line */
end = memchr(start, NMEA_END_CHAR_1, total_bytes - (start - buffer));
if (NULL == end || NMEA_END_CHAR_2 != *(++end)) {
continue;
}
/* handle data */
data = nmea_parse(start, end - start + 1, 0);
if (NULL != data) {
if (0 < data->errors) {
printf("{ type: 'GPWRN', data: { message:'The following sentence contains parse errors!' } }\n");
}
if (NMEA_GPGGA == data->type) {
nmea_gpgga_s *gpgga = (nmea_gpgga_s *) data;
printf("{ type: 'GPGGA', data: { satellites: %d, altitude: '%d%c' } }\n", gpgga->n_satellites, gpgga->altitude, gpgga->altitude_unit);
}
if (NMEA_GPGLL == data->type) {
nmea_gpgll_s *pos = (nmea_gpgll_s *) data;
strftime(buf, sizeof(buf), "%H:%M:%S", &pos->time);
printf("{ type: 'GPGLL', data: { long: '%c%d:%f', lat: '%c%d:%f', time: '%s' } }\n",\
(char) pos->longitude.cardinal, pos->longitude.degrees, pos->longitude.minutes,\
(char) pos->latitude.cardinal, pos->latitude.degrees, pos->latitude.minutes,\
buf);
}
if (NMEA_GPRMC == data->type) {
nmea_gprmc_s *pos = (nmea_gprmc_s *) data;
strftime(buf, sizeof(buf), "%H:%M:%S", &pos->time);
printf("{ type: 'GPRMC', data: { long: '%c%d:%f', lat: '%c%d:%f', time: '%s' } }\n",\
(char) pos->longitude.cardinal, pos->longitude.degrees, pos->longitude.minutes,\
(char) pos->latitude.cardinal, pos->latitude.degrees, pos->latitude.minutes,\
buf);
}
nmea_free(data);
}
/* buffer empty? */
if (end == buffer + total_bytes) {
total_bytes = 0;
continue;
}
/* copy rest of buffer to beginning */
if (buffer != memmove(buffer, end, total_bytes - (end - buffer))) {
total_bytes = 0;
continue;
}
total_bytes -= end - buffer;
}
return 0;
}
int parse_gps(char * database, int tripid) {
int retval;
// Open database connection
retval = sqlite3_open(database, &handle);
if (retval) {
printf("GPS Parsing: Database connection failed: %d\n", retval);
return -1;
}
sqlite3_exec(handle, "PRAGMA synchronous = NORMAL", 0, 0, 0);
sqlite3_exec(handle, "PRAGMA journal_mode = MEMORY", 0, 0, 0);
// Select data from database
char selectquery[100];
sprintf(selectquery,
"SELECT Gps_ID, RawData FROM GpsData WHERE Trip_ID = %d", tripid);
sqlite3_stmt *stmt;
retval = sqlite3_prepare_v2(handle, selectquery, -1, &stmt, 0);
if (retval) {
printf("GPS Parsing: Selecting data from DB Failed: %d\n", retval);
printf("Query: %s\n", selectquery);
return -1;
}
// Initialize NMEA parser
nmeaINFO info;
nmeaPARSER parser;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
// Begin SQL transaction
sqlite3_exec(handle, "BEGIN TRANSACTION", 0, 0, 0);
// Step through SELECT query
while (1) {
retval = sqlite3_step(stmt);
if (retval == SQLITE_ROW) {
// Gather row from database
int gpsid = (int) sqlite3_column_int(stmt, 0);
char *rawdata = (char*) sqlite3_column_text(stmt, 1);
int len = (int) strlen(rawdata);
// Parse NMEA data
nmea_parse(&parser, rawdata, len, &info);
// Convert time format to C_time
struct tm converttime;
converttime.tm_year = info.utc.year;
converttime.tm_mon = info.utc.mon;
converttime.tm_mday = info.utc.day;
converttime.tm_hour = info.utc.hour;
converttime.tm_min = info.utc.min;
converttime.tm_sec = info.utc.sec;
// Update row with NMEA data
char query[200];
sprintf(
query,
"UPDATE GpsData SET UTC = %f, Fix = %d, Latitude = %f, Longitude = %f, Speed = %f, Direction = %f, Declination = %f WHERE Gps_ID = %d", (float) mktime(&converttime), info.fix, nmea_ndeg2degree(info.lat), nmea_ndeg2degree(info.lon), info.speed, info.direction, info.declination, gpsid);
retval = sqlite3_exec(handle, query, 0, 0, 0);
if (retval) {
printf("GPS Parsing: Updating data in DB Failed: %d\n", retval);
return -1;
}
} else if (retval == SQLITE_DONE) {
break;
} else {
printf("GPS Parsing: SQL error whilst reading rows: %d\n", retval);
return -1;
}
}
// End SQL transaction
sqlite3_exec(handle, "END TRANSACTION", 0, 0, 0);
// Remove unnecesary data
char removequery[100];
sprintf(removequery,
"DELETE FROM GpsData WHERE RawData NOT LIKE '$GPRMC%%'");
retval = sqlite3_exec(handle, removequery, 0, 0, 0);
if (retval) {
printf("GPS Parsing: Removing data from DB Failed: %d\n", retval);
printf("Query: %s\n", removequery);
return -1;
}
// Destroy the evidence!
nmea_parser_destroy(&parser);
sqlite3_close(handle);
return 0;
}
void processNMEA() {
argsCarrier *Args = malloc(sizeof(argsCarrier));
nmeaArgs *nArgs = malloc(sizeof(nmeaArgs));
nmeaINFO *info = malloc(sizeof(nmeaINFO));
nmeaPARSER parser;
nmea_zero_INFO(info);
nmea_parser_init(&parser);
char *NMEA2;
nmeaINFO info2;
// Fix up the end so the NMEA parser accepts it
int count = (int)strlen(NMEA);
NMEA[count-1] = '\r';
NMEA[count] = '\n';
NMEA[count+1] = '\0';
// Parse the data
nmea_parse(&parser, NMEA, (int)strlen(NMEA), info);
if (info->smask == 0) {
//Bad data
free(nArgs);
free(info);
free(Args);
return;
}
// NOTE: Make copy of NMEA and Data for these threads - Don't want them to be overwritten by other threads
// Have the individual case function take care of freeing them
Args->NMEA = (char *)malloc((strlen(&NMEA[0])+1)*sizeof(char));
nArgs->NMEA = (char *)malloc((strlen(&NMEA[0])+1)*sizeof(char));
strcpy(Args->NMEA, NMEA);
strcpy(nArgs->NMEA, NMEA);
Args->info = info;
nArgs->time = getUTCTime(&(info->utc));
adamGpsCallbacks->create_thread_cb("adamgps-nmea", updateNMEA, nArgs);
switch (info->smask) {
case 1:
//< GGA - Essential fix data which provide 3D location and accuracy data.
adamGpsCallbacks->create_thread_cb("adamgps-gga", updateGGA, Args);
break;
case 2:
//< GSA - GPS receiver operating mode, SVs used for navigation, and DOP values.
adamGpsCallbacks->create_thread_cb("adamgps-gsa", updateGSA, Args);
break;
case 4:
//< GSV - Number of SVs in view, PRN numbers, elevation, azimuth & SNR values.
adamGpsCallbacks->create_thread_cb("adamgps-gsv", updateGSV, Args);
break;
case 8:
//< RMC - Recommended Minimum Specific GPS/TRANSIT Data.
//adamGpsCallbacks->create_thread_cb("adamgps-loc", updateRMC, Args);
free(Args->info);
free(Args->NMEA);
free(Args);
break;
case 16:
//< VTG - Actual track made good and speed over ground.
free(Args->info);
free(Args->NMEA);
free(Args);
break;
default:
free(Args->info);
free(Args->NMEA);
free(Args);
break;
}
//LOGV("Successful read: %i", info->smask);
}
int main(int argc, char *argv[])
{
int it = 0;
int done = 0;
nmeaINFO info;
nmeaPARSER parser;
int select_result = -1; // value returned frome select()
struct timeval select_timeout;
int nfds = 0;
fd_set rset;
char rs232_device;
int bytes_read;
char rs232_buffer[1024];
char *token;
char nmea_message[256];
if(argc < 2)
{
printf("Usage: %s <file to parse>\n", argv[0]);
return -1;
}
rs232_device = open(argv[1], O_RDWR | O_NOCTTY);
if(rs232_device < 0)
perror("com_open");
// Select UART2_TX and set it as output
printf("Setting tx. . .\n");
sprintf(rs232_buffer, "echo 11 > /sys/kernel/debug/omap_mux/spi0_d0");
if(system(rs232_buffer) < 0)
perror("setting tx");
// Select UART1_RX and set it as input pulled up
printf("Setting rx. . .\n");
sprintf(rs232_buffer, "echo 39 > /sys/kernel/debug/omap_mux/spi0_sclk");
if(system(rs232_buffer) < 0)
perror("setting rx");
const char *buff[] = {
"$GPRMC,173843,A,3349.896,N,11808.521,W,000.0,360.0,230108,013.4,E*69\r\n",
"$GPGGA,111609.14,5001.27,N,3613.06,E,3,08,0.0,10.2,M,0.0,M,0.0,0000*70\r\n",
"$GPGSV,2,1,08,01,05,005,80,02,05,050,80,03,05,095,80,04,05,140,80*7f\r\n",
"$GPGSV,2,2,08,05,05,185,80,06,05,230,80,07,05,275,80,08,05,320,80*71\r\n",
"$GPGSA,A,3,01,02,03,04,05,06,07,08,00,00,00,00,0.0,0.0,0.0*3a\r\n",
"$GPRMC,111609.14,A,5001.27,N,3613.06,E,11.2,0.0,261206,0.0,E*50\r\n",
"$GPVTG,217.5,T,208.8,M,000.00,N,000.01,K*4C\r\n"
};
select_timeout.tv_sec = 1;
select_timeout.tv_usec = 0;
nmea_zero_INFO(&info);
nmea_parser_init(&parser);
while(!done)
{
fflush(stdout);
FD_ZERO(&rset);
if(rs232_device > 0)
{
FD_SET(rs232_device, &rset);
nfds = max(nfds, rs232_device);
}
select_result = select(nfds + 1, &rset, NULL, NULL, NULL);
if(select_result == -1 && errno == EAGAIN)
{
perror("select");
continue;
}
if(select_result == -1)
{
perror("main:");
return 1;
}
if(rs232_device > 0)
{
if(FD_ISSET(rs232_device, &rset))
{
//bytes_read = read(rs232_device, rs232_buffer, sizeof(rs232_buffer));
bytes_read = rs232_read(rs232_device);
if(bytes_read > 0)
{
if(rs232_check_last_char('\n'))
{
bytes_read = rs232_unload_rx(rs232_buffer);
if(bytes_read > 0)
{
rs232_buffer[bytes_read] = 0;
token = strtok(rs232_buffer, "\n");
while(token != NULL)
{
sprintf(nmea_message, "%s\n", token);
nmea_parse(&parser, nmea_message, (int)strlen(nmea_message), &info);
if(it > 0)
{
printf("\033[15A");
}
else
it++;
printf("Time: %i/%i/%i %i:%i:%i.%i\n", info.utc.day, info.utc.mon + 1, info.utc.year + 1900, info.utc.hour, info.utc.min, info.utc.sec, info.utc.hsec);
printf("Signal: %i\n", info.sig);
printf("Operating Mode: %i\n", info.fix);
printf("Position Diluition of Precision: %f\n", info.PDOP);
printf("Horizontal Diluition of Precision: %f\n", info.HDOP);
printf("Vertical Diluition of Precisione: %f\n", info.VDOP);
printf("Latitude: %f\n", info.lat);
printf("Longitude: %f\n", info.lon);
printf("Elevation: %f m\n", info.elv);
printf("Speed: %f km/h\n", info.speed);
printf("Direction: %f degrees\n", info.direction);
printf("Magnetic variation degrees: %f\n", info.declination);
printf("\nSatellite: \tin view: %i\n\t\tin use: %i\n", info.satinfo.inview, info.satinfo.inuse);
token = strtok(NULL, "\n");
}
}
}
}
}
}
//int smask; /**< Mask specifying types of packages from which data have been obtained */
//nmeaTIME utc; /**< UTC of position */
//int sig; /**< GPS quality indicator (0 = Invalid; 1 = Fix; 2 = Differential, 3 = Sensitive) */
//int fix; /**< Operating mode, used for navigation (1 = Fix not available; 2 = 2D; 3 = 3D) */
//double PDOP; /**< Position Dilution Of Precision */
//double HDOP; /**< Horizontal Dilution Of Precision */
//double VDOP; /**< Vertical Dilution Of Precision */
//double lat; /**< Latitude in NDEG - +/-[degree][min].[sec/60] */
//double lon; /**< Longitude in NDEG - +/-[degree][min].[sec/60] */
//double elv; /**< Antenna altitude above/below mean sea level (geoid) in meters */
//double speed; /**< Speed over the ground in kilometers/hour */
//double direction; /**< Track angle in degrees True */
//double declination; /**< Magnetic variation degrees (Easterly var. subtracts from true course) */
//nmeaSATINFO satinfo; /**< Satellites information */
/*it++;
if(it > 6)
done = 1;
select_timeout.tv_sec = 1;
select_timeout.tv_usec = 0;*/
}
nmea_parser_destroy(&parser);
return 0;
}
int read_gps_nmea(int *fd, char *gps_rx_buffer, int buffer_size, nmeaINFO *info, nmeaPARSER *parser)
{
int ret = 1;
char c;
int start_flag = 0;
int found_cr = 0;
int rx_count = 0;
int gpsRxOverflow = 0;
struct pollfd fds;
fds.fd = *fd;
fds.events = POLLIN;
// NMEA or SINGLE-SENTENCE GPS mode
while (1) {
//check if the thread should terminate
if (terminate_gps_thread == true) {
// printf("terminate_gps_thread=%u ", terminate_gps_thread);
// printf("exiting mtk thread\n");
// fflush(stdout);
ret = 2;
break;
}
if (poll(&fds, 1, 1000) > 0) {
if (read(*fd, &c, 1) > 0) {
// detect start while acquiring stream
// printf("Char = %c\n", c);
if (!start_flag && (c == '$')) {
start_flag = 1;
found_cr = 0;
rx_count = 0;
} else if (!start_flag) { // keep looking for start sign
continue;
}
if (rx_count >= buffer_size) {
// The buffer is already full and we haven't found a valid NMEA sentence.
// Flush the buffer and note the overflow event.
gpsRxOverflow++;
start_flag = 0;
found_cr = 0;
rx_count = 0;
if (gps_verbose) printf("\t[gps] Buffer full\n");
} else {
// store chars in buffer
gps_rx_buffer[rx_count] = c;
rx_count++;
}
// look for carriage return CR
if (start_flag && c == 0x0d) {
found_cr = 1;
}
// and then look for line feed LF
if (start_flag && found_cr && c == 0x0a) {
// parse one NMEA line, use buffer up to rx_count
if (nmea_parse(parser, gps_rx_buffer, rx_count, info) > 0) {
ret = 0;
}
break;
}
} else {
break;
}
} else {
break;
}
}
// As soon as one NMEA message has been parsed, we break out of the loop and end here
return(ret);
}
