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