PUD: remove debug code
[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 #ifndef NDEBUG
161         unsigned long long maxCount = positionAverageList->entriesMaxCount;
162 #endif
163         int amount = (add ? 1 : -1);
164
165         /* smask */
166         if ((entry->nmeaInfo.smask & GPGGA) != 0) {
167                 assert(add ? (counters->gpgga < maxCount):(counters->gpgga > 0));
168                 counters->gpgga += amount;
169         }
170         if ((entry->nmeaInfo.smask & GPGSA) != 0) {
171                 assert(add ? (counters->gpgsa < maxCount):(counters->gpgsa > 0));
172                 counters->gpgsa += amount;
173         }
174         if ((entry->nmeaInfo.smask & GPGSV) != 0) {
175                 assert(add ? (counters->gpgsv < maxCount):(counters->gpgsv > 0));
176                 counters->gpgsv += amount;
177         }
178         if ((entry->nmeaInfo.smask & GPRMC) != 0) {
179                 assert(add ? (counters->gprmc < maxCount):(counters->gprmc > 0));
180                 counters->gprmc += amount;
181         }
182         if ((entry->nmeaInfo.smask & GPVTG) != 0) {
183                 assert(add ? (counters->gpvtg < maxCount):(counters->gpvtg > 0));
184                 counters->gpvtg += amount;
185         }
186
187         /* sig */
188         if (nmea_INFO_has_field(entry->nmeaInfo.smask, SIG)) {
189                 if (entry->nmeaInfo.sig == NMEA_SIG_HIGH) {
190                         assert(add ? (counters->sigHigh < maxCount):(counters->sigHigh > 0));
191                         counters->sigHigh += amount;
192                 } else if (entry->nmeaInfo.sig == NMEA_SIG_MID) {
193                         assert(add ? (counters->sigMid < maxCount):(counters->sigMid > 0));
194                         counters->sigMid += amount;
195                 } else if (entry->nmeaInfo.sig == NMEA_SIG_LOW) {
196                         assert(add ? (counters->sigLow < maxCount):(counters->sigLow > 0));
197                         counters->sigLow += amount;
198                 } else {
199                         assert(add ? (counters->sigBad < maxCount):(counters->sigBad > 0));
200                         counters->sigBad += amount;
201                 }
202         }
203
204         /* fix */
205         if (nmea_INFO_has_field(entry->nmeaInfo.smask, FIX)) {
206                 if (entry->nmeaInfo.fix == NMEA_FIX_3D) {
207                         assert(add ? (counters->fix3d < maxCount):(counters->fix3d > 0));
208                         counters->fix3d += amount;
209                 } else if (entry->nmeaInfo.fix == NMEA_FIX_2D) {
210                         assert(add ? (counters->fix2d < maxCount):(counters->fix2d > 0));
211                         counters->fix2d += amount;
212                 } else {
213                         assert(add ? (counters->fixBad < maxCount):(counters->fixBad > 0));
214                         counters->fixBad += amount;
215                 }
216         }
217 }
218
219 /**
220  Determine the new smask, sig and fix of the average position based on the
221  counters. The relevant smask bits (like GPGGA) are only set when all entries
222  in the average list have that bit set. The sig and fix will be set to the
223  lowest/worst value of all entries and will only be set to the highest/best
224  value when all entries in the average list are set to the highest/best value.
225
226  @param positionAverageList
227  The position average list
228  */
229 static void determineCumulativeSmaskSigFix(
230                 PositionAverageList * positionAverageList) {
231         PositionUpdateEntry * cumulative =
232                         &positionAverageList->positionAverageCumulative;
233         PositionUpdateCounters * counters = &positionAverageList->counters;
234         unsigned long long count = positionAverageList->entriesCount;
235
236         /* smask */
237         cumulative->nmeaInfo.smask = 0;
238
239         if (counters->gpgga >= count) {
240                 cumulative->nmeaInfo.smask |= GPGGA;
241         }
242
243         if (counters->gpgsa >= count) {
244                 cumulative->nmeaInfo.smask |= GPGSA;
245         }
246
247         if (counters->gpgsv >= count) {
248                 cumulative->nmeaInfo.smask |= GPGSV;
249         }
250
251         if (counters->gprmc >= count) {
252                 cumulative->nmeaInfo.smask |= GPRMC;
253         }
254
255         if (counters->gpvtg >= count) {
256                 cumulative->nmeaInfo.smask |= GPVTG;
257         }
258
259         /* sig */
260         cumulative->nmeaInfo.sig = NMEA_SIG_BAD;
261         if (nmea_INFO_has_field(cumulative->nmeaInfo.smask, SIG)) {
262                 if (counters->sigBad == 0) {
263                         if (counters->sigHigh >= count) {
264                                 cumulative->nmeaInfo.sig = NMEA_SIG_HIGH;
265                         } else if (counters->sigMid > 0) {
266                                 cumulative->nmeaInfo.sig = NMEA_SIG_MID;
267                         } else if (counters->sigLow > 0) {
268                                 cumulative->nmeaInfo.sig = NMEA_SIG_LOW;
269                         }
270                 }
271         }
272
273         /* fix */
274         cumulative->nmeaInfo.fix = NMEA_FIX_BAD;
275         if (nmea_INFO_has_field(cumulative->nmeaInfo.smask, FIX)) {
276                 if (counters->fixBad == 0) {
277                         if (counters->fix3d >= count) {
278                                 cumulative->nmeaInfo.fix = NMEA_FIX_3D;
279                         } else if (counters->fix2d > 0) {
280                                 cumulative->nmeaInfo.fix = NMEA_FIX_2D;
281                         }
282                 }
283         }
284 }
285
286 /**
287  Add/remove a position update entry to/from the average position list, updates
288  the counters, adjusts the entriesCount and redetermines the cumulative
289  smask, sig and fix.
290
291  @param positionAverageList
292  The position average list
293  @param entry
294  The entry to add/remove
295  @param add
296  True when the entry must be added to the list, false when it must be removed
297  */
298 static void addOrRemoveEntryToFromCumulativeAverage(
299                 PositionAverageList * positionAverageList, PositionUpdateEntry * entry,
300                 bool add) {
301         PositionUpdateEntry * cumulative =
302                         &positionAverageList->positionAverageCumulative;
303
304         if (!add) {
305                 assert(positionAverageList->entriesCount >= positionAverageList->entriesMaxCount);
306                 assert(entry == getPositionAverageEntry(positionAverageList, OLDEST));
307
308                 /* do not touch smask */
309
310                 /* do not touch utc */
311
312                 /* do not touch sig */
313                 /* do not touch fix */
314
315                 /* do not touch satinfo */
316         } else {
317                 assert(positionAverageList->entriesCount < positionAverageList->entriesMaxCount);
318                 assert(entry == getPositionAverageEntry(positionAverageList, INCOMING));
319
320                 /* smask at the end */
321
322                 /* use the latest utc */
323                 cumulative->nmeaInfo.utc = entry->nmeaInfo.utc;
324
325                 /* sig at the end */
326                 /* fix at the end */
327
328                 /* use the latest satinfo */
329                 cumulative->nmeaInfo.satinfo = entry->nmeaInfo.satinfo;
330         }
331
332         /* PDOP, HDOP, VDOP */
333         cumulative->nmeaInfo.PDOP += add ? entry->nmeaInfo.PDOP
334                         : -entry->nmeaInfo.PDOP;
335         cumulative->nmeaInfo.HDOP += add ? entry->nmeaInfo.HDOP
336                         : -entry->nmeaInfo.HDOP;
337         cumulative->nmeaInfo.VDOP += add ? entry->nmeaInfo.VDOP
338                         : -entry->nmeaInfo.VDOP;
339
340         /* lat, lon */
341         cumulative->nmeaInfo.lat += add ? entry->nmeaInfo.lat
342                         : -entry->nmeaInfo.lat;
343         cumulative->nmeaInfo.lon += add ? entry->nmeaInfo.lon
344                         : -entry->nmeaInfo.lon;
345
346         /* elv, speed, direction, declination */
347         cumulative->nmeaInfo.elv += add ? entry->nmeaInfo.elv
348                         : -entry->nmeaInfo.elv;
349         cumulative->nmeaInfo.speed += add ? entry->nmeaInfo.speed
350                         : -entry->nmeaInfo.speed;
351         cumulative->nmeaInfo.direction += add ? entry->nmeaInfo.direction
352                         : -entry->nmeaInfo.direction;
353         cumulative->nmeaInfo.declination += add ? entry->nmeaInfo.declination
354                         : -entry->nmeaInfo.declination;
355
356         positionAverageList->entriesCount += (add ? 1 : -1);
357
358         updateCounters(positionAverageList, entry, add);
359         determineCumulativeSmaskSigFix(positionAverageList);
360 }
361
362 /**
363  Update the average position from the cumulative average position. Basically
364  divide all relevant cumulative values by the number of entries in the list.
365
366  @param positionAverageList
367  The position average list
368  */
369 static void updatePositionAverageFromCumulative(
370                 PositionAverageList * positionAverageList) {
371         double divider = positionAverageList->entriesCount;
372
373         positionAverageList->positionAverage = positionAverageList->positionAverageCumulative;
374
375         /* smask: use from cumulative average */
376
377         /* utc: use from cumulative average */
378
379         /* sig: use from cumulative average */
380         /* fix: use from cumulative average */
381
382         if (divider > 1.0) {
383                 positionAverageList->positionAverage.nmeaInfo.PDOP /= divider;
384                 positionAverageList->positionAverage.nmeaInfo.HDOP /= divider;
385                 positionAverageList->positionAverage.nmeaInfo.VDOP /= divider;
386
387                 positionAverageList->positionAverage.nmeaInfo.lat /= divider;
388                 positionAverageList->positionAverage.nmeaInfo.lon /= divider;
389
390                 positionAverageList->positionAverage.nmeaInfo.elv /= divider;
391                 positionAverageList->positionAverage.nmeaInfo.speed /= divider;
392                 positionAverageList->positionAverage.nmeaInfo.direction /= divider;
393                 positionAverageList->positionAverage.nmeaInfo.declination /= divider;
394         }
395
396         /* satinfo: use from average */
397 }
398
399 /**
400  Add a new (incoming) position update to the position average list
401
402  @param positionAverageList
403  The position average list
404  @param newEntry
405  The new (incoming) position update (must be the same as the one returned from
406  the function getPositionAverageEntryForIncoming:INCOMING)
407  */
408 void addNewPositionToAverage(PositionAverageList * positionAverageList,
409                 PositionUpdateEntry * newEntry) {
410         assert (positionAverageList != NULL);
411         assert (newEntry == getPositionAverageEntry(positionAverageList, INCOMING));
412
413         if (positionAverageList->entriesCount
414                         >= positionAverageList->entriesMaxCount) {
415                 /* list is full, so first remove the oldest from the average */
416                 addOrRemoveEntryToFromCumulativeAverage(positionAverageList,
417                                 getPositionAverageEntry(positionAverageList, OLDEST), false);
418         }
419
420         /* now just add the new position */
421         addOrRemoveEntryToFromCumulativeAverage(positionAverageList, newEntry, true);
422
423         /* update the place where the new entry is stored */
424         positionAverageList->newestEntryIndex
425                         = WRAPINDEX(positionAverageList, NEWESTINDEX(positionAverageList) + 1);
426
427         /* update average position */
428         updatePositionAverageFromCumulative(positionAverageList);
429 }