PUD: add positionFile plugin parameter
authorFerry Huberts <ferry.huberts@pelagic.nl>
Fri, 13 Apr 2012 12:43:30 +0000 (14:43 +0200)
committerFerry Huberts <ferry.huberts@pelagic.nl>
Tue, 17 Apr 2012 12:41:30 +0000 (14:41 +0200)
You can now define a (GPS) position in a file and let the PUD
plugin read the file on startup to determine its position.

This is useful for nodes with a fixed position.

Signed-off-by: Ferry Huberts <ferry.huberts@pelagic.nl>
lib/pud/Makefile
lib/pud/doc/olsrd.conf.default.pud
lib/pud/resources/olsrd.pud.position.conf [new file with mode: 0644]
lib/pud/src/configuration.c
lib/pud/src/configuration.h
lib/pud/src/posFile.c [new file with mode: 0644]
lib/pud/src/posFile.h [new file with mode: 0644]
lib/pud/src/pudOlsrdPlugin.h
lib/pud/src/receiver.c

index 857dcca..e479ea8 100644 (file)
@@ -14,6 +14,7 @@ CFLAGS += -Werror -D_GNU_SOURCE
 
 
 VERSION_FILE = ./src/version.h
+RESOURCESDIR = ./resources
 LIBRARY_INC = $(LIBRARY_PATH)/include
 LIBRARY_LIB = $(LIBRARY_PATH)/lib
 
@@ -51,13 +52,15 @@ $(VERSION_FILE): library scripts/makeVersionH
 install: all
        @$(MAKE) -C $(LIBRARY_PATH) DESTDIR=$(DESTDIR) install
        $(INSTALL_LIB)
+       mkdir -p "$(ETCDIR)"
+       cp "$(RESOURCESDIR)/olsrd.pud.position.conf" "$(ETCDIR)"
        $(STRIP) "$(LIBDIR)/$(PLUGIN_FULLNAME)"
 
 uninstall:
        @$(MAKE) -C $(LIBRARY_PATH) DESTDIR=$(DESTDIR) uninstall
-       rm -f "$(LIBDIR)/lib$(PLUGIN_NAME).so" "$(LIBDIR)/$(PLUGIN_NAME)"
+       rm -f "$(LIBDIR)/lib$(PLUGIN_NAME).so" "$(LIBDIR)/$(PLUGIN_NAME)" "$(ETCDIR)/olsrd.pud.position.conf"
        $(UNINSTALL_LIB)
-       rmdir -v -p --ignore-fail-on-non-empty "$(LIBDIR)"
+       rmdir -v -p --ignore-fail-on-non-empty "$(LIBDIR)" "$(ETCDIR)"
 
 clean:
        @echo "[$@]"
index b55e0f5..2878ed1 100644 (file)
@@ -516,6 +516,10 @@ LoadPlugin "./lib/pud/olsrd_pud.so.1.1.0"
     #
     #PlParam     "rxMcPort"                     "2240"
 
+    # positionFile is the file that contains the position information that the
+    #              plugin should use. When this parameter is set then the
+    #              file is read during olsrd startup.
+    #PlParam     "positionFile"                 "/etc/olsrd.pud.position.conf"
 
     #
     # TX Parameters
diff --git a/lib/pud/resources/olsrd.pud.position.conf b/lib/pud/resources/olsrd.pud.position.conf
new file mode 100644 (file)
index 0000000..0ab0fb7
--- /dev/null
@@ -0,0 +1,51 @@
+#
+# OLSrd PUD plugin position configuration file
+#
+
+# GPS operating mode.
+# Values : bad, 2d, 3d
+# Default: bad
+# Note   : a value of 'bad' will make the resulting position information invalid
+#          (the mask will indicate that all other information is not present)
+#fix = bad
+
+# GPS quality indicator.
+# Values : bad, low, mid, high
+# Default: high
+#sig = high
+
+# HDOP (Horizontal Dilution Of Precision)
+# Values : floating point value
+# Default: 0.0
+# Note   : From HDOP, VDOP and PDOP are determined: VDOP = HDOP, and PDOP = sqrt(2) * HDOP.
+#hdop = 0.0
+
+# Latitude
+# Values : floating point value
+#          Unit is NMEA like latitude: ddmm.sssss (d=degrees, m=minutes, s=seconds)
+# Default: 0000.00000
+#lat = 0000.00000
+
+# Longitude
+# Values: floating point value
+#          Unit is NMEA like longitude: dddmm.sssss (d=degrees, m=minutes, s=seconds)
+# Default: 00000.00000
+#lon = 00000.00000
+
+# Elevation
+# Values: floating point value
+#         Unit is meters above the mean sea level (geoid)
+# Default: 0.0
+#elv = 0.0
+
+# Speed
+# Values: floating point value
+#         Unit is over the ground in kph
+# Default: 0.0
+#speed = 0.0
+
+# Direction/Track
+# Values: floating point value
+#         Unit is (compass) degrees
+# Default: 0.0
+#direction = 0.0
index 91ea03b..286768c 100644 (file)
@@ -4,6 +4,7 @@
 #include "pud.h"
 #include "networkInterfaces.h"
 #include "netTools.h"
+#include "posFile.h"
 
 /* OLSR includes */
 #include <olsr_protocol.h>
@@ -14,6 +15,7 @@
 #include <arpa/inet.h>
 #include <nmea/util.h>
 #include <OlsrdPudWireFormat/nodeIdConversion.h>
+#include <limits.h>
 
 /*
  * Utility functions
@@ -108,8 +110,7 @@ static bool readULL(const char * valueName, const char * value,
  - true on success
  - false otherwise
  */
-static bool readDouble(const char * valueName, const char * value,
-               double * valueNumber) {
+bool readDouble(const char * valueName, const char * value, double * valueNumber) {
        char * endPtr = NULL;
        double valueNew;
 
@@ -752,6 +753,67 @@ int setRxMcPort(const char *value, void *data __attribute__ ((unused)), set_plug
 }
 
 /*
+ * positionFile
+ */
+
+/** The positionFile buffer */
+static char positionFile[PATH_MAX + 1];
+
+/** True when the positionFile is set */
+static bool positionFileSet = false;
+
+/**
+ @return
+ The positionFile (NULL when not set)
+ */
+char * getPositionFile(void) {
+       if (!positionFileSet) {
+               return NULL;
+       }
+
+       return &positionFile[0];
+}
+
+/**
+ Set the positionFile.
+
+ @param value
+ The value of the positionFile to set (in string representation)
+ @param data
+ Unused
+ @param addon
+ Unused
+
+ @return
+ - true when an error is detected
+ - false otherwise
+ */
+int setPositionFile(const char *value, void *data __attribute__ ((unused)),
+               set_plugin_parameter_addon addon __attribute__ ((unused))) {
+       static const char * valueName = PUD_POSFILE_NAME;
+       size_t valueLength;
+
+       assert(value != NULL);
+
+       if (!startPositionFile()) {
+               stopPositionFile();
+               return true;
+       }
+
+       valueLength = strlen(value);
+       if (valueLength > PATH_MAX) {
+               pudError(false, "Configured %s is too long, maximum length is"
+                               " %u, current length is %lu", valueName, PATH_MAX, (unsigned long) valueLength);
+               return true;
+       }
+
+       strcpy((char *) &positionFile[0], value);
+       positionFileSet = true;
+
+       return false;
+}
+
+/*
  * txNonOlsrIf
  */
 
index a44b2cf..819c80e 100644 (file)
 #include <OlsrdPudWireFormat/wireFormat.h>
 
 /*
+ * Utilities
+ */
+
+bool readDouble(const char * valueName, const char * value, double * valueNumber);
+
+/*
  * Global Parameters
  */
 
@@ -75,6 +81,12 @@ unsigned short getRxMcPort(void);
 int
 setRxMcPort(const char *value, void *data, set_plugin_parameter_addon addon);
 
+/** The name of the positionFile plugin parameter */
+#define PUD_POSFILE_NAME                                               "positionFile"
+
+char * getPositionFile(void);
+int setPositionFile(const char *value, void *data, set_plugin_parameter_addon addon);
+
 /*
  * TX Parameters
  */
diff --git a/lib/pud/src/posFile.c b/lib/pud/src/posFile.c
new file mode 100644 (file)
index 0000000..a3af08e
--- /dev/null
@@ -0,0 +1,217 @@
+#include "posFile.h"
+
+/* Plugin includes */
+#include "pud.h"
+#include "configuration.h"
+
+/* OLSR includes */
+
+/* System includes */
+#include <stdio.h>
+#include <string.h>
+#include <regex.h>
+
+#define LINE_LENGTH 256
+
+static const char * regexCommentString = "^([[:space:]]*|[[:space:]#]+.*)$";
+static const char * regexNameValueString =
+               "^[[:space:]]*([^[:space:]]+)[[:space:]]*=[[:space:]]*([^[:space:]]+)[[:space:]]*$";
+static const size_t regexNameValuematchCount = 3;
+
+static regex_t regexComment;
+static regex_t regexNameValue;
+static bool started = false;
+
+bool startPositionFile(void) {
+       if (started) {
+               return true;
+       }
+
+       if (regcomp(&regexComment, regexCommentString, REG_EXTENDED)) {
+               pudError(false, "Could not compile regex \"%s\"", regexCommentString);
+               return false;
+       }
+
+       if (regcomp(&regexNameValue, regexNameValueString, REG_EXTENDED)) {
+               pudError(false, "Could not compile regex \"%s\"", regexNameValueString);
+               return false;
+       }
+
+       started = true;
+       return true;
+}
+
+void stopPositionFile(void) {
+       if (started) {
+               regfree(&regexNameValue);
+               regfree(&regexComment);
+               started = false;
+       }
+}
+
+static bool regexMatch(regex_t * regex, char * line, size_t nmatch, regmatch_t pmatch[]) {
+       int result = regexec(regex, line, nmatch, pmatch, 0);
+       if (!result) {
+               return true;
+       }
+
+       if (result == REG_NOMATCH) {
+               return false;
+       }
+
+       {
+               char msgbuf[256];
+               regerror(result, regex, msgbuf, sizeof(msgbuf));
+               pudError(false, "Regex match failed: %s", msgbuf);
+       }
+
+       return false;
+}
+
+static char line[LINE_LENGTH];
+static char name[LINE_LENGTH];
+static char value[LINE_LENGTH];
+
+bool readPositionFile(char * fileName, nmeaINFO * nmeaInfo) {
+       bool retval = false;
+       nmeaINFO result;
+       FILE * fd = NULL;
+       unsigned int lineNumber = 0;
+
+       nmea_zero_INFO(&result);
+       result.sig = POSFILE_DEFAULT_SIG;
+       result.fix = POSFILE_DEFAULT_FIX;
+       result.HDOP = POSFILE_DEFAULT_HDOP;
+       result.VDOP = POSFILE_CALCULATED_VDOP(result.HDOP);
+       result.PDOP = POSFILE_CALCULATED_PDOP(result.HDOP);
+       result.lat = POSFILE_DEFAULT_LAT;
+       result.lon = POSFILE_DEFAULT_LON;
+       result.elv = POSFILE_DEFAULT_ELV;
+       result.speed = POSFILE_DEFAULT_SPEED;
+       result.direction = POSFILE_DEFAULT_DIRECTION;
+
+       fd = fopen(fileName, "r");
+       if (!fd) {
+               goto out;
+       }
+
+       while (fgets(line, LINE_LENGTH, fd)) {
+               regmatch_t pmatch[regexNameValuematchCount];
+               int matchLen;
+
+               lineNumber++;
+
+               if (regexMatch(&regexComment, line, 0, NULL)) {
+                       continue;
+               }
+
+               if (!regexMatch(&regexNameValue, line, regexNameValuematchCount, pmatch)) {
+                       pudError(false, "Position file \"%s\", line %d uses invalid syntax: %s", fileName, lineNumber, line);
+                       goto out;
+               }
+
+               /* copy name/value */
+               matchLen = pmatch[1].rm_eo - pmatch[1].rm_so;
+               memcpy(name, &line[pmatch[1].rm_so], matchLen);
+               name[matchLen] = '\0';
+               matchLen = pmatch[2].rm_eo - pmatch[2].rm_so;
+               memcpy(value, &line[pmatch[2].rm_so], matchLen);
+               value[matchLen] = '\0';
+
+               if (!strncasecmp(POSFILE_NAME_SIG, name, sizeof(name))) {
+                       if (!strncasecmp(POSFILE_VALUE_SIG_BAD, value, sizeof(value))) {
+                               result.sig = NMEA_SIG_BAD;
+                       } else if (!strncasecmp(POSFILE_VALUE_SIG_LOW, value, sizeof(value))) {
+                               result.sig = NMEA_SIG_LOW;
+                       } else if (!strncasecmp(POSFILE_VALUE_SIG_MID, value, sizeof(value))) {
+                               result.sig = NMEA_SIG_MID;
+                       } else if (!strncasecmp(POSFILE_VALUE_SIG_HIGH, value, sizeof(value))) {
+                               result.sig = NMEA_SIG_HIGH;
+                       } else {
+                               pudError(false, "Position file \"%s\", line %d uses an invalid value for \"%s\","
+                                               " valid values are [%s|%s\%s|%s]", fileName, lineNumber, POSFILE_NAME_SIG,
+                                               POSFILE_VALUE_SIG_BAD, POSFILE_VALUE_SIG_LOW, POSFILE_VALUE_SIG_MID, POSFILE_VALUE_SIG_HIGH);
+                               goto out;
+                       }
+               } else if (!strncasecmp(POSFILE_NAME_FIX, name, sizeof(name))) {
+                       if (!strncasecmp(POSFILE_VALUE_FIX_BAD, value, sizeof(value))) {
+                               result.fix = NMEA_FIX_BAD;
+                       } else if (!strncasecmp(POSFILE_VALUE_FIX_2D, value, sizeof(value))) {
+                               result.fix = NMEA_FIX_2D;
+                       } else if (!strncasecmp(POSFILE_VALUE_FIX_3D, value, sizeof(value))) {
+                               result.fix = NMEA_FIX_3D;
+                       } else {
+                               pudError(false, "Position file \"%s\", line %d uses an invalid value for \"%s\","
+                                               " valid values are [%s\%s|%s]", fileName, lineNumber, POSFILE_NAME_FIX, POSFILE_VALUE_FIX_BAD,
+                                               POSFILE_VALUE_FIX_2D, POSFILE_VALUE_FIX_3D);
+                               goto out;
+                       }
+               } else if (!strncasecmp(POSFILE_NAME_HDOP, name, sizeof(name))) {
+                       double val;
+                       if (!readDouble(POSFILE_NAME_HDOP, value, &val)) {
+                               goto out;
+                       }
+
+                       result.HDOP = val;
+                       result.VDOP = POSFILE_CALCULATED_VDOP(result.HDOP);
+                       result.PDOP = POSFILE_CALCULATED_PDOP(result.HDOP);
+               } else if (!strncasecmp(POSFILE_NAME_LAT, name, sizeof(name))) {
+                       double val;
+                       if (!readDouble(POSFILE_NAME_LAT, value, &val)) {
+                               goto out;
+                       }
+
+                       result.lat = val;
+               } else if (!strncasecmp(POSFILE_NAME_LON, name, sizeof(name))) {
+                       double val;
+                       if (!readDouble(POSFILE_NAME_LON, value, &val)) {
+                               goto out;
+                       }
+
+                       result.lon = val;
+               } else if (!strncasecmp(POSFILE_NAME_ELV, name, sizeof(name))) {
+                       double val;
+                       if (!readDouble(POSFILE_NAME_ELV, value, &val)) {
+                               goto out;
+                       }
+
+                       result.elv = val;
+               } else if (!strncasecmp(POSFILE_NAME_SPEED, name, sizeof(name))) {
+                       double val;
+                       if (!readDouble(POSFILE_NAME_SPEED, value, &val)) {
+                               goto out;
+                       }
+
+                       result.speed = val;
+               } else if (!strncasecmp(POSFILE_NAME_DIRECTION, name, sizeof(name))) {
+                       double val;
+                       if (!readDouble(POSFILE_NAME_DIRECTION, value, &val)) {
+                               goto out;
+                       }
+
+                       result.direction = val;
+               } else {
+                       pudError(false, "Position file \"%s\", line %d uses an invalid option \"%s\","
+                                       " valid options are [%s|%s|%s|%s|%s|%s|%s|%s]", fileName, lineNumber, name, POSFILE_NAME_SIG,
+                                       POSFILE_NAME_FIX, POSFILE_NAME_HDOP, POSFILE_NAME_LAT, POSFILE_NAME_LON, POSFILE_NAME_ELV,
+                                       POSFILE_NAME_SPEED, POSFILE_NAME_DIRECTION);
+                       goto out;
+               }
+       }
+
+       fclose(fd);
+
+       result.smask = POSFILE_SANITISE_SMASK;
+       nmea_INFO_sanitise(&result);
+       nmea_INFO_unit_conversion(&result);
+       if (result.fix == NMEA_FIX_BAD) {
+               result.smask = 0;
+       } else {
+               result.smask = POSFILE_DEFAULT_SMASK;
+       }
+
+       memcpy(nmeaInfo, &result, sizeof(result));
+       retval = true;
+
+       out: return retval;
+}
diff --git a/lib/pud/src/posFile.h b/lib/pud/src/posFile.h
new file mode 100644 (file)
index 0000000..0beb1cc
--- /dev/null
@@ -0,0 +1,67 @@
+#ifndef _PUD_POSFILE_H_
+#define _PUD_POSFILE_H_
+
+/* Plugin includes */
+
+/* OLSR includes */
+
+/* System includes */
+#include <stdbool.h>
+#include <nmea/info.h>
+#include <nmea/sentence.h>
+
+/**
+ <pre>
+ field/sentence GPGGA   GPRMC
+ utc:           x       x
+ sig:           x       x
+ fix:                   x
+ PDOP:                          =sqrt(2)*HDOP  (GPGSA)
+ HDOP:          x
+ VDOP:                          =HDOP          (GPGSA)
+ lat:           x       x
+ lon:           x       x
+ elv:           x
+ speed:                 x
+ direction:             x
+ </pre>
+ */
+#define POSFILE_DEFAULT_SMASK           (GPGGA | GPRMC)
+#define POSFILE_SANITISE_SMASK          (GPGGA | GPRMC | GPGSA)
+
+/* no default utc: current time is always used */
+#define POSFILE_DEFAULT_SIG             (NMEA_SIG_HIGH)
+#define POSFILE_DEFAULT_FIX             (NMEA_FIX_BAD)
+#define POSFILE_DEFAULT_HDOP            (0.0)
+#define POSFILE_DEFAULT_LAT             (0.0)
+#define POSFILE_DEFAULT_LON             (0.0)
+#define POSFILE_DEFAULT_ELV             (0.0)
+#define POSFILE_DEFAULT_SPEED           (0.0)
+#define POSFILE_DEFAULT_DIRECTION       (0.0)
+
+#define POSFILE_CALCULATED_VDOP(hdop)   (hdop)
+#define POSFILE_CALCULATED_PDOP(hdop)   (hdop * 1.414213562)
+
+#define POSFILE_NAME_SIG                "sig"
+#define POSFILE_NAME_FIX                "fix"
+#define POSFILE_NAME_HDOP               "hdop"
+#define POSFILE_NAME_LAT                "lat"
+#define POSFILE_NAME_LON                "lon"
+#define POSFILE_NAME_ELV                "elv"
+#define POSFILE_NAME_SPEED              "speed"
+#define POSFILE_NAME_DIRECTION          "direction"
+
+#define POSFILE_VALUE_SIG_BAD           "bad"
+#define POSFILE_VALUE_SIG_LOW           "low"
+#define POSFILE_VALUE_SIG_MID           "mid"
+#define POSFILE_VALUE_SIG_HIGH          "high"
+
+#define POSFILE_VALUE_FIX_BAD           "bad"
+#define POSFILE_VALUE_FIX_2D            "2d"
+#define POSFILE_VALUE_FIX_3D            "3d"
+
+bool startPositionFile(void);
+void stopPositionFile(void);
+bool readPositionFile(char * fileName, nmeaINFO * nmeaInfo);
+
+#endif /* _PUD_POSFILE_H_ */
index 0e8304d..e5acbfd 100644 (file)
@@ -33,6 +33,7 @@ static const struct olsrd_plugin_parameters plugin_parameters[] = {
        {       .name = PUD_RX_ALLOWED_SOURCE_IP_NAME, .set_plugin_parameter = &addRxAllowedSourceIpAddress, .data = NULL},
        {       .name = PUD_RX_MC_ADDR_NAME, .set_plugin_parameter = &setRxMcAddr, .data = NULL},
        {       .name = PUD_RX_MC_PORT_NAME, .set_plugin_parameter = &setRxMcPort, .data = NULL},
+       {       .name = PUD_POSFILE_NAME, .set_plugin_parameter = &setPositionFile, .data = NULL},
 
        /* TX */
        {       .name = PUD_TX_NON_OLSR_IF_NAME, .set_plugin_parameter = &addTxNonOlsrInterface, .data = NULL},
index 16bf35b..827d8d5 100644 (file)
@@ -9,6 +9,7 @@
 #include "networkInterfaces.h"
 #include "timers.h"
 #include "uplinkGateway.h"
+#include "posFile.h"
 
 /* OLSRD includes */
 #include "olsr_types.h"
@@ -747,13 +748,18 @@ bool receiverUpdateGpsInformation(unsigned char * rxBuffer, size_t rxCount) {
  */
 bool startReceiver(void) {
        MovementState externalState;
+       char * positionFile = getPositionFile();
 
        if (!nmea_parser_init(&nmeaParser)) {
                pudError(false, "Could not initialise NMEA parser");
                return false;
        }
 
-       nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
+       if (positionFile) {
+               readPositionFile(positionFile, &transmitGpsInformation.txPosition.nmeaInfo);
+       } else {
+               nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
+       }
        transmitGpsInformation.txGateway = olsr_cnf->main_addr;
        transmitGpsInformation.positionUpdated = false;