/*************************************************************************** * gps.c * * Copyright 2010 - Steffen Pohle * steffen@gulpe.de ****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. */ #include "osmroute.h" #include "gui.h" #ifdef HAVE_LOCALE_H #include #endif #include #include #include #include #include "gps.h" #include "memoryleak.h" #include "draw.h" #include "system.h" int gps_getdatatime (char *text); float gps_getlon (char *text); float gps_getlat (char *text); int gps_convert_line (char *line, struct gps_data *data); struct gps_data gpspos; struct gps_data gpsposvalid; #if !defined(__MINGW32CE__) && !defined(_WIN32_WCE) && !defined(__MINGW32__) char gpsdevice[GPS_DEVICELEN] = "serial:/dev/rfcomm0"; #else char gpsdevice[GPS_DEVICELEN] = "serial:com1,38400"; #endif int gpsflags = GPSF_LOG; char gpsdata[GPS_INBUFSIZE]; int gpsdatalen = 0; int gpsstatus = 0; /* status: 0 = not running, 1 = running, -1 = error */ time_t gpsstarttime; /* timestamp when we started to get gps informations */ int gpsdevtime = 0; /* gps device time */ int gpslasttype = GPS_T_NONE; /* type of the last opened device GPS_T_SERIAL, GPS_T_FILE */ struct gps_route *gpsroute; char logfile[LEN_FILENAME] = "gpslog.nmea"; /********************************************************************************************* * * gpsroute stuff */ void gpsroute_free (struct gps_route *gr) { if (gr == gpsroute) gpsroute = NULL; if (gr->data) ml_free (gr->data); if (gr->pdata) ml_free (gr->pdata); ml_free (gr); }; struct gps_route *gpsroute_new () { struct gps_route *gr = (struct gps_route*) ml_malloc (sizeof (struct gps_route)); gr->max = 0; gr->cnt = 0; gr->data = NULL; gr->pmax = 0; gr->pcnt = 0; gr->pdata = NULL; return gr; }; void gpsroute_addpos (struct gps_route *gr, float lon, float lat, float speed, time_t t) { if (gr == NULL) return; if (gr->cnt+1 >= gr->max) { struct gps_routedata *tmp; gr->max = gr->max + 1024; tmp = (struct gps_routedata*) ml_malloc (sizeof (struct gps_routedata) * (gr->max)); ml_addinfo (tmp, "gpsroute"); if (gr->data) { memcpy (tmp, gr->data, sizeof (struct gps_routedata) * gr->cnt); ml_free (gr->data); } gr->data = tmp; } gr->data[gr->cnt].lon = lon; gr->data[gr->cnt].lat = lat; gr->data[gr->cnt].speed = speed; gr->data[gr->cnt].time = t; gr->cnt++; }; void gpsroute_addpoi (struct gps_route *gr, float lon, float lat, char *text) { if (gr == NULL) return; if (gr->pcnt+1 >= gr->pmax) { struct gps_routepoidata *tmp; gr->pmax = gr->pmax + 1024; tmp = (struct gps_routepoidata*) ml_malloc (sizeof (struct gps_routepoidata) * (gr->pmax)); ml_addinfo (tmp, "gpspoiroute"); if (gr->pdata) { memcpy (tmp, gr->pdata, sizeof (struct gps_routepoidata) * gr->pcnt); ml_free (gr->pdata); } gr->pdata = tmp; } gr->pdata[gr->pcnt].lon = lon; gr->pdata[gr->pcnt].lat = lat; strncpy (gr->pdata[gr->pcnt].name, text, GPS_ROUTEPOILEN); gr->pcnt++; }; void gpsroute_exportgpx (struct gps_route *gr, char *filename) { int i; float olon = 0.0, olat = 0.0; char tt[128]; FILE *f; if (gr) { f = fopen (filename, "w"); fprintf (f, "\n"); fprintf (f, " \n"); fprintf (f, " noname\n"); for (i = 0; i < gr->cnt; i++) { if (i == 0 || (olon != gr->data[i].lon && olat != gr->data[i].lat)) { strftime (tt, 32, "%Y-%m-%dT%H:%M:%SZ", localtime(&gr->data[i].time)); fprintf (f, " \n", gr->data[i].lat, gr->data[i].lon, tt); olon = gr->data[i].lon; olat = gr->data[i].lat; } } fprintf (f, " \n"); for (i = 0; i < gr->pcnt; i++) { fprintf (f, " \n", gr->pdata[i].lat, gr->pdata[i].lon); fprintf (f, " %s\n", gr->pdata[i].name); fprintf (f, " \n"); } fprintf (f, " \n"); fclose (f); } }; /* void gpsroute_saveroute (struct gps_route *gr, char *filename) { int i; float olon = 0.0, olat = 0.0; FILE *f; char tt[32]; if (gr) { f = fopen (filename, "w"); for (i = 0; i < gr->cnt; i++) { if (i == 0 || (olon != gr->data[i].lon && olat != gr->data[i].lat)) { strftime (tt, 32, "%Y-%m-%dT%H:%M:%SZ", localtime(&gr->data[i].time)); fprintf (f, "%s lon:%7.4f lat:%7.4f\n", tt, gr->data[i].lon, gr->data[i].lat); olon = gr->data[i].lon; olat = gr->data[i].lat; } } fclose (f); } }; */ /********************************************************************************************* * * variables and functions needed by all gps_file_device_ functions. * gps_file_device* will be used to simulate gps log files, this makes it possible to playback * driving on the street without going out. * */ char *gps_file_buffer = NULL; // buffer to keep the data inside int gps_file_buflen; // bytes inside the buffer int gps_file_fd = 0; // file descriptor int gps_file_size = 0; // file size char gps_file_fn[GPS_DEVICELEN]; // currently open file /* the times will be used to detect when we need to send another line, so we can simulate the * whole gps stream also with simulated time */ time_t gps_file_hosttime; // daytime when we started to read the log file (in seconds) int gps_file_filetime; // daytime of the first time found (we use $GPRMC for time) (in seconds) int gps_file_state; // simulation? int gps_file_get_state () { return gps_file_state; }; void gps_file_set_state (int state) { if (state == GPSF_RUN || state == GPSF_INIT) gps_file_state = GPSF_INIT; else gps_file_state = GPSF_PAUSE; }; void gps_file_set_pos (int newpos) { if (newpos >= gps_file_size) lseek (gps_file_fd, 0, SEEK_END); else if (newpos < 0) lseek (gps_file_fd, 0, SEEK_SET); else lseek (gps_file_fd, newpos, SEEK_SET); gps_file_state = GPSF_INIT; }; int gps_file_get_pos () { int i; i = lseek (gps_file_fd, 0, SEEK_CUR); return i; }; int gps_file_get_size () { return gps_file_size; }; int gps_file_device_open () { /* copy filename */ strncpy (gps_file_fn, gpsdevice+5, GPS_DEVICELEN); #if defined(__MINGW32CE__) || defined(_WIN32_WCE) || defined(__MINGW32__) gps_file_fd = open (gps_file_fn, O_RDONLY | _O_BINARY); #else gps_file_fd = open (gps_file_fn, O_RDONLY); #endif if (gps_file_fd == -1) { d_printf ("%s:%d gps_file_device_open could not open file:%s gpsdevice:%s", __FILE__, __LINE__, gps_file_fn, gpsdevice); return 0; } if (gps_file_buffer) ml_free (gps_file_buffer); gps_file_size = lseek (gps_file_fd, 0, SEEK_END); lseek (gps_file_fd, 0, SEEK_SET); gps_file_buffer = (char *) ml_malloc (GPS_INBUFSIZE); gps_file_state = GPSF_INIT; gpswin_show (); return 1; }; void gps_file_device_close () { if (gps_file_fd > 0) close (gps_file_fd); gps_file_size = 0; gps_file_fd = 0; gps_file_state = GPSF_NONE; gps_file_fn[0] = 0; if (gps_file_buffer) ml_free (gps_file_buffer); gps_file_buffer = NULL; gps_file_buflen = 0; }; int gps_file_device_read (char *ptr, int ptrsize) { int i, nr = 0, datatime; time_t curtime; int eof = 0; /* * test if we are in state init */ if (gps_file_state == GPSF_INIT) { gps_file_hosttime = time (NULL); gps_file_filetime = -1; /* we still don't have any data to get the time */ gps_file_buflen = 0; gps_file_state = GPSF_RUN; } if (gps_file_state == GPSF_RUN) { /* * refill the buffer */ if (gps_file_buflen < GPS_INBUFSIZE) { i = read (gps_file_fd, gps_file_buffer + gps_file_buflen, GPS_INBUFSIZE-gps_file_buflen); gps_file_buflen += i; if (i == 0) eof = 1; } /* * check if we can send a new line */ curtime = time (NULL); if (strncmp (gps_file_buffer, "$GPRMC,", 7) == 0) { datatime = gps_getdatatime (gps_file_buffer+7); if (gps_file_filetime == -1) gps_file_filetime = datatime; } if (strncmp (gps_file_buffer, "$GPRMC,", 7) != 0 || (curtime-gps_file_hosttime >= datatime-gps_file_filetime)) { for (i = 0; i < (ptrsize-1) && i < gps_file_buflen; i++) { ptr[i] = gps_file_buffer[i]; if (gps_file_buffer[i] == '\r' || gps_file_buffer[i] == '\n') { i++; ptr[i] = 0; break; } } /* move the leftover inside the buffer to the beginning */ if (i >= 0) { if (gps_file_buflen > 0) { memmove (gps_file_buffer, gps_file_buffer+i, GPS_INBUFSIZE-i); gps_file_buflen = gps_file_buflen - i; } nr = i; } } if (gps_file_buflen < GPS_INBUFSIZE && eof) gps_file_state = GPSF_END; } return nr; }; /******************************************************************************************** * convert text "173436.000" into time in seconds form 00:00:00 */ int gps_getdatatime (char *text) { char c_hms[3][3]; /* hours, mins, secs */ int i; for (i = 0; i < 2; i++) { c_hms[0][i] = text[i]; c_hms[1][i] = text[2+i]; c_hms[2][i] = text[4+i]; } c_hms[0][i] = '\0'; c_hms[1][i] = '\0'; c_hms[2][i] = '\0'; return (atoi (c_hms[0]) * 3600 + atoi (c_hms[1]) * 60 + atoi (c_hms[2])); }; float gps_getlon (char *text) { // GGGMM.mmmm float ret; char g[4]; memcpy (g, text, 3); g[3] = 0; ret = atof (text+3)/60.0 + (float) atoi (g); return ret; }; float gps_getlat (char *text) { // GGMM.mmmm float ret; char g[3]; memcpy (g, text, 2); g[2] = 0; ret = atof (text+2)/60.0 + (float) atoi (g); return ret; }; /******************************************************************************************** * * other gps stuff * */ /* * return type of currently set up gps device */ int gps_get_devicetype () { if (strncmp (gpsdevice, "serial:", 7) == 0) return GPS_T_SERIAL; else if (strncmp (gpsdevice, "file:", 5) == 0) return GPS_T_FILE; else return GPS_T_NONE; }; /* * setup the new device and reinit device if it was running */ void gps_set_device (char *device) { int oldstatus = gpsstatus; d_printf ("%s:%d gps_set_device (%s)", __FILE__, __LINE__, device); gps_stop (); strncpy (gpsdevice, device, GPS_DEVICELEN); if (oldstatus == 1 || oldstatus == -1) gps_start (); }; char *gps_get_device () { d_printf ("%s:%d gps_get_device (%s)", __FILE__, __LINE__, gpsdevice); return gpsdevice; }; void gps_start () { int error = 0; char fn[LEN_FILENAME]; d_printf ("%s:%d gps_start device(%s)", __FILE__, __LINE__, gpsdevice); gpsstarttime = time (NULL); strftime (fn, LEN_FILENAME, "%F-%H-%M%S.nmea", localtime(&gpsstarttime)); snprintf (logfile, LEN_FILENAME, "%s%s", cfg.logpath, fn); d_printf ("gps-logfile set to:%s", logfile); gpsdevtime = -1; if (gpsroute == NULL) gpsroute = gpsroute_new(); else { gpsroute->cnt = 0; gpsroute->pcnt = 0; } gpslasttype = gps_get_devicetype (); if (gpslasttype == GPS_T_SERIAL) error = gps_serial_device_open (); else if (gpslasttype == GPS_T_FILE) error = gps_file_device_open (); else error = 0; if (error) { d_printf ("%s:%d gps_start : ok", __FILE__, __LINE__); gpsstatus = 1; draw_set_flag (DRAW_GPS); } else { d_printf ("%s:%d gps_start : error", __FILE__, __LINE__); gpsstatus = -1; draw_del_flag (DRAW_GPS); } gpsdatalen = 0; }; void gps_stop () { d_printf ("%s:%d gps_stop", __FILE__, __LINE__); if (gpslasttype == GPS_T_SERIAL) gps_serial_device_close (); else if (gpslasttype == GPS_T_FILE) gps_file_device_close (); gpsstatus = 0; gpsdatalen = 0; draw_del_flag (DRAW_GPS); d_printf ("gps is clean now."); }; /* * convert the givin line into the gps_data struct, the valid flag will * not been deleted first, only if there is no active flag */ int gps_convert_line (char *line, struct gps_data *data) { char *pos; float lon, lat; int i; /* reset poi data */ data->poi.name[0] = 0; /* kick the checksum away */ if ((pos = strchr (line, '*'))) { pos[0] = 0; } /* * read the gps line */ pos = line; if (strncmp (pos, "$GPRMC", 6) == 0) { data->time = gpsstarttime; if ((pos = strchr (pos, ',')) == NULL) return 0; // Time pos++; i = gps_getdatatime (pos); if (gpsdevtime == -1) gpsdevtime = i; if (gpsdevtime > i) i = i + (3600*24); data->time += (i - gpsdevtime); if ((pos = strchr (pos, ',')) == NULL) return 0; // Active pos++; if (pos[0] == 'A') data->valid = 1; if ((pos = strchr (pos, ',')) == NULL) return 0; // North-South pos++; data->lat = gps_getlat (pos); if ((pos = strchr (pos, ',')) == NULL) return 0; // N|S pos++; if (pos[0] == 'S') data->lat = -data->lat; else if (pos[0] != 'N') return 0; if ((pos = strchr (pos, ',')) == NULL) return 0; // East-West pos++; data->lon = gps_getlon (pos); if ((pos = strchr (pos, ',')) == NULL) return 0; // E|W pos++; if (pos[0] == 'W') data->lon = -data->lon; else if (pos[0] != 'E') return 0; if ((pos = strchr (pos, ',')) == NULL) return 0; // Speed pos++; data->speed = atof (pos) * GPS_KNOTEN2KMH; if ((pos = strchr (pos, ',')) == NULL) return 0; // direction pos++; if (data->speed > 1.5) data->direction = atof (pos); } else if (strncmp (pos, "$POI,", 5) == 0) { if ((pos = strchr (pos, ',')) == NULL) return 0; pos++; lon = atof (pos); if ((pos = strchr (pos, ',')) == NULL) return 0; pos++; lat = atof (pos); if ((pos = strchr (pos, ',')) == NULL) return 0; pos++; strncpy (data->poi.name, pos, GPS_ROUTEPOILEN); data->poi.lon = lon; data->poi.lat = lat; } else if (strncmp (pos, "$GPGGA", 6) == 0) { data->time = gpsstarttime; if ((pos = strchr (pos, ',')) == NULL) return 0; // Time pos++; if ((pos = strchr (pos, ',')) == NULL) return 0; // North-South pos++; data->lat = gps_getlat (pos); if ((pos = strchr (pos, ',')) == NULL) return 0; // N|S pos++; if (pos[0] == 'S') data->lat = -data->lat; if ((pos = strchr (pos, ',')) == NULL) return 0; // East-West pos++; data->lon = gps_getlon (pos); if ((pos = strchr (pos, ',')) == NULL) return 0; // E|W pos++; if (pos[0] == 'W') data->lon = -data->lon; if ((pos = strchr (pos, ',')) == NULL) return 0; // Quallity pos++; data->quality = atoi (pos); if ((pos = strchr (pos, ',')) == NULL) return 0; // Numer of Satelites pos++; data->numofsatelites = atoi (pos); if ((pos = strchr (pos, ',')) == NULL) return 0; // HDOP pos++; data->hdop = atof (pos); if ((pos = strchr (pos, ',')) == NULL) return 0; // Height in m pos++; data->height = atof (pos); } else if (strncmp (pos, "$GPGSV", 6) == 0) { int n, satnr; data->time = gpsstarttime; if ((pos = strchr (pos, ',')) == NULL) return 0; // max data blocks pos++; if ((pos = strchr (pos, ',')) == NULL) return 0; // current data block pos++; satnr = atoi (pos); if (satnr == 1) for (n = 0; n < 32; n++) { data->sat[n].deg_elev = 0; data->sat[n].deg_north = 0; data->sat[n].quality = 0; } if ((pos = strchr (pos, ',')) == NULL) return 0; // number of sats pos++; if ((pos = strchr (pos, ',')) == NULL) return 0; // first data n = 0; while (pos && n < 4) { n++; pos++; satnr = atoi (pos); // satnr; if (satnr > 32 || satnr < 0) { d_printf ("%s:%d satnr invlid: %d Line:'%s'", __FILE__, __LINE__, satnr, line); return 0; } if ((pos = strchr (pos, ',')) == NULL) return 0; // elev in deg pos++; if (satnr != 0) data->sat[satnr-1].deg_elev = atoi (pos); if ((pos = strchr (pos, ',')) == NULL) return 0; // position in deg from north pos++; if (satnr != 0) data->sat[satnr-1].deg_north = atoi (pos); if ((pos = strchr (pos, ',')) == NULL) return 0; // quality pos++; if (pos[0] != '*' && satnr != 0) data->sat[satnr-1].quality = atoi (pos); pos = strchr (pos, ','); // next data } } return 1; }; /* * returns the pointer of the last know datablock (actually only a pointer to position) * or returns NULL if gps is not started correctly */ struct gps_data *gps_loop () { int dataread = 0, i; int to = GPS_LOOP_TIMEOUT; char line[2*GPS_LINELEN]; time_t call; #ifdef HAVE_LOCALE_H setlocale(LC_ALL, "C"); #endif gpspos.valid = gpsposvalid.valid = 0; call = time (NULL); /* * read all completed lines */ do { if (time (NULL) - call > 3) d_printf ("time??????"); /* * check if gps is open and fillup the buffer */ if (gpsstatus <= 0) return NULL; if (gpslasttype == GPS_T_SERIAL) dataread = gps_serial_device_read (gpsdata + gpsdatalen, GPS_INBUFSIZE - gpsdatalen); else if (gpslasttype == GPS_T_FILE) dataread = gps_file_device_read (gpsdata + gpsdatalen, GPS_INBUFSIZE - gpsdatalen); else dataread = -1; if (dataread < 0) { gps_stop(); gpsstatus = -1; } else if (dataread+gpsdatalen > GPS_INBUFSIZE) { d_printf ("%s:%d gps_loop: ERROR datapos(%d) > GPS_INBUFSIZE(%d)", __FILE__, __LINE__, gpsdatalen + dataread, GPS_INBUFSIZE); gpsdatalen = GPS_INBUFSIZE; gps_stop (); } else gpsdatalen += dataread; for (i = 0; i < GPS_LINELEN*2 && i < gpsdatalen && gpsdata[i] != '\n' && gpsdata[i] != '\r' && gpsdata[i] != '\0'; i++) { line[i] = gpsdata[i]; } if (gpsdata[i] == '\n' || gpsdata[i] == '\r' || gpsdata[i] == '\0') { if (gpsdatalen - (i+1) > 0) { memmove (gpsdata, gpsdata+i+1, gpsdatalen - (i+1)); gpsdatalen = gpsdatalen - (i+1); } else gpsdatalen = 0; } else if (i == gpsdatalen || i >= GPS_LINELEN*2) { break; } line[i] = 0; /* write into logfile */ if (i > 0) { if (gpslasttype != GPS_T_FILE && gpsflags & GPSF_LOG) gps_save_log (logfile, line); if (gps_convert_line (line, &gpspos) == 0) gpspos.valid = 0; else { if (gpspos.quality == 1) gpspos.valid = 1; } if (gpspos.valid == 1) { gpsposvalid = gpspos; gpsroute_addpos (gpsroute, gpspos.lon, gpspos.lat, gpspos.speed, gpspos.time); } } } while (gpsstatus > 0 && gpsdatalen > 0 && --to > 0); #ifdef HAVE_LOCALE_H setlocale(LC_ALL, NULL); #endif if (gpsposvalid.valid == 1) return &gpsposvalid; else return NULL; }; /********************************************************************* * returns TRUE (1) if gps is running */ int gps_isrunning () { return (gpsstatus); }; struct gps_data *gps_getposition () { if (gpsposvalid.valid == 1) return &gpsposvalid; else return &gpspos; }; /* * open logfile and add one line to the end. * If fname==NULL the file name stored in logfile will be used. */ void gps_save_log (char *fname, char *text) { int f; if (fname == NULL && gpsstatus == 1 && logfile[0] != 0) f = open (logfile, O_RDWR | O_CREAT, 0644); else f = open (fname, O_RDWR | O_CREAT, 0644); if (f) { lseek (f, 0, SEEK_END); write (f, text, strlen (text)); #if !defined(__MINGW32CE__) && !defined(_WIN32_WCE) && !defined(__MINGW32__) write (f, "\n", 1); #else write (f, "\n\r", 2); #endif close (f); } }; void gps_load_log (char *fn) { float minlon, minlat; float maxlon, maxlat; int start; struct gps_data *pos; char oldgps_dev[GPS_DEVICELEN]; gps_stop (); strncpy (oldgps_dev, gpsdevice, GPS_DEVICELEN); snprintf (gpsdevice, GPS_DEVICELEN, "file:%s", fn); gps_start (); start = 1; while (gpsstatus == 1) { pos = gps_loop (); if (pos) if (pos->valid) { if (start) { minlon = maxlon = pos->lon; minlat = maxlat = pos->lat; start = 0; } else if (pos->lon < minlon) minlon = pos->lon; else if (pos->lon > maxlon) maxlon = pos->lon; else if (pos->lat < minlat) minlat = pos->lat; else if (pos->lat > maxlat) maxlat = pos->lat; } } gps_stop (); strncpy (gpsdevice, oldgps_dev, GPS_DEVICELEN); if (start == 0) { view_lon = minlon+(maxlon-minlon)/2.0; view_lat = minlat+(maxlat-minlat)/2.0; draw(); } };