PUD: fix averaging of direction
[olsrd.git] / lib / pud / src / posAvg.c
index 276af69..1638949 100644 (file)
@@ -1,17 +1,13 @@
 #include "posAvg.h"
 
 /* Plugin includes */
-#include "dump.h"
 
 /* OLSR includes */
 #include "olsr.h"
 
 /* System includes */
 #include <assert.h>
-#include <string.h>
-#include <stdlib.h>
 #include <nmea/sentence.h>
-#include <nmea/info.h>
 
 /* Defines */
 
 void flushPositionAverageList(PositionAverageList * positionAverageList) {
        assert (positionAverageList != NULL);
 
-       (void) pthread_mutex_lock(&positionAverageList->mutex);
-
        positionAverageList->entriesCount = 0;
        memset(&positionAverageList->counters, 0,
                        sizeof(positionAverageList->counters));
 
        nmea_zero_INFO(&positionAverageList->positionAverageCumulative.nmeaInfo);
        nmea_zero_INFO(&positionAverageList->positionAverage.nmeaInfo);
-
-       (void) pthread_mutex_unlock(&positionAverageList->mutex);
-
-#if defined(PUD_DUMP_AVERAGING)
-       olsr_printf(0, "flushPositionAverageList: Flushed the averaging list\n");
-#endif /* PUD_DUMP_AVERAGING */
 }
 
 /**
@@ -62,7 +50,6 @@ void flushPositionAverageList(PositionAverageList * positionAverageList) {
  */
 bool initPositionAverageList(PositionAverageList * positionAverageList,
                unsigned long long maxEntries) {
-       pthread_mutexattr_t attr;
        void * p;
 
        if (positionAverageList == NULL) {
@@ -72,16 +59,6 @@ bool initPositionAverageList(PositionAverageList * positionAverageList,
                return false;
        }
 
-       if (pthread_mutexattr_init(&attr)) {
-               return false;
-       }
-       if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE_NP)) {
-               return false;
-       }
-       if (pthread_mutex_init(&positionAverageList->mutex, &attr)) {
-               return false;
-       }
-
        p = olsr_malloc((maxEntries + 1) * sizeof(PositionUpdateEntry),
                        "PositionAverageEntry entries for PositionAverageList (PUD)");
        if (p == NULL) {
@@ -106,8 +83,6 @@ bool initPositionAverageList(PositionAverageList * positionAverageList,
 void destroyPositionAverageList(PositionAverageList * positionAverageList) {
        assert (positionAverageList != NULL);
 
-       (void) pthread_mutex_lock(&positionAverageList->mutex);
-
        flushPositionAverageList(positionAverageList);
 
        if (positionAverageList->entries != NULL) {
@@ -117,10 +92,6 @@ void destroyPositionAverageList(PositionAverageList * positionAverageList) {
 
        positionAverageList->entriesMaxCount = 0;
        positionAverageList->newestEntryIndex = 0;
-
-       (void) pthread_mutex_unlock(&positionAverageList->mutex);
-
-       pthread_mutex_destroy(&positionAverageList->mutex);
 }
 
 /**
@@ -139,7 +110,6 @@ PositionUpdateEntry * getPositionAverageEntry(
                AverageEntryPositionType positionType) {
        PositionUpdateEntry * r = NULL;
 
-       (void) pthread_mutex_lock(&positionAvgList->mutex);
        switch (positionType) {
                case OLDEST:
                        assert(positionAvgList->entriesCount >= positionAvgList->entriesMaxCount);
@@ -166,7 +136,6 @@ PositionUpdateEntry * getPositionAverageEntry(
                        r = NULL;
                        break;
        }
-       (void) pthread_mutex_unlock(&positionAvgList->mutex);
 
        return r;
 }
@@ -315,6 +284,52 @@ static void determineCumulativeSmaskSigFix(
 }
 
 /**
+ * Adjust the range of the direction so that we can correctly average it:
+ * <pre>
+ * [   0, 180) --> [   0, 180)
+ * [ 180, 360) --> [-180,   0)
+ * </pre>
+ * @param direction the direction to adjust
+ * @return the adjusted direction
+ */
+static double getAdjustedDirectionForAveraging(double direction) {
+       assert(direction >= (double)0.0);
+       assert(direction < (double)360.0);
+
+       printf("  getAdjustedDirectionForAveraging=%f (entry)\n", direction);
+       if (direction >= (double)180.0) {
+               printf("  getAdjustedDirectionForAveraging=%f (exit)\n", direction - (double)360.0);
+               return (direction - (double)360.0);
+       }
+
+       printf("  getAdjustedDirectionForAveraging=%f (exit)\n", direction);
+       return direction;
+}
+
+/**
+ * Adjust the range of the direction after averaging: the reverse of getAdjustedDirectionForAveraging
+ * <pre>
+ * [-180,   0) --> [ 180, 360)
+ * [   0, 180) --> [   0, 180)
+ * </pre>
+ * @param direction the direction to adjust
+ * @return the adjusted direction
+ */
+static double getAdjustedDirectionAfterAveraging(double direction) {
+       assert(direction >= (double)-180.0);
+       assert(direction < (double)180.0);
+
+       printf("  getAdjustedDirectionAfterAveraging=%f (entry)\n", direction);
+       if (direction < (double)0.0) {
+               printf("  getAdjustedDirectionAfterAveraging=%f (exit)\n", direction + (double)360.0);
+               return (direction + (double)360.0);
+       }
+
+       printf("  getAdjustedDirectionAfterAveraging=%f (exit)\n", direction);
+       return direction;
+}
+
+/**
  Add/remove a position update entry to/from the average position list, updates
  the counters, adjusts the entriesCount and redetermines the cumulative
  smask, sig and fix.
@@ -331,39 +346,7 @@ static void addOrRemoveEntryToFromCumulativeAverage(
                bool add) {
        PositionUpdateEntry * cumulative =
                        &positionAverageList->positionAverageCumulative;
-
-#if defined(PUD_DUMP_AVERAGING)
-       dump_nmeaInfo(&entry->nmeaInfo, "addOrRemoveEntryToFromCumulativeAverage: entry");
-       dump_nmeaInfo(&cumulative->nmeaInfo, "addOrRemoveEntryToFromCumulativeAverage: positionAverageList->positionAverageCumulative (before)");
-       olsr_printf(0,
-                       "addOrRemoveEntryToFromCumulativeAverage: positionAverageList->counters (before)\n"
-                       "  gpgga   = %llu\n"
-                       "  gpgsa   = %llu\n"
-                       "  gpgsv   = %llu\n"
-                       "  gprmc   = %llu\n"
-                       "  gpvtg   = %llu\n"
-                       "  sigBad  = %llu\n"
-                       "  sigLow  = %llu\n"
-                       "  sigMid  = %llu\n"
-                       "  sigHigh = %llu\n"
-                       "  fixBad  = %llu\n"
-                       "  fix2d   = %llu\n"
-                       "  fix3d   = %llu\n"
-                       "\n",
-                       positionAverageList->counters.gpgga,
-                       positionAverageList->counters.gpgsa,
-                       positionAverageList->counters.gpgsv,
-                       positionAverageList->counters.gprmc,
-                       positionAverageList->counters.gpvtg,
-                       positionAverageList->counters.sigBad,
-                       positionAverageList->counters.sigLow,
-                       positionAverageList->counters.sigMid,
-                       positionAverageList->counters.sigHigh,
-                       positionAverageList->counters.fixBad,
-                       positionAverageList->counters.fix2d,
-                       positionAverageList->counters.fix3d
-       );
-#endif /* PUD_DUMP_AVERAGING */
+       double  adjustedDirection = getAdjustedDirectionForAveraging(entry->nmeaInfo.direction);
 
        if (!add) {
                assert(positionAverageList->entriesCount >= positionAverageList->entriesMaxCount);
@@ -412,8 +395,8 @@ static void addOrRemoveEntryToFromCumulativeAverage(
                        : -entry->nmeaInfo.elv;
        cumulative->nmeaInfo.speed += add ? entry->nmeaInfo.speed
                        : -entry->nmeaInfo.speed;
-       cumulative->nmeaInfo.direction += add ? entry->nmeaInfo.direction
-                       : -entry->nmeaInfo.direction;
+       cumulative->nmeaInfo.direction += add ? adjustedDirection
+                       : -adjustedDirection;
        cumulative->nmeaInfo.declination += add ? entry->nmeaInfo.declination
                        : -entry->nmeaInfo.declination;
 
@@ -421,38 +404,6 @@ static void addOrRemoveEntryToFromCumulativeAverage(
 
        updateCounters(positionAverageList, entry, add);
        determineCumulativeSmaskSigFix(positionAverageList);
-
-#if defined(PUD_DUMP_AVERAGING)
-       dump_nmeaInfo(&cumulative->nmeaInfo, "addOrRemoveEntryToFromCumulativeAverage: positionAverageList->positionAverageCumulative (after)");
-       olsr_printf(0,
-                       "addOrRemoveEntryToFromCumulativeAverage: positionAverageList->counters (before)\n"
-                       "  gpgga   = %llu\n"
-                       "  gpgsa   = %llu\n"
-                       "  gpgsv   = %llu\n"
-                       "  gprmc   = %llu\n"
-                       "  gpvtg   = %llu\n"
-                       "  sigBad  = %llu\n"
-                       "  sigLow  = %llu\n"
-                       "  sigMid  = %llu\n"
-                       "  sigHigh = %llu\n"
-                       "  fixBad  = %llu\n"
-                       "  fix2d   = %llu\n"
-                       "  fix3d   = %llu\n"
-                       "\n",
-                       positionAverageList->counters.gpgga,
-                       positionAverageList->counters.gpgsa,
-                       positionAverageList->counters.gpgsv,
-                       positionAverageList->counters.gprmc,
-                       positionAverageList->counters.gpvtg,
-                       positionAverageList->counters.sigBad,
-                       positionAverageList->counters.sigLow,
-                       positionAverageList->counters.sigMid,
-                       positionAverageList->counters.sigHigh,
-                       positionAverageList->counters.fixBad,
-                       positionAverageList->counters.fix2d,
-                       positionAverageList->counters.fix3d
-       );
-#endif /* PUD_DUMP_AVERAGING */
 }
 
 /**
@@ -466,13 +417,7 @@ static void updatePositionAverageFromCumulative(
                PositionAverageList * positionAverageList) {
        double divider = positionAverageList->entriesCount;
 
-#if defined(PUD_DUMP_AVERAGING)
-       dump_nmeaInfo(&positionAverageList->positionAverage.nmeaInfo, "updatePositionAverageFromCumulative: positionAverageList->positionAverage (before)");
-#endif /* PUD_DUMP_AVERAGING */
-
-       memcpy(&positionAverageList->positionAverage,
-                       &positionAverageList->positionAverageCumulative,
-                       sizeof(positionAverageList->positionAverage));
+       positionAverageList->positionAverage = positionAverageList->positionAverageCumulative;
 
        /* smask: use from cumulative average */
 
@@ -495,11 +440,9 @@ static void updatePositionAverageFromCumulative(
                positionAverageList->positionAverage.nmeaInfo.declination /= divider;
        }
 
-       /* satinfo: use from average */
+       positionAverageList->positionAverage.nmeaInfo.direction = getAdjustedDirectionAfterAveraging(positionAverageList->positionAverage.nmeaInfo.direction);
 
-#if defined(PUD_DUMP_AVERAGING)
-       dump_nmeaInfo(&positionAverageList->positionAverage.nmeaInfo, "updatePositionAverageFromCumulative: positionAverageList->positionAverage (after)");
-#endif /* PUD_DUMP_AVERAGING */
+       /* satinfo: use from average */
 }
 
 /**
@@ -516,14 +459,6 @@ void addNewPositionToAverage(PositionAverageList * positionAverageList,
        assert (positionAverageList != NULL);
        assert (newEntry == getPositionAverageEntry(positionAverageList, INCOMING));
 
-#if defined(PUD_DUMP_AVERAGING)
-       dump_nmeaInfo(&newEntry->nmeaInfo, "addNewPositionToAverage: newEntry");
-       olsr_printf(0, "addNewPositionToAverage: positionAverageList->newestEntryIndex = %llu (before)\n", NEWESTINDEX(positionAverageList));
-       dump_nmeaInfo(&positionAverageList->positionAverageCumulative.nmeaInfo, "addNewPositionToAverage: positionAverageList->positionAverageCumulative.nmeaInfo (before)");
-#endif /* PUD_DUMP_AVERAGING */
-
-       (void) pthread_mutex_lock(&positionAverageList->mutex);
-
        if (positionAverageList->entriesCount
                        >= positionAverageList->entriesMaxCount) {
                /* list is full, so first remove the oldest from the average */
@@ -538,17 +473,6 @@ void addNewPositionToAverage(PositionAverageList * positionAverageList,
        positionAverageList->newestEntryIndex
                        = WRAPINDEX(positionAverageList, NEWESTINDEX(positionAverageList) + 1);
 
-#if defined(PUD_DUMP_AVERAGING)
-       olsr_printf(0, "addNewPositionToAverage: positionAverageList->newestEntryIndex = %llu (after)\n", NEWESTINDEX(positionAverageList));
-       dump_nmeaInfo(&positionAverageList->positionAverageCumulative.nmeaInfo, "addNewPositionToAverage: positionAverageList->positionAverageCumulative.nmeaInfo (before)");
-#endif /* PUD_DUMP_AVERAGING */
-
        /* update average position */
        updatePositionAverageFromCumulative(positionAverageList);
-
-#if defined(PUD_DUMP_AVERAGING)
-       dump_nmeaInfo(&positionAverageList->positionAverage.nmeaInfo, "addNewPositionToAverage: positionAverageList->positionAverage.nmeaInfo (after)");
-#endif /* PUD_DUMP_AVERAGING */
-
-       (void) pthread_mutex_unlock(&positionAverageList->mutex);
 }