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