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