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