4c9d9ed6593dfec859bbf496ebfcad5379338c9d
[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
429  * the components. Set to NULL when the angle is not present in the input data.
430  *
431  */
432 static void calculateAngleComponents(AngleComponents * components, double * angle) {
433         if (!components)
434                 return;
435
436         if (!angle) {
437                 components->x = 0.0;
438                 components->y = 0.0;
439                 return;
440         }
441
442         components->x = cos(nmea_degree2radian(*angle));
443         components->y = sin(nmea_degree2radian(*angle));
444 }
445
446 /**
447  * Calculate angle from its components
448  *
449  * @param components a pointer to the components structure
450  * @return angle the angle (in degrees)
451  */
452 static double calculateAngle(AngleComponents * components) {
453         if (!components)
454                 return 0;
455
456         return nmea_radian2degree(atan2(components->y, components->x));
457 }
458
459 /**
460  * Add the src angle components to the dst angle components (accumulate)
461  *
462  * @param dst a pointer to the destination components structure
463  * @param src a pointer to the source components structure
464  * @param add true to add, false to subtract
465  */
466 static void addAngleComponents(AngleComponents * dst, AngleComponents * src, bool add) {
467         if (!dst || !src)
468                 return;
469
470         dst->x += add ? src->x : -src->x;
471         dst->y += add ? src->y : -src->y;
472 }
473
474 /**
475  Add/remove a position update entry to/from the average position list, updates
476  the counters, adjusts the entriesCount and redetermines the cumulative
477  smask, sig and fix.
478
479  @param positionAverageList
480  The position average list
481  @param entry
482  The entry to add/remove
483  @param add
484  True when the entry must be added to the list, false when it must be removed
485  */
486 static void addOrRemoveEntryToFromCumulativeAverage(
487                 PositionAverageList * positionAverageList, PositionUpdateEntry * entry,
488                 bool add) {
489         PositionUpdateEntry * cumulative =
490                         &positionAverageList->positionAverageCumulative;
491
492         if (!add) {
493                 assert(positionAverageList->entriesCount >= positionAverageList->entriesMaxCount);
494                 assert(entry == getPositionAverageEntry(positionAverageList, OLDEST));
495
496                 /* do not touch present */
497                 /* do not touch smask */
498
499                 /* do not touch utc */
500
501                 /* do not touch sig */
502                 /* do not touch fix */
503
504                 /* do not touch satinfo */
505         } else {
506                 assert(positionAverageList->entriesCount < positionAverageList->entriesMaxCount);
507                 assert(entry == getPositionAverageEntry(positionAverageList, INCOMING));
508
509                 /* present at the end */
510                 /* smask at the end */
511
512                 /* use the latest utc */
513                 cumulative->nmeaInfo.utc = entry->nmeaInfo.utc;
514
515                 /* sig at the end */
516                 /* fix at the end */
517
518                 /* use the latest satinfo */
519                 cumulative->nmeaInfo.satinfo = entry->nmeaInfo.satinfo;
520         }
521
522         /* PDOP, HDOP, VDOP */
523         cumulative->nmeaInfo.PDOP += add ? entry->nmeaInfo.PDOP
524                         : -entry->nmeaInfo.PDOP;
525         cumulative->nmeaInfo.HDOP += add ? entry->nmeaInfo.HDOP
526                         : -entry->nmeaInfo.HDOP;
527         cumulative->nmeaInfo.VDOP += add ? entry->nmeaInfo.VDOP
528                         : -entry->nmeaInfo.VDOP;
529
530         /* lat, lon */
531         cumulative->nmeaInfo.lat += add ? entry->nmeaInfo.lat
532                         : -entry->nmeaInfo.lat;
533         cumulative->nmeaInfo.lon += add ? entry->nmeaInfo.lon
534                         : -entry->nmeaInfo.lon;
535
536         /* elv, speed */
537         cumulative->nmeaInfo.elv += add ? entry->nmeaInfo.elv
538                         : -entry->nmeaInfo.elv;
539         cumulative->nmeaInfo.speed += add ? entry->nmeaInfo.speed
540                         : -entry->nmeaInfo.speed;
541
542         /* track, mtrack, magvar */
543         addAngleComponents(&cumulative->track, &entry->track, add);
544         addAngleComponents(&cumulative->mtrack, &entry->mtrack, add);
545         addAngleComponents(&cumulative->magvar, &entry->magvar, add);
546
547         /* adjust list count */
548         positionAverageList->entriesCount += (add ? 1 : -1);
549
550         updateCounters(positionAverageList, entry, add);
551         determineCumulativePresentSmaskSigFix(positionAverageList);
552 }
553
554 /**
555  Update the average position from the cumulative average position. Basically
556  divide all relevant cumulative values by the number of entries in the list.
557
558  @param positionAverageList
559  The position average list
560  */
561 static void updatePositionAverageFromCumulative(
562                 PositionAverageList * positionAverageList) {
563         double divider = positionAverageList->entriesCount;
564
565         positionAverageList->positionAverage = positionAverageList->positionAverageCumulative;
566
567         /* smask: use from cumulative average */
568
569         /* utc: use from cumulative average */
570
571         /* sig: use from cumulative average */
572         /* fix: use from cumulative average */
573
574         if (divider > 1.0) {
575                 positionAverageList->positionAverage.nmeaInfo.PDOP /= divider;
576                 positionAverageList->positionAverage.nmeaInfo.HDOP /= divider;
577                 positionAverageList->positionAverage.nmeaInfo.VDOP /= divider;
578
579                 positionAverageList->positionAverage.nmeaInfo.lat /= divider;
580                 positionAverageList->positionAverage.nmeaInfo.lon /= divider;
581
582                 positionAverageList->positionAverage.nmeaInfo.elv /= divider;
583                 positionAverageList->positionAverage.nmeaInfo.speed /= divider;
584
585                 positionAverageList->positionAverage.nmeaInfo.track = calculateAngle(&positionAverageList->positionAverageCumulative.track);
586                 positionAverageList->positionAverage.nmeaInfo.mtrack = calculateAngle(&positionAverageList->positionAverageCumulative.mtrack);
587                 positionAverageList->positionAverage.nmeaInfo.magvar = calculateAngle(&positionAverageList->positionAverageCumulative.magvar);
588         }
589
590         /* satinfo: use from average */
591 }
592
593 /**
594  Add a new (incoming) position update to the position average list
595
596  @param positionAverageList
597  The position average list
598  @param newEntry
599  The new (incoming) position update (must be the same as the one returned from
600  the function getPositionAverageEntryForIncoming:INCOMING)
601  */
602 void addNewPositionToAverage(PositionAverageList * positionAverageList,
603                 PositionUpdateEntry * newEntry) {
604         assert (positionAverageList != NULL);
605         assert (newEntry == getPositionAverageEntry(positionAverageList, INCOMING));
606
607         if (positionAverageList->entriesCount
608                         >= positionAverageList->entriesMaxCount) {
609                 /* list is full, so first remove the oldest from the average */
610                 addOrRemoveEntryToFromCumulativeAverage(positionAverageList,
611                                 getPositionAverageEntry(positionAverageList, OLDEST), false);
612         }
613
614         /* calculate the angle components */
615         calculateAngleComponents(&newEntry->track, nmea_INFO_is_present(newEntry->nmeaInfo.present, TRACK) ? &newEntry->nmeaInfo.track : NULL);
616         calculateAngleComponents(&newEntry->mtrack, nmea_INFO_is_present(newEntry->nmeaInfo.present, MTRACK) ? &newEntry->nmeaInfo.mtrack : NULL);
617         calculateAngleComponents(&newEntry->magvar, nmea_INFO_is_present(newEntry->nmeaInfo.present, MAGVAR) ? &newEntry->nmeaInfo.magvar : NULL);
618
619         /* now just add the new position */
620         addOrRemoveEntryToFromCumulativeAverage(positionAverageList, newEntry, true);
621
622         /* update the place where the new entry is stored */
623         positionAverageList->newestEntryIndex
624                         = WRAPINDEX(positionAverageList, NEWESTINDEX(positionAverageList) + 1);
625
626         /* update average position */
627         updatePositionAverageFromCumulative(positionAverageList);
628 }