Positioning PoC Update only [no API change!]

1.) Sensors Service PoC:
- add support for LSM9DS1 6DOF inertial sensor
- refactor I2C access
- try to autodetect availablity if i2c-dev.h (needs further improvement)
  if you really have the correct i2c-dev.h, then comment out
    add_definitions(-DI2C_NOT_AVAILABLE)
  in sensors-service\src\CMakeLists.txt
- to test:
  cmake -DWITH_ENHANCED_POSITION_SERVICE=OFF -DWITH_NMEA=ON -DWITH_SENSORS=ON -DIMU_TYPE=LSM9DS1 -DWITH_LOGGER=ON -DWITH_TESTS=ON -DWITH_DEBUG=ON -DGNSS_DEVICE=\"/dev/ttyACM0\" -DGNSS_BAUDRATE=B38400 -DGNSS_CHIPSET=UBLOX -DGNSS_DELAY=50 ../
2.) Logger PoC
- add more functionality: buffered logging to file, ability to receive external input via UDP_BUFLEN

2 files changed, 292 insertions, 29 deletions

diff --git a/logger/test/CMakeLists.txt b/logger/test/CMakeLists.txt
index 696a54f..eea7f5a 100644
--- a/logger/test/CMakeLists.txt
+++ b/logger/test/CMakeLists.txt
@@ -22,7 +22,8 @@ message(STATUS "TEST-LOGGER")
 message(STATUS "WITH_DLT = ${WITH_DLT}")
 message(STATUS "WITH_GPSD = ${WITH_GPSD}")
 message(STATUS "WITH_NMEA = ${WITH_NMEA}")
-message(STATUS "WITH_MPU6050 = ${WITH_MPU6050}")
+message(STATUS "WITH_SENSORS = ${WITH_SENSORS}")
+message(STATUS "IMU_TYPE = ${IMU_TYPE}")
 message(STATUS "WITH_REPLAYER = ${WITH_REPLAYER}")
 message(STATUS "WITH_DEBUG = ${WITH_DEBUG}")
 
@@ -77,8 +78,8 @@ set(sns-service_INCLUDE_DIRS "${PROJECT_SOURCE_DIR}/../sensors-service/api")
 set(sns-service_LIBRARY_DIRS "${PROJECT_BINARY_DIR}/../sensors-service/src")
 if(WITH_IPHONE)
     set(SNS_LIBRARIES "sensors-service-use-iphone")
-elseif(WITH_MPU6050)
-    set(SNS_LIBRARIES "sensors-service-use-mpu6050")
+elseif(WITH_SENSORS)
+    set(SNS_LIBRARIES "sensors-service-use-sensors")
 elseif(WITH_REPLAYER)
     set(SNS_LIBRARIES "sensors-service-use-replayer")
 else()
diff --git a/logger/test/log-gnss-sns.cpp b/logger/test/log-gnss-sns.cpp
index 08611b3..33e332f 100644
--- a/logger/test/log-gnss-sns.cpp
+++ b/logger/test/log-gnss-sns.cpp
@@ -18,6 +18,11 @@
 * @licence end@
 **************************************************************************/
 
+
+#define __STDC_FORMAT_MACROS
+#include <inttypes.h>
+
+
 #include "poslog.h"
 #include "gnsslog.h"
 #include "snslog.h"
@@ -31,29 +36,195 @@
 #include <stdio.h>
 #include <pthread.h>
 #include <signal.h>
+#include <inttypes.h>
 
 #include "gnss-init.h"
 #include "gnss.h"
 #include "gnss-status.h"
 #include "sns-init.h"
 
+
+
+/**
+ * Double buffer for log strings
+ * Purpose: avoid blocking of callback functions by I/O
+ * Multiple writer threads are allowed but only on reader thread
+ */
+class DBuf {
+
+#define DBUF_STRING_SIZE 256
+#define DBUF_NUM_LINES 512
+
+    struct SBuf {
+    public:
+        char strings [DBUF_STRING_SIZE] [DBUF_NUM_LINES];
+        uint16_t rnext;
+        uint16_t wnext;
+        SBuf(): rnext(0), wnext(0) {};
+    };
+
+    SBuf* wbuf;
+    SBuf* rbuf;
+    pthread_mutex_t wmutex;
+    pthread_mutex_t rmutex;
+public:
+
+    DBuf()
+    {
+        pthread_mutex_init(&wmutex, NULL);
+        pthread_mutex_init(&rmutex, NULL);
+        wbuf = new(SBuf);
+        rbuf = new(SBuf);
+    }
+
+    /**
+     * Add a string to the buffer.
+     * Return true on success, false on failure (e.g. buffer full).
+     */
+    bool write(const char* s)
+    {
+        bool retval = false;
+        pthread_mutex_lock(&wmutex);
+        if (s && wbuf && (wbuf->wnext < DBUF_NUM_LINES))
+        {
+            strncpy(wbuf->strings[wbuf->wnext], s, DBUF_STRING_SIZE-1);
+            wbuf->strings[wbuf->wnext][DBUF_STRING_SIZE-1] = 0;
+            wbuf->wnext++;
+            retval = true;
+        }
+        pthread_mutex_unlock(&wmutex);
+        return retval;
+    }
+
+    /**
+     * Read next string from the buffer
+     * Return NULL, if no more string available
+     */
+    const char* read()
+    {
+        const char* ret = NULL;
+        pthread_mutex_lock(&rmutex);
+        if (rbuf && (rbuf->rnext < rbuf->wnext) && (rbuf->rnext < DBUF_NUM_LINES) )
+        {
+            ret = rbuf->strings[rbuf->rnext];
+            rbuf->rnext++;
+        }
+        pthread_mutex_unlock(&rmutex);
+        return ret;
+    }
+
+    /**
+     * Swap read and write buffers.
+     * Clears read buffer before to ensure that new write buffer is empty.
+     * Shall only be called by reader thread when read buffer has been
+     * completely processed.
+     */
+    void swap()
+    {
+        SBuf* tmp;
+        pthread_mutex_lock(&rmutex);
+        rbuf->rnext = 0;
+        rbuf->wnext = 0;
+        pthread_mutex_lock(&wmutex);
+        tmp = wbuf;
+        wbuf = rbuf;
+        rbuf = tmp;
+        pthread_mutex_unlock(&wmutex);
+        pthread_mutex_unlock(&rmutex);
+    }
+
+};
+
+
+
 #define GNSS_INIT_MAX_RETRIES 30
 
 //global variable to control the main loop - will be set by signal handlers or status callback
-static volatile bool sigint = false;
-static volatile bool sigterm = false;
-static volatile bool gnss_failure = false;
+enum EExitCondition {
+    eExitNone = 0,
+    eExitSigInt,
+    eExitGnssFailure
+};
+
+static volatile bool g_sigterm = false;
+static volatile EExitCondition g_exit = eExitNone;
+static volatile bool g_gnss_failure = false;
+//global pointer to the double buffer
+DBuf* g_dbuf = 0;
+pthread_t g_logthread;
+uint32_t g_write_failures = 0;
+FILE* g_logfile = 0;
+
+/**
+ * Logger callback to add string to the double buffer.
+ */
+void dbufCb(const char* string)
+{
+    if (g_dbuf)
+    {
+        bool write_success = g_dbuf->write(string);
+        if (!write_success)
+        {
+            g_write_failures++;
+        }
+        //printf("WBUF %s\n", string);
+    }
+}
+
+/**
+ * Background thread to write double buffer to a file.
+ *
+ */
+void* loop_log_writer(void*)
+{
+    bool stop = false;
+    //printf("LOGWRITER\n");
+    while (g_dbuf && !stop && !g_sigterm)
+    {
+        const char* s = 0;
+        //process read buffer - should always be empty
+        while (s = g_dbuf->read())
+        {
+            printf("BUF1 %s\n", s);
+        }
+        //swap and process previous write buffer
+        g_dbuf->swap();
+        while (s = g_dbuf->read())
+        {
+            fprintf(g_logfile, "%s\n", s);
+        }
+        fflush(g_logfile);
+        //printf("LOGWRITER SLEEP\n");
+        if (g_exit == eExitNone)
+        {
+            sleep(2);
+            //TODO it seems that only the main thread will be interrupted by the signal???
+            //TODO CHECK how to force faster reaction
+        }
+        else
+        {
+            stop = true;
+        }
+        //printf("LOGWRITER WAKEUP\n");
+    }
+    //printf("LOGWRITER END\n");
+}
 
 static void sigHandler (int sig, siginfo_t *siginfo, void *context)
 {
     if (sig == SIGINT) 
     {
-        sigint = true;
+        g_exit = eExitSigInt;
+        if (g_logfile)
+        {
+            //ensure that also the logger thread is interrupted
+            //pthread_kill(g_logthread, sig);  //seems not to work somehow
+        }
     }
     else 
     if (sig == SIGTERM) 
     {
-        sigterm = true;
+        g_sigterm = true;
     }
 }
 
@@ -61,19 +232,19 @@ static bool registerSigHandlers()
 {
     bool is_success = true;
     
-	struct sigaction action;
- 	memset (&action, '\0', sizeof(action));
-	action.sa_sigaction = &sigHandler;
-	action.sa_flags = SA_SIGINFO;
+    struct sigaction action;
+    memset (&action, '\0', sizeof(action));
+    action.sa_sigaction = &sigHandler;
+    action.sa_flags = SA_SIGINFO;
 
-  	if (sigaction(SIGINT, &action, NULL) < 0) 
+    if (sigaction(SIGINT, &action, NULL) < 0)
     {
-		is_success = false;
-	}
-	if (sigaction(SIGTERM, &action, NULL) < 0) 
+        is_success = false;
+    }
+    if (sigaction(SIGTERM, &action, NULL) < 0)
     {
-		is_success = false;
-	}
+        is_success = false;
+    }
     return is_success;
 }
 
@@ -98,7 +269,7 @@ static void cbGNSSStatus(const TGNSSStatus *status)
         poslogAddString(status_string);
         if (status->status == GNSS_STATUS_FAILURE)
         {
-            gnss_failure = true;
+            g_exit = eExitGnssFailure;
         }
     }
 }
@@ -113,31 +284,112 @@ static void cbGyro(const TGyroscopeData gyroData[], uint16_t numElements)
     gyroscopeDataLog(snslogGetTimestamp(), gyroData, numElements);
 }
 
+#define UDP_LOG
+#ifdef UDP_LOG
+#include<stdlib.h> //exit(0);
+#include<arpa/inet.h>
+#include<sys/socket.h>
+#define UDP_BUFLEN  128 //Max length of buffer
+#define UDP_PORT 5701   //The port on which to listen for incoming data
+#define UDP_LOGLEN  256
+
+pthread_t g_udpthread;
+
+void* loop_udp_log(void*)
+{
+    struct sockaddr_in si_me, si_other;
+    int s;
+    socklen_t slen = sizeof(si_other);
+    ssize_t recv_len;
+    char buf[UDP_BUFLEN];
+    char log_string[UDP_LOGLEN];
+    bool ok = true;
+
+    //create a UDP socket
+    if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1)
+    {
+        ok = false;
+    }
+
+    if (ok)
+    {
+        // zero out the structure
+        memset((char *) &si_me, 0, sizeof(si_me));
+        si_me.sin_family = AF_INET;
+        si_me.sin_port = htons(UDP_PORT);
+        si_me.sin_addr.s_addr = htonl(INADDR_ANY);
+
+        //bind socket to port
+        if( bind(s , (struct sockaddr*)&si_me, sizeof(si_me) ) == -1)
+        {
+            ok = false;
+        }
+    }
+
+    //keep listening for data
+    while(ok)
+    {
+        //try to receive some data, this is a blocking call
+        if ((recv_len = recvfrom(s, buf, UDP_BUFLEN-1, 0, (struct sockaddr *) &si_other, &slen)) == -1)
+        {
+            ok = false;
+        }
+        else
+        {
+            buf[recv_len]=0;
+            snprintf(log_string, UDP_LOGLEN-1, "%"PRIu64",0,$UDP,%s,%d,%s",
+                snslogGetTimestamp(),
+                inet_ntoa(si_other.sin_addr),
+                ntohs(si_other.sin_port),
+                buf
+            );
+            log_string[UDP_LOGLEN-1] = 0; //ensure that string is null-terminated
+            poslogAddString(log_string);
+        }
+    }
+    close(s);
+}
+#endif
+
+
 
-int main()
+
+int main (int argc, char *argv[])
 {
     int major;
     int minor;
     int micro;
     char version_string[64];
-    
+
     bool is_poslog_init_ok = false;
     bool is_sns_init_ok = false;    
     bool is_sns_gyro_init_ok = false;
     bool is_sns_accel_init_ok = false;
     bool is_gnss_init_ok = false;
     int gnss_init_tries = 0;
-    
+
     registerSigHandlers();
-    
+
 #if (DLT_ENABLED)
     DLT_REGISTER_APP("GLT","GNSS/SNS Logger");
 #endif
     poslogSetFD(STDOUT_FILENO);
     is_poslog_init_ok = poslogInit();
+
     if (is_poslog_init_ok)
     {
-        poslogSetActiveSinks(POSLOG_SINK_DLT|POSLOG_SINK_FD|POSLOG_SINK_CB);
+
+        if(argv[1] && (g_logfile = fopen(argv[1], "a")))
+        {
+            g_dbuf = new(DBuf);
+            poslogSetCB(dbufCb);
+            pthread_create(&g_logthread, NULL, loop_log_writer, NULL);
+            poslogSetActiveSinks(POSLOG_SINK_DLT|POSLOG_SINK_CB);
+        }
+        else
+        {
+            poslogSetActiveSinks(POSLOG_SINK_DLT|POSLOG_SINK_FD|POSLOG_SINK_CB);
+        }
 
         gnssGetVersion(&major, &minor, &micro);
         sprintf(version_string, "0,0$GVGNSVER,%d,%d,%d", major, minor, micro);
@@ -146,6 +398,9 @@ int main()
         sprintf(version_string, "0,0$GVSNSVER,%d,%d,%d", major, minor, micro);
         poslogAddString(version_string);
 
+#ifdef UDP_LOG
+        pthread_create(&g_udpthread, NULL, loop_udp_log, NULL);
+#endif
         is_sns_init_ok = snsInit();
         if (is_sns_init_ok)
         {
@@ -174,7 +429,7 @@ int main()
 
         //GNSS device may be available a bit late after startup
         is_gnss_init_ok = gnssInit();
-        while (!is_gnss_init_ok && (gnss_init_tries < GNSS_INIT_MAX_RETRIES) && !sigint && !sigterm)
+        while (!is_gnss_init_ok && (gnss_init_tries < GNSS_INIT_MAX_RETRIES) && (g_exit == eExitNone) && !g_sigterm)
         {
             sleep(1);
             is_gnss_init_ok = gnssInit();
@@ -190,7 +445,7 @@ int main()
 
         if (is_sns_init_ok || is_gnss_init_ok)
         {
-            while(!sigint && !sigterm && !gnss_failure)
+            while(!g_sigterm && (g_exit == eExitNone))
             {
                 sleep(1);
             }
@@ -201,13 +456,13 @@ int main()
         }
         
         //if not interrupted by SIGTERM then we have time to cleanup
-        if (!sigterm) 
+        if (!g_sigterm)
         {
-            if (sigint)
+            if (g_exit == eExitSigInt)
             {
                 poslogAddString("#SIGINT");
             }
-            if (gnss_failure)
+            if (g_exit == eExitGnssFailure)
             {
                 poslogAddString("#GNSS_STATUS_FAILURE");
             }
@@ -233,8 +488,15 @@ int main()
                 gnssDestroy();
             }
         }
+        if (g_logfile)
+        {
+            pthread_join(g_logthread, NULL);
+            fclose(g_logfile);
+            printf("#Write Failures: %"PRIu64"\n", g_write_failures);
+        }
         poslogDestroy();
     }
+
 #if (DLT_ENABLED)
     DLT_UNREGISTER_APP();
 #endif