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