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