412 lines
8.7 KiB
C
412 lines
8.7 KiB
C
/*
|
|
* (C) 2010 by Andreas Eversberg <jolly@eversberg.eu>
|
|
*
|
|
* All Rights Reserved
|
|
*
|
|
* 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.
|
|
*
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <sys/file.h>
|
|
#include <termios.h>
|
|
#include <unistd.h>
|
|
#include <stdlib.h>
|
|
#include <errno.h>
|
|
#include <time.h>
|
|
#include <stdbool.h>
|
|
|
|
#ifdef _HAVE_GPSD
|
|
#include <gps.h>
|
|
#endif
|
|
|
|
#include <osmocom/core/utils.h>
|
|
|
|
#include <osmocom/bb/common/osmocom_data.h>
|
|
#include <osmocom/bb/common/logging.h>
|
|
#include <osmocom/bb/common/gps.h>
|
|
|
|
struct osmo_gps g = {
|
|
0,
|
|
GPS_TYPE_UNDEF,
|
|
#ifdef _HAVE_GPSD
|
|
"localhost",
|
|
"2947",
|
|
#endif
|
|
"/dev/ttyACM0",
|
|
0,
|
|
0,
|
|
0,
|
|
0,0
|
|
};
|
|
|
|
static struct osmo_fd gps_bfd;
|
|
|
|
#ifdef _HAVE_GPSD
|
|
|
|
static struct gps_data_t* gdata = NULL;
|
|
|
|
#if GPSD_API_MAJOR_VERSION >= 5
|
|
static struct gps_data_t _gdata;
|
|
#endif
|
|
|
|
static inline int compat_gps_read(struct gps_data_t *data)
|
|
{
|
|
/* API break in gpsd 6bba8b329fc7687b15863d30471d5af402467802 */
|
|
#if GPSD_API_MAJOR_VERSION >= 7 && GPSD_API_MINOR_VERSION >= 0
|
|
return gps_read(data, NULL, 0);
|
|
#elif GPSD_API_MAJOR_VERSION >= 5
|
|
return gps_read(data);
|
|
#else
|
|
return gps_poll(data);
|
|
#endif
|
|
}
|
|
|
|
int osmo_gpsd_cb(struct osmo_fd *bfd, unsigned int what)
|
|
{
|
|
struct tm *tm;
|
|
unsigned diff = 0;
|
|
|
|
g.valid = 0;
|
|
|
|
/* gps is offline */
|
|
#if GPSD_API_MAJOR_VERSION >= 9 && GPSD_API_MINOR_VERSION >= 0
|
|
if (gdata->online.tv_sec || gdata->online.tv_nsec)
|
|
#else
|
|
if (gdata->online)
|
|
#endif
|
|
goto gps_not_ready;
|
|
|
|
#if GPSD_API_MAJOR_VERSION >= 5
|
|
/* gps has no data */
|
|
if (gps_waiting(gdata, 500))
|
|
goto gps_not_ready;
|
|
#else
|
|
/* gps has no data */
|
|
if (gps_waiting(gdata))
|
|
goto gps_not_ready;
|
|
#endif
|
|
|
|
/* polling returned an error */
|
|
if (compat_gps_read(gdata))
|
|
goto gps_not_ready;
|
|
|
|
/* data are valid */
|
|
if (gdata->set & LATLON_SET) {
|
|
g.valid = 1;
|
|
#if GPSD_API_MAJOR_VERSION >= 9 && GPSD_API_MINOR_VERSION >= 0
|
|
g.gmt = gdata->fix.time.tv_sec;
|
|
#else
|
|
g.gmt = gdata->fix.time;
|
|
#endif
|
|
tm = localtime(&g.gmt);
|
|
diff = time(NULL) - g.gmt;
|
|
g.latitude = gdata->fix.latitude;
|
|
g.longitude = gdata->fix.longitude;
|
|
|
|
LOGP(DGPS, LOGL_INFO, " time=%02d:%02d:%02d %04d-%02d-%02d, "
|
|
"diff-to-host=%d, latitude=%do%.4f, longitude=%do%.4f\n",
|
|
tm->tm_hour, tm->tm_min, tm->tm_sec, tm->tm_year + 1900,
|
|
tm->tm_mday, tm->tm_mon + 1, diff,
|
|
(int)g.latitude,
|
|
(g.latitude - ((int)g.latitude)) * 60.0,
|
|
(int)g.longitude,
|
|
(g.longitude - ((int)g.longitude)) * 60.0);
|
|
}
|
|
|
|
return 0;
|
|
|
|
gps_not_ready:
|
|
LOGP(DGPS, LOGL_DEBUG, "gps is offline");
|
|
return -1;
|
|
}
|
|
|
|
int osmo_gpsd_open(void)
|
|
{
|
|
LOGP(DGPS, LOGL_INFO, "Connecting to gpsd at '%s:%s'\n", g.gpsd_host, g.gpsd_port);
|
|
|
|
#if GPSD_API_MAJOR_VERSION >= 5
|
|
if (gps_open(g.gpsd_host, g.gpsd_port, &_gdata) == -1)
|
|
gdata = NULL;
|
|
else
|
|
gdata = &_gdata;
|
|
#else
|
|
gdata = gps_open(g.gpsd_host, g.gpsd_port);
|
|
#endif
|
|
if (gdata == NULL) {
|
|
LOGP(DGPS, LOGL_ERROR, "Can't connect to gpsd\n");
|
|
return -1;
|
|
}
|
|
if (gdata->gps_fd < 0)
|
|
return gdata->gps_fd;
|
|
|
|
if (gps_stream(gdata, WATCH_ENABLE, NULL) == -1) {
|
|
LOGP(DGPS, LOGL_ERROR, "Error in gps_stream()\n");
|
|
return -1;
|
|
}
|
|
|
|
osmo_fd_setup(&gps_bfd, gdata->gps_fd, OSMO_FD_READ, osmo_gpsd_cb, NULL, 0);
|
|
osmo_fd_register(&gps_bfd);
|
|
|
|
return 0;
|
|
}
|
|
|
|
void osmo_gpsd_close(void)
|
|
{
|
|
if (gps_bfd.fd <= 0)
|
|
return;
|
|
|
|
LOGP(DGPS, LOGL_INFO, "Disconnecting from gpsd\n");
|
|
|
|
osmo_fd_unregister(&gps_bfd);
|
|
|
|
#if GPSD_API_MAJOR_VERSION >= 5
|
|
gps_stream(gdata, WATCH_DISABLE, NULL);
|
|
#endif
|
|
gps_close(gdata);
|
|
gps_bfd.fd = -1; /* -1 or 0 indicates: 'close' */
|
|
}
|
|
|
|
#endif
|
|
|
|
static struct termios gps_termios, gps_old_termios;
|
|
|
|
static int osmo_serialgps_line(char *line)
|
|
{
|
|
time_t gps_now, host_now;
|
|
struct tm *tm;
|
|
int32_t diff;
|
|
double latitude, longitude;
|
|
|
|
if (!!strncmp(line, "$GPGLL", 6))
|
|
return 0;
|
|
line += 7;
|
|
if (strlen(line) < 37)
|
|
return 0;
|
|
line[37] = '\0';
|
|
/* ddmm.mmmm,N,dddmm.mmmm,E,hhmmss.mmm,A */
|
|
|
|
/* valid position */
|
|
if (line[36] != 'A') {
|
|
LOGP(DGPS, LOGL_INFO, "%s (invalid)\n", line);
|
|
g.valid = 0;
|
|
return 0;
|
|
}
|
|
g.valid = 1;
|
|
|
|
/* time stamp */
|
|
gps_now = line[30] - '0';
|
|
gps_now += (line[29] - '0') * 10;
|
|
gps_now += (line[28] - '0') * 60;
|
|
gps_now += (line[27] - '0') * 600;
|
|
gps_now += (line[26] - '0') * 3600;
|
|
gps_now += (line[25] - '0') * 36000;
|
|
time(&host_now);
|
|
/* calculate the number of seconds the host differs from GPS */
|
|
diff = host_now % 86400 - gps_now;
|
|
if (diff < 0)
|
|
diff += 86400;
|
|
if (diff >= 43200)
|
|
diff -= 86400;
|
|
/* apply the "date" part to the GPS time */
|
|
gps_now = host_now - diff;
|
|
g.gmt = gps_now;
|
|
tm = localtime(&gps_now);
|
|
|
|
/* position */
|
|
latitude = (double)(line[0] - '0') * 10.0;
|
|
latitude += (double)(line[1] - '0');
|
|
latitude += (double)(line[2] - '0') / 6.0;
|
|
latitude += (double)(line[3] - '0') / 60.0;
|
|
latitude += (double)(line[5] - '0') / 600.0;
|
|
latitude += (double)(line[6] - '0') / 6000.0;
|
|
latitude += (double)(line[7] - '0') / 60000.0;
|
|
latitude += (double)(line[8] - '0') / 600000.0;
|
|
if (line[10] == 'S')
|
|
latitude = 0.0 - latitude;
|
|
g.latitude = latitude;
|
|
longitude = (double)(line[12] - '0') * 100.0;
|
|
longitude += (double)(line[13] - '0') * 10.0;
|
|
longitude += (double)(line[14] - '0');
|
|
longitude += (double)(line[15] - '0') / 6.0;
|
|
longitude += (double)(line[16] - '0') / 60.0;
|
|
longitude += (double)(line[18] - '0') / 600.0;
|
|
longitude += (double)(line[19] - '0') / 6000.0;
|
|
longitude += (double)(line[20] - '0') / 60000.0;
|
|
longitude += (double)(line[21] - '0') / 600000.0;
|
|
if (line[23] == 'W')
|
|
longitude = 360.0 - longitude;
|
|
g.longitude = longitude;
|
|
|
|
LOGP(DGPS, LOGL_DEBUG, "%s\n", line);
|
|
LOGP(DGPS, LOGL_INFO, " time=%02d:%02d:%02d %04d-%02d-%02d, "
|
|
"diff-to-host=%d, latitude=%do%.4f, longitude=%do%.4f\n",
|
|
tm->tm_hour, tm->tm_min, tm->tm_sec, tm->tm_year + 1900,
|
|
tm->tm_mday, tm->tm_mon + 1, diff,
|
|
(int)g.latitude,
|
|
(g.latitude - ((int)g.latitude)) * 60.0,
|
|
(int)g.longitude,
|
|
(g.longitude - ((int)g.longitude)) * 60.0);
|
|
return 0;
|
|
}
|
|
|
|
static int nmea_checksum(char *line)
|
|
{
|
|
uint8_t checksum = 0;
|
|
|
|
while (*line) {
|
|
if (*line == '$') {
|
|
line++;
|
|
continue;
|
|
}
|
|
if (*line == '*')
|
|
break;
|
|
checksum ^= *line++;
|
|
}
|
|
return (strtoul(line+1, NULL, 16) == checksum);
|
|
}
|
|
|
|
int osmo_serialgps_cb(struct osmo_fd *bfd, unsigned int what)
|
|
{
|
|
char buff[128];
|
|
static char line[128];
|
|
static int lpos = 0;
|
|
int i = 0, len;
|
|
|
|
len = read(bfd->fd, buff, sizeof(buff));
|
|
if (len <= 0) {
|
|
fprintf(stderr, "error reading GPS device (errno=%d)\n", errno);
|
|
return len;
|
|
}
|
|
while(i < len) {
|
|
if (buff[i] == 13) {
|
|
i++;
|
|
continue;
|
|
}
|
|
if (buff[i] == 10) {
|
|
line[lpos] = '\0';
|
|
lpos = 0;
|
|
i++;
|
|
if (!nmea_checksum(line))
|
|
fprintf(stderr, "NMEA checksum error\n");
|
|
else
|
|
osmo_serialgps_line(line);
|
|
continue;
|
|
}
|
|
line[lpos++] = buff[i++];
|
|
if (lpos == sizeof(line))
|
|
lpos--;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
int osmo_serialgps_open(void)
|
|
{
|
|
int baud = 0;
|
|
int fd;
|
|
|
|
if (gps_bfd.fd > 0)
|
|
return 0;
|
|
|
|
LOGP(DGPS, LOGL_INFO, "Open GPS device '%s'\n", g.device);
|
|
|
|
fd = open(g.device, O_RDONLY);
|
|
if (fd < 0)
|
|
return fd;
|
|
osmo_fd_setup(&gps_bfd, fd, OSMO_FD_READ, osmo_serialgps_cb, NULL, 0);
|
|
|
|
switch (g.baud) {
|
|
case 4800:
|
|
baud = B4800; break;
|
|
case 9600:
|
|
baud = B9600; break;
|
|
case 19200:
|
|
baud = B19200; break;
|
|
case 38400:
|
|
baud = B38400; break;
|
|
case 57600:
|
|
baud = B57600; break;
|
|
case 115200:
|
|
baud = B115200; break;
|
|
}
|
|
|
|
if (isatty(gps_bfd.fd))
|
|
{
|
|
/* get termios */
|
|
tcgetattr(gps_bfd.fd, &gps_old_termios);
|
|
tcgetattr(gps_bfd.fd, &gps_termios);
|
|
/* set baud */
|
|
if (baud) {
|
|
gps_termios.c_cflag |= baud;
|
|
cfsetispeed(&gps_termios, baud);
|
|
cfsetospeed(&gps_termios, baud);
|
|
}
|
|
if (tcsetattr(gps_bfd.fd, TCSANOW, &gps_termios))
|
|
printf("Failed to set termios for GPS\n");
|
|
}
|
|
|
|
osmo_fd_register(&gps_bfd);
|
|
|
|
return 0;
|
|
}
|
|
|
|
void osmo_serialgps_close(void)
|
|
{
|
|
if (gps_bfd.fd <= 0)
|
|
return;
|
|
|
|
LOGP(DGPS, LOGL_INFO, "Close GPS device\n");
|
|
|
|
osmo_fd_unregister(&gps_bfd);
|
|
|
|
if (isatty(gps_bfd.fd))
|
|
tcsetattr(gps_bfd.fd, TCSANOW, &gps_old_termios);
|
|
|
|
close(gps_bfd.fd);
|
|
gps_bfd.fd = -1; /* -1 or 0 indicates: 'close' */
|
|
}
|
|
|
|
void osmo_gps_init(void)
|
|
{
|
|
memset(&gps_bfd, 0, sizeof(gps_bfd));
|
|
}
|
|
|
|
int osmo_gps_open(void)
|
|
{
|
|
switch (g.gps_type) {
|
|
#ifdef _HAVE_GPSD
|
|
case GPS_TYPE_GPSD:
|
|
return osmo_gpsd_open();
|
|
#endif
|
|
case GPS_TYPE_SERIAL:
|
|
return osmo_serialgps_open();
|
|
|
|
default:
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
void osmo_gps_close(void)
|
|
{
|
|
switch (g.gps_type) {
|
|
#ifdef _HAVE_GPSD
|
|
case GPS_TYPE_GPSD:
|
|
return osmo_gpsd_close();
|
|
#endif
|
|
case GPS_TYPE_SERIAL:
|
|
return osmo_serialgps_close();
|
|
|
|
default:
|
|
return;
|
|
}
|
|
}
|