025b35cf11c3d95d2f206c13f4c9f8ec507606d2
[olsrd.git] / lib / pud / src / posAvg.c
1 #include "posAvg.h"
2
3 /* Plugin includes */
4 #include "nmeaTools.h"
5 #include "dump.h"
6 #include "configuration.h"
7
8 /* OLSR includes */
9 #include "olsr.h"
10
11 /* System includes */
12 #include <assert.h>
13 #include <stddef.h>
14 #include <string.h>
15 #include <stdlib.h>
16 #include <nmea/sentence.h>
17
18 /* Defines */
19
20 #define LISTSIZE(x)                     (((x)->entriesMaxCount) + 1) /* always valid */
21 #define NEWESTINDEX(x)          ((x)->newestEntryIndex) /* always valid */
22 #define WRAPINDEX(x, i)         ((i) % LISTSIZE(x)) /* always valid for i>=0 */
23 #define INCOMINGINDEX(x)        WRAPINDEX(x, (NEWESTINDEX(x) + 1)) /* always valid */
24 #define OLDESTINDEX(x)          (((x)->entriesCount > 1) ? WRAPINDEX(x, (INCOMINGINDEX(x) + LISTSIZE(x) - (x)->entriesCount)) : NEWESTINDEX(x)) /* always valid */
25
26 /**
27  Flush/empty the position average list
28
29  @param positionAverageList
30  The position average list
31  */
32 void flushPositionAverageList(PositionAverageList * positionAverageList) {
33         assert (positionAverageList != NULL);
34
35         (void) pthread_mutex_lock(&positionAverageList->mutex);
36
37         positionAverageList->entriesCount = 0;
38         memset(&positionAverageList->counters, 0,
39                         sizeof(positionAverageList->counters));
40
41         nmea_zero_INFO(&positionAverageList->positionAverageCumulative.nmeaInfo);
42         nmea_zero_INFO(&positionAverageList->positionAverage.nmeaInfo);
43
44         (void) pthread_mutex_unlock(&positionAverageList->mutex);
45
46 #if defined(PUD_DUMP_AVERAGING)
47         olsr_printf(0, "flushPositionAverageList: Flushed the averaging list\n");
48 #endif /* PUD_DUMP_AVERAGING */
49 }
50
51 /**
52  Initialise the position average list: allocate memory for the entries and
53  reset fields.
54
55  @param positionAverageList
56  The position average list
57  @param maxEntries
58  The maximum number of entries in the list (the number of entries that should
59  be averaged)
60
61  @return
62  - false on failure
63  - true otherwise
64  */
65 bool initPositionAverageList(PositionAverageList * positionAverageList,
66                 unsigned long long maxEntries) {
67         pthread_mutexattr_t attr;
68         void * p;
69
70         if (positionAverageList == NULL) {
71                 return false;
72         }
73         if (maxEntries < 2) {
74                 return false;
75         }
76
77         if (pthread_mutexattr_init(&attr)) {
78                 return false;
79         }
80         if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE_NP)) {
81                 return false;
82         }
83         if (pthread_mutex_init(&positionAverageList->mutex, &attr)) {
84                 return false;
85         }
86
87         p = olsr_malloc((maxEntries + 1) * sizeof(PositionUpdateEntry),
88                         "PositionAverageEntry entries for PositionAverageList (PUD)");
89         if (p == NULL) {
90                 return false;
91         }
92
93         positionAverageList->entriesMaxCount = maxEntries;
94         positionAverageList->entries = p;
95         positionAverageList->newestEntryIndex = 0;
96
97         positionAverageList->txPosition.nmeaInfo.smask = GPNON;
98         positionAverageList->txPosition.nmeaInfo.fix = NMEA_FIX_BAD;
99         positionAverageList->txPosition.nmeaInfo.sig = NMEA_SIG_BAD;
100
101         flushPositionAverageList(positionAverageList);
102
103         return true;
104 }
105
106 /**
107  Clean up the position average list: free memory and reset fields.
108
109  @param positionAverageList
110  The position average list
111  */
112 void destroyPositionAverageList(PositionAverageList * positionAverageList) {
113         assert (positionAverageList != NULL);
114
115         (void) pthread_mutex_lock(&positionAverageList->mutex);
116
117         flushPositionAverageList(positionAverageList);
118
119         if (positionAverageList->entries != NULL) {
120                 free(positionAverageList->entries);
121                 positionAverageList->entries = NULL;
122         }
123
124         positionAverageList->entriesMaxCount = 0;
125         positionAverageList->newestEntryIndex = 0;
126
127         (void) pthread_mutex_unlock(&positionAverageList->mutex);
128
129         pthread_mutex_destroy(&positionAverageList->mutex);
130 }
131
132 /**
133  Get the entry for a certain type of position update.
134
135  @param positionAvgList
136  The position average list
137  @param positionType
138  The type of the position of the average entry
139
140  @return
141  A pointer to the requested position update entry
142  */
143 PositionUpdateEntry * getPositionAverageEntry(
144                 PositionAverageList * positionAvgList,
145                 AverageEntryPositionType positionType) {
146         PositionUpdateEntry * r = NULL;
147
148         (void) pthread_mutex_lock(&positionAvgList->mutex);
149         switch (positionType) {
150                 case OLDEST:
151                         assert(positionAvgList->entriesCount >= positionAvgList->entriesMaxCount);
152                         r = &positionAvgList->entries[OLDESTINDEX(positionAvgList)];
153                         break;
154
155                 case INCOMING:
156                         r = &positionAvgList->entries[INCOMINGINDEX(positionAvgList)];
157                         break;
158
159                 case NEWEST:
160                         r = &positionAvgList->entries[NEWESTINDEX(positionAvgList)];
161                         break;
162
163                 case AVERAGECUMULATIVE:
164                         r = &positionAvgList->positionAverageCumulative;
165                         break;
166
167                 case AVERAGE:
168                         r = &positionAvgList->positionAverage;
169                         break;
170
171                 case LASTTX:
172                         r = &positionAvgList->txPosition;
173                         break;
174
175                 default:
176                         r = NULL;
177                         break;
178         }
179         (void) pthread_mutex_unlock(&positionAvgList->mutex);
180
181         return r;
182 }
183
184 /**
185  Update position average mask and fix counters for a new entry or for an entry
186  that is/will be removed. Update the respective counters when the smask of the
187  entry has the corresponding flag set. The fix counters count the fix values
188  separately.
189
190  @param positionAverageList
191  The position average list
192  @param entry
193  The entry to update the counters from
194  @param add
195  True when updating the counters for a new entry, false for an entry that
196  is/will be removed
197  */
198 static void updateCounters(PositionAverageList * positionAverageList,
199                 PositionUpdateEntry * entry, bool add) {
200         PositionUpdateCounters * counters = &positionAverageList->counters;
201         unsigned long long maxCount = positionAverageList->entriesMaxCount;
202         int amount = (add ? 1 : -1);
203
204         /* smask */
205         if ((entry->nmeaInfo.smask & GPGGA) != 0) {
206                 assert(add ? (counters->gpgga < maxCount):(counters->gpgga > 0));
207                 counters->gpgga += amount;
208         }
209         if ((entry->nmeaInfo.smask & GPGSA) != 0) {
210                 assert(add ? (counters->gpgsa < maxCount):(counters->gpgsa > 0));
211                 counters->gpgsa += amount;
212         }
213         if ((entry->nmeaInfo.smask & GPGSV) != 0) {
214                 assert(add ? (counters->gpgsv < maxCount):(counters->gpgsv > 0));
215                 counters->gpgsv += amount;
216         }
217         if ((entry->nmeaInfo.smask & GPRMC) != 0) {
218                 assert(add ? (counters->gprmc < maxCount):(counters->gprmc > 0));
219                 counters->gprmc += amount;
220         }
221         if ((entry->nmeaInfo.smask & GPVTG) != 0) {
222                 assert(add ? (counters->gpvtg < maxCount):(counters->gpvtg > 0));
223                 counters->gpvtg += amount;
224         }
225
226         /* sig */
227         if (nmeaInfoHasField(entry->nmeaInfo.smask, SIG)) {
228                 if (entry->nmeaInfo.sig == NMEA_SIG_HIGH) {
229                         assert(add ? (counters->sigHigh < maxCount):(counters->sigHigh > 0));
230                         counters->sigHigh += amount;
231                 } else if (entry->nmeaInfo.sig == NMEA_SIG_MID) {
232                         assert(add ? (counters->sigMid < maxCount):(counters->sigMid > 0));
233                         counters->sigMid += amount;
234                 } else if (entry->nmeaInfo.sig == NMEA_SIG_LOW) {
235                         assert(add ? (counters->sigLow < maxCount):(counters->sigLow > 0));
236                         counters->sigLow += amount;
237                 } else {
238                         assert(add ? (counters->sigBad < maxCount):(counters->sigBad > 0));
239                         counters->sigBad += amount;
240                 }
241         }
242
243         /* fix */
244         if (nmeaInfoHasField(entry->nmeaInfo.smask, FIX)) {
245                 if (entry->nmeaInfo.fix == NMEA_FIX_3D) {
246                         assert(add ? (counters->fix3d < maxCount):(counters->fix3d > 0));
247                         counters->fix3d += amount;
248                 } else if (entry->nmeaInfo.fix == NMEA_FIX_2D) {
249                         assert(add ? (counters->fix2d < maxCount):(counters->fix2d > 0));
250                         counters->fix2d += amount;
251                 } else {
252                         assert(add ? (counters->fixBad < maxCount):(counters->fixBad > 0));
253                         counters->fixBad += amount;
254                 }
255         }
256 }
257
258 /**
259  Determine the new smask, sig and fix of the average position based on the
260  counters. The relevant smask bits (like GPGGA) are only set when all entries
261  in the average list have that bit set. The sig and fix will be set to the
262  lowest/worst value of all entries and will only be set to the highest/best
263  value when all entries in the average list are set to the highest/best value.
264
265  @param positionAverageList
266  The position average list
267  */
268 static void determineCumulativeSmaskSigFix(
269                 PositionAverageList * positionAverageList) {
270         PositionUpdateEntry * cumulative =
271                         &positionAverageList->positionAverageCumulative;
272         PositionUpdateCounters * counters = &positionAverageList->counters;
273         unsigned long long count = positionAverageList->entriesCount;
274
275         /* smask */
276         cumulative->nmeaInfo.smask = 0;
277
278         if (counters->gpgga >= count) {
279                 cumulative->nmeaInfo.smask |= GPGGA;
280         }
281
282         if (counters->gpgsa >= count) {
283                 cumulative->nmeaInfo.smask |= GPGSA;
284         }
285
286         if (counters->gpgsv >= count) {
287                 cumulative->nmeaInfo.smask |= GPGSV;
288         }
289
290         if (counters->gprmc >= count) {
291                 cumulative->nmeaInfo.smask |= GPRMC;
292         }
293
294         if (counters->gpvtg >= count) {
295                 cumulative->nmeaInfo.smask |= GPVTG;
296         }
297
298         /* sig */
299         cumulative->nmeaInfo.sig = NMEA_SIG_BAD;
300         if (nmeaInfoHasField(cumulative->nmeaInfo.smask, SIG)) {
301                 if (counters->sigBad == 0) {
302                         if (counters->sigHigh >= count) {
303                                 cumulative->nmeaInfo.sig = NMEA_SIG_HIGH;
304                         } else if (counters->sigMid > 0) {
305                                 cumulative->nmeaInfo.sig = NMEA_SIG_MID;
306                         } else if (counters->sigLow > 0) {
307                                 cumulative->nmeaInfo.sig = NMEA_SIG_LOW;
308                         }
309                 }
310         }
311
312         /* fix */
313         cumulative->nmeaInfo.fix = NMEA_FIX_BAD;
314         if (nmeaInfoHasField(cumulative->nmeaInfo.smask, FIX)) {
315                 if (counters->fixBad == 0) {
316                         if (counters->fix3d >= count) {
317                                 cumulative->nmeaInfo.fix = NMEA_FIX_3D;
318                         } else if (counters->fix2d > 0) {
319                                 cumulative->nmeaInfo.fix = NMEA_FIX_2D;
320                         }
321                 }
322         }
323 }
324
325 /**
326  Add/remove a position update entry to/from the average position list, updates
327  the counters, adjusts the entriesCount and redetermines the cumulative
328  smask, sig and fix.
329
330  @param positionAverageList
331  The position average list
332  @param entry
333  The entry to add/remove
334  @param add
335  True when the entry must be added to the list, false when it must be removed
336  */
337 static void addOrRemoveEntryToFromCumulativeAverage(
338                 PositionAverageList * positionAverageList, PositionUpdateEntry * entry,
339                 bool add) {
340         PositionUpdateEntry * cumulative =
341                         &positionAverageList->positionAverageCumulative;
342
343 #if defined(PUD_DUMP_AVERAGING)
344         dump_nmeaInfo(&entry->nmeaInfo, "addOrRemoveEntryToFromCumulativeAverage: entry");
345         dump_nmeaInfo(&cumulative->nmeaInfo, "addOrRemoveEntryToFromCumulativeAverage: positionAverageList->positionAverageCumulative (before)");
346         olsr_printf(0,
347                         "addOrRemoveEntryToFromCumulativeAverage: positionAverageList->counters (before)\n"
348                         "  gpgga   = %llu\n"
349                         "  gpgsa   = %llu\n"
350                         "  gpgsv   = %llu\n"
351                         "  gprmc   = %llu\n"
352                         "  gpvtg   = %llu\n"
353                         "  sigBad  = %llu\n"
354                         "  sigLow  = %llu\n"
355                         "  sigMid  = %llu\n"
356                         "  sigHigh = %llu\n"
357                         "  fixBad  = %llu\n"
358                         "  fix2d   = %llu\n"
359                         "  fix3d   = %llu\n"
360                         "\n",
361                         positionAverageList->counters.gpgga,
362                         positionAverageList->counters.gpgsa,
363                         positionAverageList->counters.gpgsv,
364                         positionAverageList->counters.gprmc,
365                         positionAverageList->counters.gpvtg,
366                         positionAverageList->counters.sigBad,
367                         positionAverageList->counters.sigLow,
368                         positionAverageList->counters.sigMid,
369                         positionAverageList->counters.sigHigh,
370                         positionAverageList->counters.fixBad,
371                         positionAverageList->counters.fix2d,
372                         positionAverageList->counters.fix3d
373         );
374 #endif /* PUD_DUMP_AVERAGING */
375
376         if (!add) {
377                 assert(positionAverageList->entriesCount >= positionAverageList->entriesMaxCount);
378                 assert(entry == getPositionAverageEntry(positionAverageList, OLDEST));
379
380                 /* do not touch smask */
381
382                 /* do not touch utc */
383
384                 /* do not touch sig */
385                 /* do not touch fix */
386
387                 /* do not touch satinfo */
388         } else {
389                 assert(positionAverageList->entriesCount < positionAverageList->entriesMaxCount);
390                 assert(entry == getPositionAverageEntry(positionAverageList, INCOMING));
391
392                 /* smask at the end */
393
394                 /* use the latest utc */
395                 cumulative->nmeaInfo.utc = entry->nmeaInfo.utc;
396
397                 /* sig at the end */
398                 /* fix at the end */
399
400                 /* use the latest satinfo */
401                 cumulative->nmeaInfo.satinfo = entry->nmeaInfo.satinfo;
402         }
403
404         /* PDOP, HDOP, VDOP */
405         cumulative->nmeaInfo.PDOP += add ? entry->nmeaInfo.PDOP
406                         : -entry->nmeaInfo.PDOP;
407         cumulative->nmeaInfo.HDOP += add ? entry->nmeaInfo.HDOP
408                         : -entry->nmeaInfo.HDOP;
409         cumulative->nmeaInfo.VDOP += add ? entry->nmeaInfo.VDOP
410                         : -entry->nmeaInfo.VDOP;
411
412         /* lat, lon */
413         cumulative->nmeaInfo.lat += add ? entry->nmeaInfo.lat
414                         : -entry->nmeaInfo.lat;
415         cumulative->nmeaInfo.lon += add ? entry->nmeaInfo.lon
416                         : -entry->nmeaInfo.lon;
417
418         /* elv, speed, direction, declination */
419         cumulative->nmeaInfo.elv += add ? entry->nmeaInfo.elv
420                         : -entry->nmeaInfo.elv;
421         cumulative->nmeaInfo.speed += add ? entry->nmeaInfo.speed
422                         : -entry->nmeaInfo.speed;
423         cumulative->nmeaInfo.direction += add ? entry->nmeaInfo.direction
424                         : -entry->nmeaInfo.direction;
425         cumulative->nmeaInfo.declination += add ? entry->nmeaInfo.declination
426                         : -entry->nmeaInfo.declination;
427
428         positionAverageList->entriesCount += (add ? 1 : -1);
429
430         updateCounters(positionAverageList, entry, add);
431         determineCumulativeSmaskSigFix(positionAverageList);
432
433 #if defined(PUD_DUMP_AVERAGING)
434         dump_nmeaInfo(&cumulative->nmeaInfo, "addOrRemoveEntryToFromCumulativeAverage: positionAverageList->positionAverageCumulative (after)");
435         olsr_printf(0,
436                         "addOrRemoveEntryToFromCumulativeAverage: positionAverageList->counters (before)\n"
437                         "  gpgga   = %llu\n"
438                         "  gpgsa   = %llu\n"
439                         "  gpgsv   = %llu\n"
440                         "  gprmc   = %llu\n"
441                         "  gpvtg   = %llu\n"
442                         "  sigBad  = %llu\n"
443                         "  sigLow  = %llu\n"
444                         "  sigMid  = %llu\n"
445                         "  sigHigh = %llu\n"
446                         "  fixBad  = %llu\n"
447                         "  fix2d   = %llu\n"
448                         "  fix3d   = %llu\n"
449                         "\n",
450                         positionAverageList->counters.gpgga,
451                         positionAverageList->counters.gpgsa,
452                         positionAverageList->counters.gpgsv,
453                         positionAverageList->counters.gprmc,
454                         positionAverageList->counters.gpvtg,
455                         positionAverageList->counters.sigBad,
456                         positionAverageList->counters.sigLow,
457                         positionAverageList->counters.sigMid,
458                         positionAverageList->counters.sigHigh,
459                         positionAverageList->counters.fixBad,
460                         positionAverageList->counters.fix2d,
461                         positionAverageList->counters.fix3d
462         );
463 #endif /* PUD_DUMP_AVERAGING */
464 }
465
466 /**
467  Update the average position from the cumulative average position. Basically
468  divide all relevant cumulative values by the number of entries in the list.
469
470  @param positionAverageList
471  The position average list
472  */
473 static void updatePositionAverageFromCumulative(
474                 PositionAverageList * positionAverageList) {
475         double divider = positionAverageList->entriesCount;
476
477 #if defined(PUD_DUMP_AVERAGING)
478         dump_nmeaInfo(&positionAverageList->positionAverage.nmeaInfo, "updatePositionAverageFromCumulative: positionAverageList->positionAverage (before)");
479 #endif /* PUD_DUMP_AVERAGING */
480
481         memcpy(&positionAverageList->positionAverage,
482                         &positionAverageList->positionAverageCumulative,
483                         sizeof(positionAverageList->positionAverage));
484
485         /* smask: use from cumulative average */
486
487         /* utc: use from cumulative average */
488
489         /* sig: use from cumulative average */
490         /* fix: use from cumulative average */
491
492         if (divider > 1.0) {
493                 positionAverageList->positionAverage.nmeaInfo.PDOP /= divider;
494                 positionAverageList->positionAverage.nmeaInfo.HDOP /= divider;
495                 positionAverageList->positionAverage.nmeaInfo.VDOP /= divider;
496
497                 positionAverageList->positionAverage.nmeaInfo.lat /= divider;
498                 positionAverageList->positionAverage.nmeaInfo.lon /= divider;
499
500                 positionAverageList->positionAverage.nmeaInfo.elv /= divider;
501                 positionAverageList->positionAverage.nmeaInfo.speed /= divider;
502                 positionAverageList->positionAverage.nmeaInfo.direction /= divider;
503                 positionAverageList->positionAverage.nmeaInfo.declination /= divider;
504         }
505
506         /* satinfo: use from average */
507
508 #if defined(PUD_DUMP_AVERAGING)
509         dump_nmeaInfo(&positionAverageList->positionAverage.nmeaInfo, "updatePositionAverageFromCumulative: positionAverageList->positionAverage (after)");
510 #endif /* PUD_DUMP_AVERAGING */
511 }
512
513 /**
514  Add a new (incoming) position update to the position average list
515
516  @param positionAverageList
517  The position average list
518  @param newEntry
519  The new (incoming) position update (must be the same as the one returned from
520  the function getPositionAverageEntryForIncoming:INCOMING)
521  */
522 void addNewPositionToAverage(PositionAverageList * positionAverageList,
523                 PositionUpdateEntry * newEntry) {
524         assert (positionAverageList != NULL);
525         assert (newEntry == getPositionAverageEntry(positionAverageList, INCOMING));
526
527 #if defined(PUD_DUMP_AVERAGING)
528         dump_nmeaInfo(&newEntry->nmeaInfo, "addNewPositionToAverage: newEntry");
529         olsr_printf(0, "addNewPositionToAverage: positionAverageList->newestEntryIndex = %llu (before)\n", NEWESTINDEX(positionAverageList));
530         dump_nmeaInfo(&positionAverageList->positionAverageCumulative.nmeaInfo, "addNewPositionToAverage: positionAverageList->positionAverageCumulative.nmeaInfo (before)");
531 #endif /* PUD_DUMP_AVERAGING */
532
533         (void) pthread_mutex_lock(&positionAverageList->mutex);
534
535         if (positionAverageList->entriesCount
536                         >= positionAverageList->entriesMaxCount) {
537                 /* list is full, so first remove the oldest from the average */
538                 addOrRemoveEntryToFromCumulativeAverage(positionAverageList,
539                                 getPositionAverageEntry(positionAverageList, OLDEST), false);
540         }
541
542         /* now just add the new position */
543         addOrRemoveEntryToFromCumulativeAverage(positionAverageList, newEntry, true);
544
545         /* update the place where the new entry is stored */
546         positionAverageList->newestEntryIndex
547                         = WRAPINDEX(positionAverageList, NEWESTINDEX(positionAverageList) + 1);
548
549 #if defined(PUD_DUMP_AVERAGING)
550         olsr_printf(0, "addNewPositionToAverage: positionAverageList->newestEntryIndex = %llu (after)\n", NEWESTINDEX(positionAverageList));
551         dump_nmeaInfo(&positionAverageList->positionAverageCumulative.nmeaInfo, "addNewPositionToAverage: positionAverageList->positionAverageCumulative.nmeaInfo (before)");
552 #endif /* PUD_DUMP_AVERAGING */
553
554         /* update average position */
555         updatePositionAverageFromCumulative(positionAverageList);
556
557 #if defined(PUD_DUMP_AVERAGING)
558         dump_nmeaInfo(&positionAverageList->positionAverage.nmeaInfo, "addNewPositionToAverage: positionAverageList->positionAverage.nmeaInfo (after)");
559 #endif /* PUD_DUMP_AVERAGING */
560
561         (void) pthread_mutex_unlock(&positionAverageList->mutex);
562 }