Merge pull request #78 from ffontaine/master
[olsrd.git] / lib / pud / src / posAvg.c
1 /*
2  * The olsr.org Optimized Link-State Routing daemon (olsrd)
3  *
4  * (c) by the OLSR project
5  *
6  * See our Git repository to find out who worked on this file
7  * and thus is a copyright holder on it.
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  *   notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above copyright
18  *   notice, this list of conditions and the following disclaimer in
19  *   the documentation and/or other materials provided with the
20  *   distribution.
21  * * Neither the name of olsr.org, olsrd nor the names of its
22  *   contributors may be used to endorse or promote products derived
23  *   from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  * Visit http://www.olsr.org for more information.
39  *
40  * If you find this software useful feel free to make a donation
41  * to the project. For more information see the website or contact
42  * the copyright holders.
43  *
44  */
45
46 #include "posAvg.h"
47
48 /* Plugin includes */
49
50 /* OLSR includes */
51 #include "olsr.h"
52
53 /* System includes */
54 #include <assert.h>
55 #include <math.h>
56 #include <string.h>
57 #include <nmealib/info.h>
58 #include <nmealib/nmath.h>
59 #include <nmealib/sentence.h>
60
61 /* Defines */
62
63 #define LISTSIZE(x)                     (((x)->entriesMaxCount) + 1) /* always valid */
64 #define NEWESTINDEX(x)          ((x)->newestEntryIndex) /* always valid */
65 #define WRAPINDEX(x, i)         ((i) % LISTSIZE(x)) /* always valid for i>=0 */
66 #define INCOMINGINDEX(x)        WRAPINDEX(x, (NEWESTINDEX(x) + 1)) /* always valid */
67 #define OLDESTINDEX(x)          (((x)->entriesCount > 1) ? WRAPINDEX(x, (INCOMINGINDEX(x) + LISTSIZE(x) - (x)->entriesCount)) : NEWESTINDEX(x)) /* always valid */
68
69 /**
70  Flush/empty the position average list
71
72  @param positionAverageList
73  The position average list
74  */
75 void flushPositionAverageList(PositionAverageList * positionAverageList) {
76         assert (positionAverageList != NULL);
77
78         positionAverageList->entriesCount = 0;
79         memset(&positionAverageList->counters, 0,
80                         sizeof(positionAverageList->counters));
81
82         nmeaInfoClear(&positionAverageList->positionAverageCumulative.nmeaInfo);
83         nmeaTimeSet(&positionAverageList->positionAverageCumulative.nmeaInfo.utc, &positionAverageList->positionAverageCumulative.nmeaInfo.present, NULL);
84         memset(&positionAverageList->positionAverageCumulative.track, 0, sizeof(positionAverageList->positionAverageCumulative.track));
85         memset(&positionAverageList->positionAverageCumulative.mtrack, 0, sizeof(positionAverageList->positionAverageCumulative.mtrack));
86         memset(&positionAverageList->positionAverageCumulative.magvar, 0, sizeof(positionAverageList->positionAverageCumulative.magvar));
87
88         nmeaInfoClear(&positionAverageList->positionAverage.nmeaInfo);
89         nmeaTimeSet(&positionAverageList->positionAverage.nmeaInfo.utc, &positionAverageList->positionAverage.nmeaInfo.present, NULL);
90 }
91
92 /**
93  Initialise the position average list: allocate memory for the entries and
94  reset fields.
95
96  @param positionAverageList
97  The position average list
98  @param maxEntries
99  The maximum number of entries in the list (the number of entries that should
100  be averaged)
101
102  @return
103  - false on failure
104  - true otherwise
105  */
106 bool initPositionAverageList(PositionAverageList * positionAverageList,
107                 unsigned long long maxEntries) {
108         void * p;
109
110         if (positionAverageList == NULL) {
111                 return false;
112         }
113         if (maxEntries < 2) {
114                 return false;
115         }
116
117         p = olsr_malloc((maxEntries + 1) * sizeof(PositionUpdateEntry),
118                         "PositionAverageEntry entries for PositionAverageList (PUD)");
119         if (p == NULL) {
120                 return false;
121         }
122
123         positionAverageList->entriesMaxCount = maxEntries;
124         positionAverageList->entries = p;
125         positionAverageList->newestEntryIndex = 0;
126
127         flushPositionAverageList(positionAverageList);
128
129         return true;
130 }
131
132 /**
133  Clean up the position average list: free memory and reset fields.
134
135  @param positionAverageList
136  The position average list
137  */
138 void destroyPositionAverageList(PositionAverageList * positionAverageList) {
139         assert (positionAverageList != NULL);
140
141         flushPositionAverageList(positionAverageList);
142
143         if (positionAverageList->entries != NULL) {
144                 free(positionAverageList->entries);
145                 positionAverageList->entries = NULL;
146         }
147
148         positionAverageList->entriesMaxCount = 0;
149         positionAverageList->newestEntryIndex = 0;
150 }
151
152 /**
153  Get the entry for a certain type of position update.
154
155  @param positionAvgList
156  The position average list
157  @param positionType
158  The type of the position of the average entry
159
160  @return
161  A pointer to the requested position update entry
162  */
163 PositionUpdateEntry * getPositionAverageEntry(
164                 PositionAverageList * positionAvgList,
165                 AverageEntryPositionType positionType) {
166         PositionUpdateEntry * r = NULL;
167
168         switch (positionType) {
169                 case OLDEST:
170                         assert(positionAvgList->entriesCount >= positionAvgList->entriesMaxCount);
171                         r = &positionAvgList->entries[OLDESTINDEX(positionAvgList)];
172                         break;
173
174                 case INCOMING:
175                         r = &positionAvgList->entries[INCOMINGINDEX(positionAvgList)];
176                         break;
177
178                 case NEWEST:
179                         r = &positionAvgList->entries[NEWESTINDEX(positionAvgList)];
180                         break;
181
182                 case AVERAGECUMULATIVE:
183                         r = &positionAvgList->positionAverageCumulative;
184                         break;
185
186                 case AVERAGE:
187                         r = &positionAvgList->positionAverage;
188                         break;
189
190                 default:
191                         r = NULL;
192                         break;
193         }
194
195         return r;
196 }
197
198 /**
199  Update position average present, smask and fix counters for a new entry or for
200  an entry that is/will be removed. Update the respective counters when the smask
201  of the entry has the corresponding flag set. The fix counters count the fix
202  values separately.
203
204  @param positionAverageList
205  The position average list
206  @param entry
207  The entry to update the counters from
208  @param add
209  True when updating the counters for a new entry, false for an entry that
210  is/will be removed
211  */
212 static void updateCounters(PositionAverageList * positionAverageList,
213                 PositionUpdateEntry * entry, bool add) {
214         PositionUpdateCounters * counters = &positionAverageList->counters;
215         uint32_t present = entry->nmeaInfo.present;
216         int smask = entry->nmeaInfo.smask;
217 #ifndef NDEBUG
218         unsigned long long maxCount = positionAverageList->entriesMaxCount;
219 #endif
220         int amount = (add ? 1 : -1);
221
222         /* present */
223         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_SMASK)) {
224                 assert(add ? (counters->smask < maxCount):(counters->smask > 0));
225                 counters->smask += amount;
226         }
227         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_UTCDATE)) {
228                 assert(add ? (counters->utcdate < maxCount):(counters->utcdate > 0));
229                 counters->utcdate += amount;
230         }
231         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_UTCTIME)) {
232                 assert(add ? (counters->utctime < maxCount):(counters->utctime > 0));
233                 counters->utctime += amount;
234         }
235         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_SIG)) {
236                 assert(add ? (counters->sig < maxCount):(counters->sig > 0));
237                 counters->sig += amount;
238         }
239         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_FIX)) {
240                 assert(add ? (counters->fix < maxCount):(counters->fix > 0));
241                 counters->fix += amount;
242         }
243         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_PDOP)) {
244                 assert(add ? (counters->pdop < maxCount):(counters->pdop > 0));
245                 counters->pdop += amount;
246         }
247         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_HDOP)) {
248                 assert(add ? (counters->hdop < maxCount):(counters->hdop > 0));
249                 counters->hdop += amount;
250         }
251         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_VDOP)) {
252                 assert(add ? (counters->vdop < maxCount):(counters->vdop > 0));
253                 counters->vdop += amount;
254         }
255         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_LAT)) {
256                 assert(add ? (counters->lat < maxCount):(counters->lat > 0));
257                 counters->lat += amount;
258         }
259         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_LON)) {
260                 assert(add ? (counters->lon < maxCount):(counters->lon > 0));
261                 counters->lon += amount;
262         }
263         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_ELV)) {
264                 assert(add ? (counters->elv < maxCount):(counters->elv > 0));
265                 counters->elv += amount;
266         }
267         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_SPEED)) {
268                 assert(add ? (counters->speed < maxCount):(counters->speed > 0));
269                 counters->speed += amount;
270         }
271         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_TRACK)) {
272                 assert(add ? (counters->track < maxCount):(counters->track > 0));
273                 counters->track += amount;
274         }
275         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_MTRACK)) {
276                 assert(add ? (counters->mtrack < maxCount):(counters->mtrack > 0));
277                 counters->mtrack += amount;
278         }
279         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_MAGVAR)) {
280                 assert(add ? (counters->magvar < maxCount):(counters->magvar > 0));
281                 counters->magvar += amount;
282         }
283         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_SATINUSECOUNT)) {
284                 assert(add ? (counters->satinusecount < maxCount):(counters->satinusecount > 0));
285                 counters->satinusecount += amount;
286         }
287         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_SATINUSE)) {
288                 assert(add ? (counters->satinuse < maxCount):(counters->satinuse > 0));
289                 counters->satinuse += amount;
290         }
291         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_SATINVIEWCOUNT)) {
292           assert(add ? (counters->satinviewcount < maxCount) : (counters->satinviewcount > 0));
293           counters->satinviewcount += amount;
294         }
295         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_SATINVIEW)) {
296                 assert(add ? (counters->satinview < maxCount):(counters->satinview > 0));
297                 counters->satinview += amount;
298         }
299         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_HEIGHT)) {
300           assert(add ? (counters->height < maxCount) : (counters->height > 0));
301           counters->height += amount;
302         }
303         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_DGPSAGE)) {
304           assert(add ? (counters->dgpsage < maxCount) : (counters->dgpsage > 0));
305           counters->dgpsage += amount;
306         }
307         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_DGPSSID)) {
308           assert(add ? (counters->dgpssid < maxCount) : (counters->dgpssid > 0));
309           counters->dgpssid += amount;
310         }
311
312         /* smask */
313         if ((smask & NMEALIB_SENTENCE_GPGGA) != 0) {
314                 assert(add ? (counters->gpgga < maxCount):(counters->gpgga > 0));
315                 counters->gpgga += amount;
316         }
317         if ((smask & NMEALIB_SENTENCE_GPGSA) != 0) {
318                 assert(add ? (counters->gpgsa < maxCount):(counters->gpgsa > 0));
319                 counters->gpgsa += amount;
320         }
321         if ((smask & NMEALIB_SENTENCE_GPGSV) != 0) {
322                 assert(add ? (counters->gpgsv < maxCount):(counters->gpgsv > 0));
323                 counters->gpgsv += amount;
324         }
325         if ((smask & NMEALIB_SENTENCE_GPRMC) != 0) {
326                 assert(add ? (counters->gprmc < maxCount):(counters->gprmc > 0));
327                 counters->gprmc += amount;
328         }
329         if ((smask & NMEALIB_SENTENCE_GPVTG) != 0) {
330                 assert(add ? (counters->gpvtg < maxCount):(counters->gpvtg > 0));
331                 counters->gpvtg += amount;
332         }
333
334         /* sig */
335         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_SIG)) {
336                 if (entry->nmeaInfo.sig == NMEALIB_SIG_SENSITIVE) {
337                         assert(add ? (counters->sigHigh < maxCount):(counters->sigHigh > 0));
338                         counters->sigHigh += amount;
339                 } else if (entry->nmeaInfo.sig == NMEALIB_SIG_DIFFERENTIAL) {
340                         assert(add ? (counters->sigMid < maxCount):(counters->sigMid > 0));
341                         counters->sigMid += amount;
342                 } else if (entry->nmeaInfo.sig == NMEALIB_SIG_FIX) {
343                         assert(add ? (counters->sigLow < maxCount):(counters->sigLow > 0));
344                         counters->sigLow += amount;
345                 } else {
346                         assert(add ? (counters->sigBad < maxCount):(counters->sigBad > 0));
347                         counters->sigBad += amount;
348                 }
349         }
350
351         /* fix */
352         if (nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_FIX)) {
353                 if (entry->nmeaInfo.fix == NMEALIB_FIX_3D) {
354                         assert(add ? (counters->fix3d < maxCount):(counters->fix3d > 0));
355                         counters->fix3d += amount;
356                 } else if (entry->nmeaInfo.fix == NMEALIB_FIX_2D) {
357                         assert(add ? (counters->fix2d < maxCount):(counters->fix2d > 0));
358                         counters->fix2d += amount;
359                 } else {
360                         assert(add ? (counters->fixBad < maxCount):(counters->fixBad > 0));
361                         counters->fixBad += amount;
362                 }
363         }
364 }
365
366 /**
367  Determine the new smask, sig and fix of the average position based on the
368  counters. The relevant smask bits (like GPGGA) are only set when all entries
369  in the average list have that bit set. The sig and fix will be set to the
370  lowest/worst value of all entries and will only be set to the highest/best
371  value when all entries in the average list are set to the highest/best value.
372
373  @param positionAverageList
374  The position average list
375  */
376 static void determineCumulativePresentSmaskSigFix(
377                 PositionAverageList * positionAverageList) {
378         PositionUpdateEntry * cumulative =
379                         &positionAverageList->positionAverageCumulative;
380         PositionUpdateCounters * counters = &positionAverageList->counters;
381         unsigned long long count = positionAverageList->entriesCount;
382
383         /* present */
384         cumulative->nmeaInfo.present = 0;
385
386         if (counters->smask >= count) {
387           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_SMASK);
388         }
389         if (counters->utcdate >= count) {
390           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_UTCDATE);
391         }
392         if (counters->utctime >= count) {
393           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_UTCTIME);
394         }
395         if (counters->sig >= count) {
396           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_SIG);
397         }
398         if (counters->fix >= count) {
399           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_FIX);
400         }
401         if (counters->pdop >= count) {
402           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_PDOP);
403         }
404         if (counters->hdop >= count) {
405           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_HDOP);
406         }
407         if (counters->vdop >= count) {
408           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_VDOP);
409         }
410         if (counters->lat >= count) {
411           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_LAT);
412         }
413         if (counters->lon >= count) {
414           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_LON);
415         }
416         if (counters->elv >= count) {
417           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_ELV);
418         }
419         if (counters->speed >= count) {
420           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_SPEED);
421         }
422         if (counters->track >= count) {
423           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_TRACK);
424         }
425         if (counters->mtrack >= count) {
426           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_MTRACK);
427         }
428         if (counters->magvar >= count) {
429           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_MAGVAR);
430         }
431         if (counters->satinusecount >= count) {
432           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_SATINUSECOUNT);
433         }
434         if (counters->satinuse >= count) {
435           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_SATINUSE);
436         }
437         if (counters->satinviewcount >= count) {
438           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_SATINVIEWCOUNT);
439         }
440         if (counters->satinview >= count) {
441           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_SATINVIEW);
442         }
443         if (counters->height >= count) {
444           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_HEIGHT);
445         }
446         if (counters->dgpsage >= count) {
447           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_DGPSAGE);
448         }
449         if (counters->dgpssid >= count) {
450           nmeaInfoSetPresent(&cumulative->nmeaInfo.present, NMEALIB_PRESENT_DGPSSID);
451         }
452
453         /* smask */
454         cumulative->nmeaInfo.smask = 0;
455
456         if (counters->gpgga >= count) {
457                 cumulative->nmeaInfo.smask |= NMEALIB_SENTENCE_GPGGA;
458         }
459         if (counters->gpgsa >= count) {
460                 cumulative->nmeaInfo.smask |= NMEALIB_SENTENCE_GPGSA;
461         }
462         if (counters->gpgsv >= count) {
463                 cumulative->nmeaInfo.smask |= NMEALIB_SENTENCE_GPGSV;
464         }
465         if (counters->gprmc >= count) {
466                 cumulative->nmeaInfo.smask |= NMEALIB_SENTENCE_GPRMC;
467         }
468         if (counters->gpvtg >= count) {
469                 cumulative->nmeaInfo.smask |= NMEALIB_SENTENCE_GPVTG;
470         }
471
472         /* sig */
473         cumulative->nmeaInfo.sig = NMEALIB_SIG_INVALID;
474         if (nmeaInfoIsPresentAll(cumulative->nmeaInfo.present, NMEALIB_PRESENT_SIG)) {
475                 if (counters->sigBad == 0) {
476                         if (counters->sigHigh >= count) {
477                                 cumulative->nmeaInfo.sig = NMEALIB_SIG_SENSITIVE;
478                         } else if (counters->sigMid > 0) {
479                                 cumulative->nmeaInfo.sig = NMEALIB_SIG_DIFFERENTIAL;
480                         } else if (counters->sigLow > 0) {
481                                 cumulative->nmeaInfo.sig = NMEALIB_SIG_FIX;
482                         }
483                 }
484         }
485
486         /* fix */
487         cumulative->nmeaInfo.fix = NMEALIB_FIX_BAD;
488         if (nmeaInfoIsPresentAll(cumulative->nmeaInfo.present, NMEALIB_PRESENT_FIX)) {
489                 if (counters->fixBad == 0) {
490                         if (counters->fix3d >= count) {
491                                 cumulative->nmeaInfo.fix = NMEALIB_FIX_3D;
492                         } else if (counters->fix2d > 0) {
493                                 cumulative->nmeaInfo.fix = NMEALIB_FIX_2D;
494                         }
495                 }
496         }
497 }
498
499 /**
500  * Calculate angle components
501  *
502  * @param components a pointer to the components structure
503  * @param angle a pointer to the angle (in degrees) from which to calculate
504  * the components. Set to NULL when the angle is not present in the input data.
505  *
506  */
507 static void calculateAngleComponents(AngleComponents * components, double * angle) {
508         if (!components)
509                 return;
510
511         if (!angle) {
512                 components->x = 0.0;
513                 components->y = 0.0;
514                 return;
515         }
516
517         components->x = cos(nmeaMathDegreeToRadian(*angle));
518         components->y = sin(nmeaMathDegreeToRadian(*angle));
519 }
520
521 /**
522  * Calculate angle from its components
523  *
524  * @param components a pointer to the components structure
525  * @return angle the angle (in degrees)
526  */
527 static double calculateAngle(AngleComponents * components) {
528         if (!components)
529                 return 0;
530
531         return nmeaMathRadianToDegree(atan2(components->y, components->x));
532 }
533
534 /**
535  * Add the src angle components to the dst angle components (accumulate)
536  *
537  * @param dst a pointer to the destination components structure
538  * @param src a pointer to the source components structure
539  * @param add true to add, false to subtract
540  */
541 static void addAngleComponents(AngleComponents * dst, AngleComponents * src, bool add) {
542         if (!dst || !src)
543                 return;
544
545         dst->x += add ? src->x : -src->x;
546         dst->y += add ? src->y : -src->y;
547 }
548
549 /**
550  Add/remove a position update entry to/from the average position list, updates
551  the counters, adjusts the entriesCount and redetermines the cumulative
552  smask, sig and fix.
553
554  @param positionAverageList
555  The position average list
556  @param entry
557  The entry to add/remove
558  @param add
559  True when the entry must be added to the list, false when it must be removed
560  */
561 static void addOrRemoveEntryToFromCumulativeAverage(
562                 PositionAverageList * positionAverageList, PositionUpdateEntry * entry,
563                 bool add) {
564         PositionUpdateEntry * cumulative =
565                         &positionAverageList->positionAverageCumulative;
566
567         if (!add) {
568                 assert(positionAverageList->entriesCount >= positionAverageList->entriesMaxCount);
569                 assert(entry == getPositionAverageEntry(positionAverageList, OLDEST));
570
571                 /* do not touch present */
572                 /* do not touch smask */
573
574                 /* do not touch utc */
575
576                 /* do not touch sig */
577                 /* do not touch fix */
578
579                 /* do not touch satinfo */
580         } else {
581                 assert(positionAverageList->entriesCount < positionAverageList->entriesMaxCount);
582                 assert(entry == getPositionAverageEntry(positionAverageList, INCOMING));
583
584                 /* present at the end */
585                 /* smask at the end */
586
587                 /* use the latest utc */
588                 cumulative->nmeaInfo.utc = entry->nmeaInfo.utc;
589
590                 /* sig at the end */
591                 /* fix at the end */
592
593                 /* use the latest satinfo */
594                 cumulative->nmeaInfo.satellites = entry->nmeaInfo.satellites;
595         }
596
597         /* PDOP, HDOP, VDOP */
598         cumulative->nmeaInfo.pdop += add ? entry->nmeaInfo.pdop
599                         : -entry->nmeaInfo.pdop;
600         cumulative->nmeaInfo.hdop += add ? entry->nmeaInfo.hdop
601                         : -entry->nmeaInfo.hdop;
602         cumulative->nmeaInfo.vdop += add ? entry->nmeaInfo.vdop
603                         : -entry->nmeaInfo.vdop;
604
605         /* lat, lon */
606         cumulative->nmeaInfo.latitude += add ? entry->nmeaInfo.latitude
607                         : -entry->nmeaInfo.latitude;
608         cumulative->nmeaInfo.longitude += add ? entry->nmeaInfo.longitude
609                         : -entry->nmeaInfo.longitude;
610
611         /* elv, speed */
612         cumulative->nmeaInfo.elevation += add ? entry->nmeaInfo.elevation
613                         : -entry->nmeaInfo.elevation;
614         cumulative->nmeaInfo.speed += add ? entry->nmeaInfo.speed
615                         : -entry->nmeaInfo.speed;
616
617         /* track, mtrack, magvar */
618         cumulative->nmeaInfo.track += add ? entry->nmeaInfo.track : -entry->nmeaInfo.track;
619         addAngleComponents(&cumulative->track, &entry->track, add);
620
621         cumulative->nmeaInfo.mtrack += add ? entry->nmeaInfo.mtrack : -entry->nmeaInfo.mtrack;
622         addAngleComponents(&cumulative->mtrack, &entry->mtrack, add);
623
624         cumulative->nmeaInfo.magvar += add ? entry->nmeaInfo.magvar : -entry->nmeaInfo.magvar;
625         addAngleComponents(&cumulative->magvar, &entry->magvar, add);
626
627         /* adjust list count */
628         positionAverageList->entriesCount += (add ? 1 : -1);
629
630         updateCounters(positionAverageList, entry, add);
631         determineCumulativePresentSmaskSigFix(positionAverageList);
632 }
633
634 /**
635  Update the average position from the cumulative average position. Basically
636  divide all relevant cumulative values by the number of entries in the list.
637
638  @param positionAverageList
639  The position average list
640  */
641 static void updatePositionAverageFromCumulative(
642                 PositionAverageList * positionAverageList) {
643         double divider = positionAverageList->entriesCount;
644
645         positionAverageList->positionAverage = positionAverageList->positionAverageCumulative;
646
647         /* smask: use from cumulative average */
648
649         /* utc: use from cumulative average */
650
651         /* sig: use from cumulative average */
652         /* fix: use from cumulative average */
653
654         if (divider > 1.0) {
655                 positionAverageList->positionAverage.nmeaInfo.pdop /= divider;
656                 positionAverageList->positionAverage.nmeaInfo.hdop /= divider;
657                 positionAverageList->positionAverage.nmeaInfo.vdop /= divider;
658
659                 positionAverageList->positionAverage.nmeaInfo.latitude /= divider;
660                 positionAverageList->positionAverage.nmeaInfo.longitude /= divider;
661
662                 positionAverageList->positionAverage.nmeaInfo.elevation /= divider;
663                 positionAverageList->positionAverage.nmeaInfo.speed /= divider;
664
665                 positionAverageList->positionAverage.nmeaInfo.track = calculateAngle(&positionAverageList->positionAverageCumulative.track);
666                 positionAverageList->positionAverage.nmeaInfo.mtrack = calculateAngle(&positionAverageList->positionAverageCumulative.mtrack);
667                 positionAverageList->positionAverage.nmeaInfo.magvar = calculateAngle(&positionAverageList->positionAverageCumulative.magvar);
668         }
669
670         /* satinfo: use from average */
671 }
672
673 /**
674  Add a new (incoming) position update to the position average list
675
676  @param positionAverageList
677  The position average list
678  @param newEntry
679  The new (incoming) position update (must be the same as the one returned from
680  the function getPositionAverageEntryForIncoming:INCOMING)
681  */
682 void addNewPositionToAverage(PositionAverageList * positionAverageList,
683                 PositionUpdateEntry * newEntry) {
684         assert (positionAverageList != NULL);
685         assert (newEntry == getPositionAverageEntry(positionAverageList, INCOMING));
686
687         if (positionAverageList->entriesCount
688                         >= positionAverageList->entriesMaxCount) {
689                 /* list is full, so first remove the oldest from the average */
690                 addOrRemoveEntryToFromCumulativeAverage(positionAverageList,
691                                 getPositionAverageEntry(positionAverageList, OLDEST), false);
692         }
693
694         /* calculate the angle components */
695         calculateAngleComponents(&newEntry->track, nmeaInfoIsPresentAll(newEntry->nmeaInfo.present, NMEALIB_PRESENT_TRACK) ? &newEntry->nmeaInfo.track : NULL);
696         calculateAngleComponents(&newEntry->mtrack, nmeaInfoIsPresentAll(newEntry->nmeaInfo.present, NMEALIB_PRESENT_MTRACK) ? &newEntry->nmeaInfo.mtrack : NULL);
697         calculateAngleComponents(&newEntry->magvar, nmeaInfoIsPresentAll(newEntry->nmeaInfo.present, NMEALIB_PRESENT_MAGVAR) ? &newEntry->nmeaInfo.magvar : NULL);
698
699         /* now just add the new position */
700         addOrRemoveEntryToFromCumulativeAverage(positionAverageList, newEntry, true);
701
702         /* update the place where the new entry is stored */
703         positionAverageList->newestEntryIndex
704                         = WRAPINDEX(positionAverageList, NEWESTINDEX(positionAverageList) + 1);
705
706         /* update average position */
707         updatePositionAverageFromCumulative(positionAverageList);
708 }