pud: add gpsd functions
authorFerry Huberts <ferry.huberts@pelagic.nl>
Thu, 28 Jul 2016 19:51:44 +0000 (21:51 +0200)
committerFerry Huberts <ferry.huberts@pelagic.nl>
Fri, 29 Jul 2016 11:40:01 +0000 (13:40 +0200)
This makes pud unsupported on android

Signed-off-by: Ferry Huberts <ferry.huberts@pelagic.nl>
Makefile
lib/pud/Makefile
lib/pud/src/configuration.c
lib/pud/src/configuration.h
lib/pud/src/gpsdclient.c [new file with mode: 0644]
lib/pud/src/gpsdclient.h [new file with mode: 0644]

index 86b3f5c..60f907a 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -230,7 +230,7 @@ ifeq ($(OS),win32)
 SUBDIRS := dot_draw httpinfo info jsoninfo mini netjson pgraph secure txtinfo
 else
 ifeq ($(OS),android)
-SUBDIRS := arprefresh bmf dot_draw dyn_gw dyn_gw_plain httpinfo info jsoninfo mdns mini nameservice netjson p2pd pgraph pud secure sgwdynspeed txtinfo watchdog
+SUBDIRS := arprefresh bmf dot_draw dyn_gw dyn_gw_plain httpinfo info jsoninfo mdns mini nameservice netjson p2pd pgraph secure sgwdynspeed txtinfo watchdog
 else
 SUBDIRS := dot_draw httpinfo info jsoninfo mini nameservice netjson pgraph secure txtinfo watchdog
 endif
index 3fe9ac4..21ffc7b 100644 (file)
@@ -65,10 +65,10 @@ LIBRARY_LIB = $(LIBRARY_PATH)/lib
 NMEALIB_LIB_STATIC = $(NMEALIB_LIB)/lib$(NMEALIB_NAME).a
 
 ifeq ($(PUD_NMEALIB_STATICALLY_LINKED),)
-LIBS += -L $(NMEALIB_LIB) -L $(LIBRARY_LIB) -l$(NMEALIB_NAME) -lm -l$(LIBRARY_NAME)
+LIBS += -L $(NMEALIB_LIB) -L $(LIBRARY_LIB) -l$(NMEALIB_NAME) -lm -l$(LIBRARY_NAME) -lgps
 else
 # libnmea is statically linked
-LIBS += -L $(LIBRARY_LIB) -lm -l$(LIBRARY_NAME)
+LIBS += -L $(LIBRARY_LIB) -lm -l$(LIBRARY_NAME) -lgps
 endif
 
 
index 8185805..52ddce2 100644 (file)
@@ -1678,7 +1678,7 @@ int setGpsdUse(const char *value, void *data __attribute__ ((unused)), set_plugi
 static char gpsd[PATH_MAX];
 
 /** The gpsd plugin parameter as gpsd source spec */
-static struct fixsource_t gpsdSource;
+static GpsDaemon gpsDaemon;
 
 /** True when the gpsd is set */
 static bool gpsdSet = false;
@@ -1687,62 +1687,12 @@ static bool gpsdSet = false;
  @return
  The gpsd plugin parameter (NULL when not set)
  */
-struct fixsource_t *getGpsd(void) {
+GpsDaemon *getGpsd(void) {
   if (!gpsdSet || !gpsdUse) {
     return NULL ;
   }
 
-  return &gpsdSource;
-}
-
-/* standard parsing of a GPS data source spec */
-static void gpsd_source_spec(const char *arg, struct fixsource_t *source) {
-  source->server = DEFAULT_GPSD_HOST;
-  source->port = DEFAULT_GPSD_PORT;
-  source->device = NULL;
-
-  if (arg != NULL && *arg) {
-    char *colon1;
-    const char *skipto;
-    char *rbrk;
-
-    strcpy(source->spec, arg);
-
-    skipto = source->spec;
-    if ((*skipto == '[') //
-        && ((rbrk = strchr(skipto, ']')) != NULL)) {
-      skipto = rbrk;
-    }
-    colon1 = strchr(skipto, ':');
-
-    if (colon1 != NULL) {
-      char *colon2;
-
-      *colon1 = '\0';
-      if (colon1 != source->spec) {
-        source->server = source->spec;
-      }
-
-      source->port = colon1 + 1;
-      colon2 = strchr(source->port, ':');
-      if (colon2 != NULL) {
-        *colon2 = '\0';
-        source->device = colon2 + 1;
-      }
-    } else if (strchr(source->spec, '/') != NULL) {
-      source->device = source->spec;
-    } else {
-      source->server = source->spec;
-    }
-  }
-
-  if (*source->server == '[') {
-    char *rbrk = strchr(source->server, ']');
-    ++source->server;
-    if (rbrk != NULL) {
-      *rbrk = '\0';
-    }
-  }
+  return &gpsDaemon;
 }
 
 int setGpsd(const char *value, void *data __attribute__ ((unused)), set_plugin_parameter_addon addon __attribute__ ((unused))) {
@@ -1764,7 +1714,7 @@ int setGpsd(const char *value, void *data __attribute__ ((unused)), set_plugin_p
   }
 
   strcpy((char *) &gpsd[0], value);
-  gpsd_source_spec(gpsd, &gpsdSource);
+  gpsdParseSourceSpec(gpsd, &gpsDaemon);
   gpsdSet = true;
 
   return false;
index a19306e..1e7668d 100644 (file)
@@ -47,6 +47,7 @@
 #define _PUD_CONFIGURATION_H_
 
 /* Plugin includes */
+#include "gpsdclient.h"
 
 /* OLSR includes */
 #include "olsrd_plugin.h"
@@ -445,13 +446,6 @@ setUseLoopback(const char *value, void *data, set_plugin_parameter_addon addon);
  * gpsd
  */
 
-struct fixsource_t {
-  char spec[PATH_MAX]; /* working space, will be modified */
-  const char *server;
-  const char *port;
-  const char *device;
-};
-
 /** The name of the gpsd update interval plugin parameter */
 #define PUD_GPSD_USE_NAME  "gpsdUse"
 
@@ -464,16 +458,7 @@ int setGpsdUse(const char *value, void *data, set_plugin_parameter_addon addon);
 /** The name of the gpsd plugin parameter */
 #define PUD_GPSD_NAME  "gpsd"
 
-/** The default gpsd host */
-#define DEFAULT_GPSD_HOST "localhost"
-
-/** The default gpsd port */
-#define DEFAULT_GPSD_PORT "2947"
-
-/** The default value of the gpsd plugin parameter */
-#define PUD_GPSD_DEFAULT (DEFAULT_GPSD_HOST ":" DEFAULT_GPSD_PORT)
-
-struct fixsource_t *getGpsd(void);
+GpsDaemon *getGpsd(void);
 int setGpsd(const char *value, void *data, set_plugin_parameter_addon addon);
 
 /*
diff --git a/lib/pud/src/gpsdclient.c b/lib/pud/src/gpsdclient.c
new file mode 100644 (file)
index 0000000..334c23f
--- /dev/null
@@ -0,0 +1,619 @@
+/*
+ * The olsr.org Optimized Link-State Routing daemon (olsrd)
+ *
+ * (c) by the OLSR project
+ *
+ * See our Git repository to find out who worked on this file
+ * and thus is a copyright holder on it.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in
+ *   the documentation and/or other materials provided with the
+ *   distribution.
+ * * Neither the name of olsr.org, olsrd nor the names of its
+ *   contributors may be used to endorse or promote products derived
+ *   from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Visit http://www.olsr.org for more information.
+ *
+ * If you find this software useful feel free to make a donation
+ * to the project. For more information see the website or contact
+ * the copyright holders.
+ *
+ */
+
+#include "gpsdclient.h"
+
+#include "configuration.h"
+
+#include <nmealib/nmath.h>
+#include <nmealib/sentence.h>
+#include <errno.h>
+#include <gps.h>
+#include <math.h>
+#include <string.h>
+#include <syslog.h>
+
+#if GPSD_API_MAJOR_VERSION < 6
+  #if GPSD_API_MINOR_VERSION < 1
+    #define GPSD_WHEEZY
+  #else
+    #define GPSD_JESSIE
+  #endif
+#else
+  #define GPSD_NEW
+#endif
+
+/*
+ * Loggers
+ */
+
+/* log gpsd log messages*/
+static void gpsdLog(const char *s) {
+  syslog(LOG_INFO, "gpsd log: %s", s);
+}
+
+/* log gpsd errors */
+static void gpsdError(const char *s) {
+  syslog(LOG_ERR, "gpsd error: %s", s);
+}
+
+/* standard parsing of a GPS data source spec */
+void gpsdParseSourceSpec(char *arg, GpsDaemon *gpsDaemon) {
+  if (!arg //
+      || !gpsDaemon) {
+    return;
+  }
+
+  gpsDaemon->source.server = (char *) (unsigned long) DEFAULT_GPSD_HOST;
+  gpsDaemon->source.port = (char *) (unsigned long) DEFAULT_GPSD_PORT;
+  gpsDaemon->source.device = NULL;
+
+  if (*arg) {
+    char *colon1;
+    const char *skipto;
+    char *rbrk;
+
+    strncpy(gpsDaemon->source.spec, arg, PATH_MAX);
+    gpsDaemon->source.spec[PATH_MAX - 1] = '\0';
+
+    skipto = gpsDaemon->source.spec;
+    if ((*skipto == '[') //
+        && ((rbrk = strchr(skipto, ']')) != NULL )) {
+      skipto = rbrk;
+    }
+    colon1 = strchr(skipto, ':');
+
+    if (colon1 != NULL) {
+      char *colon2;
+
+      *colon1 = '\0';
+      if (colon1 != gpsDaemon->source.spec) {
+        gpsDaemon->source.server = gpsDaemon->source.spec;
+      }
+
+      gpsDaemon->source.port = colon1 + 1;
+      colon2 = strchr(gpsDaemon->source.port, ':');
+      if (colon2 != NULL) {
+        *colon2 = '\0';
+        gpsDaemon->source.device = colon2 + 1;
+      }
+    } else if (strchr(gpsDaemon->source.spec, '/') != NULL) {
+      gpsDaemon->source.device = gpsDaemon->source.spec;
+    } else {
+      gpsDaemon->source.server = gpsDaemon->source.spec;
+    }
+  }
+
+  if (*gpsDaemon->source.server == '[') {
+    char *rbrk = strchr(gpsDaemon->source.server, ']');
+    ++gpsDaemon->source.server;
+    if (rbrk != NULL) {
+      *rbrk = '\0';
+    }
+  }
+}
+
+bool gpsdConnect(GpsDaemon *gpsd, struct gps_data_t *gpsData, struct GpsdConnectionState *connectionTracking) {
+  bool disconnectFirst;
+
+  if (!gpsd //
+      || !gpsData //
+      || !connectionTracking) {
+    return false;
+  }
+
+  disconnectFirst = connectionTracking->connected;
+
+  if (disconnectFirst) {
+    gpsdDisconnect(gpsData, connectionTracking);
+  }
+
+  gpsData->gps_fd = -1;
+  if (gps_open(gpsd->source.server, gpsd->source.port, gpsData)) {
+    if (!connectionTracking->connectionFailureReported) {
+      syslog(LOG_WARNING, "%sonnecting to gpsd on %s:%s failed, starting retries", disconnectFirst ? "Rec" : "C", gpsd->source.server, gpsd->source.port);
+      connectionTracking->connectionFailureReported = true;
+    }
+    connectionTracking->retryCount++;
+    return false;
+  }
+
+  if (connectionTracking->connectionFailureReported) {
+    syslog(LOG_WARNING, "%sonnecting to gpsd on %s:%s successful after %llu retr%s", disconnectFirst ? "Rec" : "C", gpsd->source.server, gpsd->source.port,
+        connectionTracking->retryCount, connectionTracking->retryCount == 1 ? "y" : "ies");
+    connectionTracking->connectionFailureReported = false;
+  }
+  connectionTracking->retryCount = 0;
+
+  if (gpsd->source.device) {
+    gpsd->gpsdStreamFlags |= WATCH_DEVICE;
+  }
+
+  /* instruct gpsd which data to receive */
+  gps_stream(gpsData, gpsd->gpsdStreamFlags, gpsd->source.device);
+
+  return true;
+}
+
+void gpsdDisconnect(struct gps_data_t *gpsdata, struct GpsdConnectionState *connectionTracking) {
+  if (!gpsdata //
+      || !connectionTracking) {
+    return;
+  }
+
+  if (gpsdata->gps_fd >= 0) {
+    gps_close(gpsdata);
+    gpsdata->gps_fd = -1;
+    syslog(LOG_INFO, "Closed gpsd connection");
+  }
+
+  memset(connectionTracking, 0, sizeof(*connectionTracking));
+}
+
+/**
+ * Convert true heading to magnetic.  Taken from the Aviation
+ * Formulary v1.43.  Valid to within two degrees within the
+ * continiental USA except for the following airports: MO49 MO86 MO50
+ * 3K6 02K and KOOA.  AK correct to better than one degree.  Western
+ * Europe correct to within 0.2 deg.
+ *
+ * If you're not in one of these areas, I apologize, I don't have the
+ * math to compute your varation.  This is obviously extremely
+ * floating-point heavy, so embedded people, beware of using.
+ *
+ * Note that there are issues with using magnetic heading.  This code
+ * does not account for the possibility of travelling into or out of
+ * an area of valid calculation beyond forcing the magnetic conversion
+ * off.  A better way to communicate this to the user is probably
+ * desirable (in case the don't notice the subtle change from "(mag)"
+ * to "(true)" on their display).
+ */
+float true2magnetic(double lat, double lon, double heading) {
+  /* Western Europe */
+  if ((lat > 36.0) && (lat < 68.0) && (lon > -10.0) && (lon < 28.0)) {
+    heading = (10.4768771667158 - (0.507385322418858 * lon) + (0.00753170031703826 * pow(lon, 2)) - (1.40596203924748e-05 * pow(lon, 3))
+        - (0.535560699962353 * lat) + (0.0154348808069955 * lat * lon) - (8.07756425110592e-05 * lat * pow(lon, 2)) + (0.00976887198864442 * pow(lat, 2))
+        - (0.000259163929798334 * lon * pow(lat, 2)) - (3.69056939266123e-05 * pow(lat, 3)) + heading);
+  }
+  /* USA */
+  else if ((lat > 24.0) && (lat < 50.0) && (lon > 66.0) && (lon < 125.0)) {
+    lon = 0.0 - lon;
+    heading = ((-65.6811) + (0.99 * lat) + (0.0128899 * pow(lat, 2)) - (0.0000905928 * pow(lat, 3)) + (2.87622 * lon) - (0.0116268 * lat * lon)
+        - (0.00000603925 * lon * pow(lat, 2)) - (0.0389806 * pow(lon, 2)) - (0.0000403488 * lat * pow(lon, 2)) + (0.000168556 * pow(lon, 3)) + heading);
+  }
+  /* AK */
+  else if ((lat > 54.0) && (lon > 130.0) && (lon < 172.0)) {
+    lon = 0.0 - lon;
+    heading = (618.854 + (2.76049 * lat) - (0.556206 * pow(lat, 2)) + (0.00251582 * pow(lat, 3)) - (12.7974 * lon) + (0.408161 * lat * lon)
+        + (0.000434097 * lon * pow(lat, 2)) - (0.00602173 * pow(lon, 2)) - (0.00144712 * lat * pow(lon, 2)) + (0.000222521 * pow(lon, 3)) + heading);
+  } else {
+    /* We don't know how to compute magnetic heading for this
+     * location. */
+    heading = NAN;
+  }
+
+  /* No negative headings. */
+  if (isnan(heading) == 0 && heading < 0.0) {
+    heading += 360.0;
+  }
+
+  return (float) (heading);
+}
+
+void nmeaInfoFromGpsd(struct gps_data_t *gpsdata, NmeaInfo *info, struct GpsdConnectionState *connectionTracking) {
+  if (!gpsdata //
+      || !info //
+      || !connectionTracking) {
+    return;
+  }
+
+  if (gpsdata->set & VERSION_SET) {
+    if (!connectionTracking->version) {
+      size_t releaseLength = strlen(gpsdata->version.release);
+      size_t revLength = strlen(gpsdata->version.rev);
+      size_t remoteLength = strlen(gpsdata->version.remote);
+      syslog(LOG_INFO, "Connected to gpsd%s%s%s%s%s%s on %s, protocol %d.%d", //
+          !releaseLength ? "" : " ", //
+          !releaseLength ? "" : gpsdata->version.release, //
+          !releaseLength ? "" : " ", //
+          !revLength ? "" : "(", //
+          !revLength ? "" : gpsdata->version.rev, //
+          !revLength ? "" : ")", //
+          !remoteLength ? "localhost" : gpsdata->version.remote, //
+          gpsdata->version.proto_major, //
+          gpsdata->version.proto_minor);
+
+      connectionTracking->version = true;
+    }
+
+    gpsdata->set &= ~VERSION_SET;
+  }
+
+  if (gpsdata->set & LOGMESSAGE_SET) {
+    gpsdLog(gpsdata->error);
+    gpsdata->set &= ~LOGMESSAGE_SET;
+  }
+
+  if (gpsdata->set & ERROR_SET) {
+    gpsdError(gpsdata->error);
+    gpsdata->set &= ~ERROR_SET;
+  }
+
+  if (gpsdata->set & (DEVICELIST_SET | DEVICE_SET | DEVICEID_SET)) {
+    int i = 0;
+    while (i < MAXUSERDEVS) {
+      struct devconfig_t *dev = &gpsdata->devices.list[i];
+      if (dev->flags //
+          && !connectionTracking->devSeen[i]) {
+        size_t subtypeLength = strlen(dev->subtype);
+        syslog(LOG_INFO, "Using %s device%s%s on %s in %s mode at %u baud (%u%c%u), refresh time %.3f (min. %.3f)", //
+            dev->driver, //
+            !subtypeLength ? "" : " ", //
+            !subtypeLength ? "" : dev->subtype, //
+            dev->path, //
+            dev->driver_mode ? "native" : "compatibility", //
+            dev->baudrate, //
+            8, //
+            dev->parity, //
+            dev->stopbits, //
+            dev->cycle, //
+            dev->mincycle);
+
+        connectionTracking->devSeen[i] = true;
+        connectionTracking->dev[i] = *dev;
+      } else if (connectionTracking->devSeen[i]) {
+        size_t subtypeLength;
+
+        dev = &connectionTracking->dev[i];
+        subtypeLength = strlen(dev->subtype);
+        syslog(LOG_INFO, "No longer using %s device%s%s on %s", //
+            dev->driver, //
+            !subtypeLength ? "" : " ", //
+            !subtypeLength ? "" : dev->subtype, //
+            dev->path);
+
+        connectionTracking->devSeen[i] = false;
+        memset(&connectionTracking->dev[i], 0, sizeof(connectionTracking->dev[i]));
+      }
+
+      i++;
+    }
+
+    gpsdata->set &= ~(DEVICELIST_SET | DEVICE_SET | DEVICEID_SET);
+  }
+
+  /* ignored */
+  gpsdata->set &= ~( //
+          ONLINE_SET // unreliable
+          | TIMERR_SET //
+          | CLIMB_SET //
+          | DOP_SET // using dop from fix
+          | ATTITUDE_SET //
+          | SPEEDERR_SET //
+          | TRACKERR_SET //
+          | CLIMBERR_SET //
+          | RTCM2_SET //
+          | RTCM3_SET //
+          | AIS_SET //
+          | PACKET_SET //
+          | SUBFRAME_SET //
+          | GST_SET //
+          | POLICY_SET //
+#ifdef GPSD_JESSIE
+          | TIMEDRIFT_SET //
+          | EOF_SET //
+#endif
+#ifdef GPSD_NEW
+          | TOFF_SET //
+          | PPS_SET //
+          | NAVDATA_SET //
+#endif
+      );
+
+  gpsdata->set &= ~STATUS_SET; /* always valid */
+  if (gpsdata->status == STATUS_NO_FIX) {
+    nmeaInfoClear(info);
+    return;
+  }
+
+  if (!gpsdata->set) {
+    return;
+  }
+
+  info->smask = NMEALIB_SENTENCE_MASK;
+  nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_SMASK);
+
+  /* date & time */
+  if (gpsdata->set & TIME_SET) {
+    if (!isNaN(gpsdata->fix.time)) {
+      double seconds;
+      double fraction = modf(fabs(gpsdata->fix.time), &seconds);
+      long sec = lrint(seconds);
+      struct tm *time = gmtime(&sec);
+      if (time) {
+        info->utc.year = (unsigned int) time->tm_year + 1900;
+        info->utc.mon = (unsigned int) time->tm_mon + 1;
+        info->utc.day = (unsigned int) time->tm_mday;
+        info->utc.hour = (unsigned int) time->tm_hour;
+        info->utc.min = (unsigned int) time->tm_min;
+        info->utc.sec = (unsigned int) time->tm_sec;
+        info->utc.hsec = (unsigned int) lrint(fraction * 100);
+
+        nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_UTCDATE | NMEALIB_PRESENT_UTCTIME);
+      }
+    }
+
+    gpsdata->set &= ~TIME_SET;
+  }
+
+  /* sig & fix */
+  if (gpsdata->set & MODE_SET) {
+    switch (gpsdata->fix.mode) {
+      case MODE_3D:
+        info->fix = NMEALIB_FIX_3D;
+        info->sig = NMEALIB_SIG_FIX;
+        break;
+
+      case MODE_2D:
+        info->fix = NMEALIB_FIX_2D;
+        info->sig = NMEALIB_SIG_FIX;
+        break;
+
+      case MODE_NOT_SEEN:
+      case MODE_NO_FIX:
+      default:
+        info->fix = NMEALIB_FIX_BAD;
+        info->sig = NMEALIB_SIG_INVALID;
+        break;
+    }
+
+    nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_FIX | NMEALIB_PRESENT_SIG);
+    gpsdata->set &= ~MODE_SET;
+  }
+
+  /* hdop */
+  if (gpsdata->set & HERR_SET) {
+    if (!isNaN(gpsdata->fix.epx) //
+        && !isNaN(gpsdata->fix.epy)) {
+      info->hdop = nmeaMathPdopCalculate(gpsdata->fix.epx, gpsdata->fix.epy);
+      nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_HDOP);
+    }
+
+    gpsdata->set &= ~HERR_SET;
+  }
+
+  /* vdop */
+  if (gpsdata->set & VERR_SET) {
+    if (!isNaN(gpsdata->fix.epv)) {
+      info->vdop = gpsdata->fix.epv;
+      nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_VDOP);
+    }
+
+    gpsdata->set &= ~VERR_SET;
+  }
+
+  /* pdop */
+  if (nmeaInfoIsPresentAll(info->present, NMEALIB_PRESENT_HDOP | NMEALIB_PRESENT_VDOP)) {
+    info->pdop = nmeaMathPdopCalculate(info->hdop, info->vdop);
+    nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_PDOP);
+  }
+
+  if (gpsdata->set & LATLON_SET) {
+    /* lat */
+    if ((gpsdata->fix.mode >= MODE_2D) //
+        && !isNaN(gpsdata->fix.latitude)) {
+      info->latitude = nmeaMathDegreeToNdeg(gpsdata->fix.latitude);
+      nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_LAT);
+    }
+
+    /* lon */
+    if ((gpsdata->fix.mode >= MODE_2D) //
+        && !isNaN(gpsdata->fix.longitude)) {
+      info->longitude = nmeaMathDegreeToNdeg(gpsdata->fix.longitude);
+      nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_LON);
+    }
+
+    gpsdata->set &= ~LATLON_SET;
+  }
+
+  if (gpsdata->set & ALTITUDE_SET) {
+    /* elv */
+    if ((gpsdata->fix.mode >= MODE_3D) //
+        && !isNaN(gpsdata->fix.altitude)) {
+      info->elevation = gpsdata->fix.altitude;
+      info->height = gpsdata->separation;
+      nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_ELV | NMEALIB_PRESENT_HEIGHT);
+    }
+
+    gpsdata->set &= ~ALTITUDE_SET;
+  }
+
+  if (gpsdata->set & SPEED_SET) {
+    /* speed */
+    if ((gpsdata->fix.mode >= MODE_2D) //
+        && !isNaN(gpsdata->fix.speed)) {
+      info->speed = gpsdata->fix.speed * MPS_TO_KPH;
+      nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_SPEED);
+    }
+
+    gpsdata->set &= ~SPEED_SET;
+  }
+
+  if (gpsdata->set & TRACK_SET) {
+    /* track & mtrack */
+    if ((gpsdata->fix.mode >= MODE_2D) //
+        && !isNaN(gpsdata->fix.track)) {
+      info->track = gpsdata->fix.track;
+      nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_TRACK);
+
+      if (nmeaInfoIsPresentAll(info->present, NMEALIB_PRESENT_LAT | NMEALIB_PRESENT_LON)) {
+        double magheading = true2magnetic(gpsdata->fix.latitude, gpsdata->fix.longitude, gpsdata->fix.track);
+        if (!isNaN(magheading)) {
+          info->mtrack = magheading;
+          nmeaInfoSetPresent(&info->present, NMEALIB_PRESENT_MTRACK);
+        }
+      }
+    }
+
+    gpsdata->set &= ~TRACK_SET;
+  }
+
+  /* magvar: not available */
+
+  /* satellites & sig */
+  if (gpsdata->set & SATELLITE_SET) {
+    size_t iGpsd;
+    size_t iInUse;
+    size_t iInView;
+
+    info->satellites.inUseCount = 0;
+    memset(&info->satellites.inUse, 0, sizeof(info->satellites.inUse));
+    info->satellites.inViewCount = 0;
+    memset(&info->satellites.inView, 0, sizeof(info->satellites.inView));
+
+    iInUse = 0;
+    iInView = 0;
+    for (iGpsd = 0; //
+        (iGpsd < MAXCHANNELS) && (iInUse < NMEALIB_MAX_SATELLITES) && (iInView < NMEALIB_MAX_SATELLITES); //
+        iGpsd++) {
+#ifndef GPSD_NEW
+      int prn = gpsdata->used[iGpsd];
+      info->satellites.inUse[iInUse++] = (prn < 0) ? 0 : (unsigned int) prn;
+
+      prn = gpsdata->PRN[iGpsd];
+      if (prn > 0) {
+        info->satellites.inViewCount++;
+        NmeaSatellite *infoSatellite = &info->satellites.inView[iInView++];
+        infoSatellite->prn = (unsigned int) prn;
+        infoSatellite->elevation = gpsdata->elevation[iGpsd];
+        infoSatellite->azimuth = (unsigned int) gpsdata->azimuth[iGpsd];
+        infoSatellite->snr = (unsigned int) lrint(gpsdata->ss[iGpsd]);
+      }
+#else
+      struct satellite_t *gpsdSatellite = &gpsdata->skyview[iGpsd];
+      if (gpsdSatellite->used) {
+        info->satellites.inUseCount++;
+        info->satellites.inUse[iInUse++] = (unsigned int) gpsdSatellite->PRN;
+      }
+
+      if (gpsdSatellite->PRN > 0) {
+        NmeaSatellite *infoSatellite;
+
+        info->satellites.inViewCount++;
+        infoSatellite = &info->satellites.inView[iInView++];
+        infoSatellite->prn = (unsigned int) gpsdSatellite->PRN;
+        infoSatellite->elevation = gpsdSatellite->elevation;
+        infoSatellite->azimuth = (unsigned int) gpsdSatellite->azimuth;
+        infoSatellite->snr = (unsigned int) lrint(gpsdSatellite->ss);
+      }
+#endif
+    }
+
+    nmeaInfoSetPresent(&info->present, //
+        NMEALIB_PRESENT_SATINUSECOUNT //
+        | NMEALIB_PRESENT_SATINUSE //
+        | NMEALIB_PRESENT_SATINVIEWCOUNT //
+        | NMEALIB_PRESENT_SATINVIEW);
+
+    gpsdata->set &= ~SATELLITE_SET;
+  }
+
+  if (nmeaInfoIsPresentAll(info->present, NMEALIB_PRESENT_FIX) && (info->fix == NMEALIB_FIX_BAD)) {
+    nmeaInfoClear(info);
+    nmeaTimeSet(&info->utc, &info->present, NULL);
+  }
+
+  if (nmeaInfoIsPresentAll(info->present, NMEALIB_PRESENT_SIG) && (info->sig == NMEALIB_SIG_INVALID)) {
+    nmeaInfoClear(info);
+    nmeaTimeSet(&info->utc, &info->present, NULL);
+  }
+
+  nmeaInfoSanitise(info);
+
+  if (gpsdata->set) {
+    syslog(LOG_WARNING, "Unhandled bits in gpsdata->set: %llx", (long long unsigned int) gpsdata->set);
+  }
+}
+
+void readFromGpsd(GpsDaemon *gpsd, struct gps_data_t *gpsdata, struct GpsdConnectionState *connectionTracking, NmeaInfo *nmeaInfo) {
+  int gpsReadCode;
+
+  if (!gpsd //
+      || !gpsdata //
+      || !connectionTracking //
+      || !nmeaInfo) {
+    return;
+  }
+
+  errno = 0;
+  if (!connectionTracking->connected) {
+    gpsReadCode = -1;
+  } else {
+    gpsReadCode = gps_read(gpsdata);
+  }
+
+  if (gpsReadCode > 0) {
+    /* data received from gpsd */
+    nmeaInfoFromGpsd(gpsdata, nmeaInfo, connectionTracking);
+  } else if (gpsReadCode < 0) {
+    /* failed to receive data from gpsd */
+
+    if (connectionTracking->connected) {
+      if (errno) {
+        syslog(LOG_WARNING, "Disconnected from gpsd: %s", strerror(errno));
+      } else {
+        syslog(LOG_WARNING, "Disconnected from gpsd");
+      }
+
+      gpsdDisconnect(gpsdata, connectionTracking);
+      nmeaInfoClear(nmeaInfo);
+    }
+
+    connectionTracking->connected = gpsdConnect(gpsd, gpsdata, connectionTracking);
+    nmeaTimeSet(&nmeaInfo->utc, &nmeaInfo->present, NULL);
+  } else {
+    /* no data received from gpsd */
+  }
+}
diff --git a/lib/pud/src/gpsdclient.h b/lib/pud/src/gpsdclient.h
new file mode 100644 (file)
index 0000000..061056b
--- /dev/null
@@ -0,0 +1,99 @@
+/*
+ * The olsr.org Optimized Link-State Routing daemon (olsrd)
+ *
+ * (c) by the OLSR project
+ *
+ * See our Git repository to find out who worked on this file
+ * and thus is a copyright holder on it.
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ *   notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ *   notice, this list of conditions and the following disclaimer in
+ *   the documentation and/or other materials provided with the
+ *   distribution.
+ * * Neither the name of olsr.org, olsrd nor the names of its
+ *   contributors may be used to endorse or promote products derived
+ *   from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Visit http://www.olsr.org for more information.
+ *
+ * If you find this software useful feel free to make a donation
+ * to the project. For more information see the website or contact
+ * the copyright holders.
+ *
+ */
+
+#ifndef _PUD_GPSD_GPSDCLIENT_H_
+#define _PUD_GPSD_GPSDCLIENT_H_
+
+#include <gps.h>
+#include <stdbool.h>
+#include <nmealib/info.h>
+
+struct GpsdConnectionState {
+    bool connected;
+    bool connectionFailureReported;
+    unsigned long long retryCount;
+
+    bool version;
+    bool devSeen[MAXUSERDEVS];
+    struct devconfig_t dev[MAXUSERDEVS];
+};
+
+/* describe a data source */
+struct fixsource_t {
+    char spec[PATH_MAX]; /* working space, will be modified */
+    char *server; /* pointer into spec field */
+    char *port; /* pointer into spec field */
+    char *device; /* pointer into spec field */
+};
+
+/**
+ * The gpsd daemon spec
+ */
+typedef struct _GpsDaemon {
+    struct fixsource_t source;
+    unsigned int gpsdStreamFlags; /**< the stream flags for the gpsd connection */
+} GpsDaemon;
+
+/** The default gpsd host */
+#define DEFAULT_GPSD_HOST "localhost"
+
+/** The default gpsd port */
+#define DEFAULT_GPSD_PORT "2947"
+
+/** The default gpsd source spec */
+#define PUD_GPSD_DEFAULT (DEFAULT_GPSD_HOST ":" DEFAULT_GPSD_PORT)
+
+void gpsdParseSourceSpec(char *fromstring, GpsDaemon *source);
+
+bool gpsdConnect(GpsDaemon *gpsd, struct gps_data_t *gpsData, struct GpsdConnectionState *connectionTracking);
+void gpsdDisconnect(struct gps_data_t *gpsdata, struct GpsdConnectionState *connectionTracking);
+
+float true2magnetic(double lat, double lon, double heading);
+
+void nmeaInfoFromGpsd(struct gps_data_t *gpsdata, NmeaInfo *info, struct GpsdConnectionState *connectionTracking);
+
+void readFromGpsd(GpsDaemon *configuration, struct gps_data_t *gpsdata, struct GpsdConnectionState *connectionTracking, NmeaInfo *nmeaInfo);
+
+#endif /* _PUD_GPSDCLIENT_H_ */