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