PUD: the default/initial state is now STATIONARY
[olsrd.git] / lib / pud / src / receiver.c
1 #include "receiver.h"
2
3 /* Plugin includes */
4 #include "pud.h"
5 #include "gpsConversion.h"
6 #include "configuration.h"
7 #include "dump.h"
8 #include "timers.h"
9 #include "posAvg.h"
10 #include "networkInterfaces.h"
11 #include "compiler.h"
12 #include "uplinkGateway.h"
13
14 /* OLSRD includes */
15 #include "net_olsr.h"
16
17 /* System includes */
18 #include <stddef.h>
19 #include <nmea/parser.h>
20 #include <nmea/info.h>
21 #include <pthread.h>
22 #include <nmea/info.h>
23 #include <string.h>
24 #include <nmea/gmath.h>
25 #include <nmea/sentence.h>
26 #include <math.h>
27 #include <net/if.h>
28 #include <assert.h>
29
30 /* Debug includes */
31 #if defined(PUD_DUMP_GPS_PACKETS_TX_OLSR) || \
32         defined(PUD_DUMP_GPS_PACKETS_TX_UPLINK) || \
33         defined(PUD_DUMP_AVERAGING)
34 #include "olsr.h"
35 #endif
36
37 /*
38  * NMEA parser
39  */
40
41 /** The NMEA string parser */
42 static nmeaPARSER nmeaParser;
43
44 /*
45  * State
46  */
47
48 /** Type describing a tri-state boolean */
49 typedef enum _TristateBoolean {
50         UNKNOWN = 0,
51         UNSET = 1,
52         SET = 2
53 } TristateBoolean;
54
55 #define TristateBooleanToString(s)      ((s == SET) ? "set" : \
56                                                                          (s == UNSET) ? "unset" : \
57                                                                          "unknown")
58
59 /** Type describing movement state */
60 typedef enum _MovementState {
61         STATIONARY = 0,
62         MOVING = 1
63 } MovementState;
64
65 #define MovementStateToString(s)        ((s == MOVING) ? "moving" : \
66                                                                          "stationary")
67
68 /** Type describing state */
69 typedef struct _StateType {
70         MovementState internalState; /**< the internal movement state */
71         MovementState externalState; /**< the externally visible movement state */
72         unsigned long long hysteresisCounter; /**< the hysteresis counter external state changes */
73 } StateType;
74
75 /** The state */
76 static StateType state = {
77                 .internalState = MOVING,
78                 .externalState = MOVING,
79                 .hysteresisCounter = 0
80 };
81
82 /** Type describing movement calculations */
83 typedef struct _MovementType {
84         TristateBoolean moving; /**< SET: we are moving */
85
86         TristateBoolean overThresholds; /**< SET: at least 1 threshold state is set */
87         TristateBoolean speedOverThreshold; /**< SET: speed is over threshold */
88         TristateBoolean hDistanceOverThreshold; /**< SET: horizontal distance is outside threshold */
89         TristateBoolean vDistanceOverThreshold; /**< SET: vertical distance is outside threshold */
90
91         TristateBoolean outside; /**< SET: at least 1 outside state is SET */
92         TristateBoolean outsideHdop; /**< SET: avg is outside lastTx HDOP */
93         TristateBoolean outsideVdop; /**< SET: avg is outside lastTx VDOP */
94
95         TristateBoolean inside; /**< SET: all inside states are SET */
96         TristateBoolean insideHdop; /**< SET: avg is inside lastTx HDOP */
97         TristateBoolean insideVdop; /**< SET: avg is inside lastTx VDOP */
98 } MovementType;
99
100 /*
101  * Averaging
102  */
103
104 /** The average position with its administration */
105 static PositionAverageList positionAverageList;
106
107 /*
108  * TX to OLSR
109  */
110
111 typedef enum _TimedTxInterface {
112         OLSR = 1,
113         UPLINK = 2
114 } TimedTxInterface;
115
116 /** Structure of the latest GPS information that is transmitted */
117 typedef struct _TransmitGpsInformation {
118         pthread_mutex_t mutex; /**< access mutex */
119         bool updated; /**< true when the information was updated */
120         PositionUpdateEntry txPosition; /**< The last transmitted position */
121 } TransmitGpsInformation;
122
123 /** The latest position information that is transmitted */
124 static TransmitGpsInformation transmitGpsInformation;
125
126 /** The last transmitted position.
127  * The same as transmitGpsInformation.txPosition.
128  * We keep this because then we can access the information without locking
129  * mutexes. */
130 static PositionUpdateEntry txPosition;
131
132 /** The size of the buffer in which the OLSR and uplink messages are assembled */
133 #define TX_BUFFER_SIZE_FOR_OLSR 1024
134
135 /*
136  * Functions
137  */
138
139 /**
140  Determine whether s position is valid.
141
142  @param position
143  a pointer to a position
144
145  @return
146  - true when valid
147  - false otherwise
148  */
149 static bool positionValid(PositionUpdateEntry * position) {
150         return (nmea_INFO_has_field(position->nmeaInfo.smask, FIX)
151                         && (position->nmeaInfo.fix != NMEA_FIX_BAD));
152 }
153
154 /** Send the transmit buffer out over all designated interfaces, called as a
155  * timer callback and also immediately on an external state change.
156
157  @param interfaces
158  a bitmap defining which interfaces to send over
159  */
160 static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
161         /** buffer used to fill in the OLSR and uplink messages */
162         unsigned char txBuffer[TX_BUFFER_SIZE_FOR_OLSR];
163         UplinkMessage * message1 = (UplinkMessage *) &txBuffer[0];
164
165         unsigned int txBufferSpaceTaken = 0;
166         #define txBufferSpaceFree       (sizeof(txBuffer) - txBufferSpaceTaken)
167
168         /* the first message in the buffer is an OLSR position update */
169         union olsr_message * olsrMessage =
170                         (union olsr_message *) &message1->msg.olsrMessage;
171         unsigned int aligned_size = 0;
172
173         /* convert nmeaINFO to wireformat olsr message */
174         (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
175         if (!transmitGpsInformation.updated
176                         && positionValid(&transmitGpsInformation.txPosition)) {
177                 nmea_time_now(&transmitGpsInformation.txPosition.nmeaInfo.utc);
178         }
179
180         txBufferSpaceTaken += sizeof(UplinkHeader);
181         aligned_size = gpsToOlsr(&transmitGpsInformation.txPosition.nmeaInfo,
182                         olsrMessage, txBufferSpaceFree,
183                         ((state.externalState == MOVING) ? getUpdateIntervalMoving()
184                                                 : getUpdateIntervalStationary()));
185         txBufferSpaceTaken += aligned_size;
186         transmitGpsInformation.updated = false;
187         (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
188
189         if (aligned_size > 0) {
190                 /* push out to all OLSR interfaces */
191                 if ((interfaces & OLSR) != 0) {
192                         int r;
193                         struct interface *ifn;
194                         for (ifn = ifnet; ifn; ifn = ifn->int_next) {
195                                 r = net_outbuffer_push(ifn, olsrMessage, aligned_size);
196                                 if (r != (int) aligned_size) {
197                                         pudError(
198                                                         false,
199                                                         "Could not send to OLSR interface %s: %s"
200                                                                         " (aligned_size=%u, r=%d)",
201                                                         ifn->int_name,
202                                                         ((r == -1) ? "no buffer was found"
203                                                         : (r == 0) ? "there was not enough room in the buffer"
204                                                         : "unknown reason"), aligned_size, r);
205                                 }
206 #ifdef PUD_DUMP_GPS_PACKETS_TX_OLSR
207                                 else {
208                                         olsr_printf(0, "%s: packet sent to OLSR interface %s (%d bytes)\n",
209                                                         PUD_PLUGIN_ABBR, ifn->int_name, aligned_size);
210                                         dump_packet((unsigned char *)olsrMessage, aligned_size);
211                                 }
212 #endif
213                         }
214
215                         /* loopback to tx interface when so configured */
216                         if (getUseLoopback()) {
217                                 (void) packetReceivedFromOlsr(olsrMessage, NULL, NULL);
218                         }
219                 }
220
221                 /* push out over uplink when an uplink is configured */
222                 if (((interfaces & UPLINK) != 0) && isUplinkAddrSet()) {
223                         int fd = getUplinkSocketFd();
224                         if (fd != -1) {
225                                 union olsr_ip_addr * gwAddr = getBestUplinkGateway();
226
227                                 UplinkMessage * message2 =
228                                                 (UplinkMessage *) &txBuffer[aligned_size
229                                                                 + sizeof(UplinkHeader)];
230                                 UplinkClusterLeader * clusterLeaderMessage =
231                                                 &message2->msg.clusterLeader;
232                                 unsigned int message2Size;
233                                 union olsr_ip_addr * clOriginator;
234                                 union olsr_ip_addr * clClusterLeader;
235
236                                 /*
237                                  * position update message (message1)
238                                  */
239
240                                 union olsr_sockaddr * address = getUplinkAddr();
241                                 PudOlsrPositionUpdate * olsrGpsMessage = getOlsrMessagePayload(
242                                                 olsr_cnf->ip_version, olsrMessage);
243
244                                 /* set header fields */
245                                 setUplinkMessageType(&message1->header, POSITION);
246                                 setUplinkMessageLength(&message1->header, aligned_size);
247                                 setUplinkMessageIPv6(&message1->header,
248                                                 (olsr_cnf->ip_version != AF_INET));
249                                 setUplinkMessagePadding(&message1->header, 0);
250
251                                 /* fixup validity time */
252                                 setValidityTime(&olsrGpsMessage->validityTime,
253                                                 (state.externalState == MOVING) ?
254                                                 getUplinkUpdateIntervalMoving() :
255                                                 getUplinkUpdateIntervalStationary());
256
257                                 /*
258                                  * cluster leader message (message2)
259                                  */
260
261                                 clOriginator = getClusterLeaderOriginator(olsr_cnf->ip_version,
262                                                 clusterLeaderMessage);
263                                 clClusterLeader = getClusterLeaderClusterLeader(
264                                                 olsr_cnf->ip_version, clusterLeaderMessage);
265                                 message2Size = sizeof(UplinkClusterLeader)
266                                                 - sizeof(clusterLeaderMessage->leader);
267                                 if (olsr_cnf->ip_version == AF_INET) {
268                                         message2Size += sizeof(clusterLeaderMessage->leader.v4);
269                                 } else {
270                                         message2Size = sizeof(clusterLeaderMessage->leader.v6);
271                                 }
272
273                                 /* set header fields */
274                                 setUplinkMessageType(&message2->header, CLUSTERLEADER);
275                                 setUplinkMessageLength(&message2->header, message2Size);
276                                 setUplinkMessageIPv6(&message2->header,
277                                                 (olsr_cnf->ip_version != AF_INET));
278                                 setUplinkMessagePadding(&message2->header, 0);
279                                 txBufferSpaceTaken += sizeof(UplinkHeader);
280
281                                 /* setup validity time */
282                                 setClusterLeaderVersion(clusterLeaderMessage, PUD_WIRE_FORMAT_VERSION);
283                                 setValidityTime(&clusterLeaderMessage->validityTime,
284                                                 (state.externalState == MOVING) ?
285                                                 getUplinkUpdateIntervalMoving() :
286                                                 getUplinkUpdateIntervalStationary());
287                                 setClusterLeaderDownlinkPort(clusterLeaderMessage,
288                                                 getDownlinkPort());
289
290                                 memcpy(clOriginator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
291                                 memcpy(clClusterLeader, gwAddr, olsr_cnf->ipsize);
292
293                                 txBufferSpaceTaken += message2Size;
294
295                                 errno = 0;
296                                 if (sendto(fd, &txBuffer, txBufferSpaceTaken, 0,
297                                         (struct sockaddr *) &address->in, sizeof(address->in)) < 0) {
298                                         pudError(true, "Could not send to uplink"
299                                                         " (aligned_size=%u)", txBufferSpaceTaken);
300                                 }
301 #ifdef PUD_DUMP_GPS_PACKETS_TX_UPLINK
302                                 else {
303                                         olsr_printf(0, "%s: packet sent to uplink (%d bytes)\n",
304                                                         PUD_PLUGIN_ABBR, aligned_size);
305                                         dump_packet((unsigned char *)&txBuffer, txBufferSpaceTaken);
306                                 }
307 #endif
308                         }
309                 }
310         }
311 }
312
313 /*
314  * Timer Callbacks
315  */
316
317 /**
318  The OLSR tx timer callback
319
320  @param context
321  unused
322  */
323 static void pud_olsr_tx_timer_callback(void *context __attribute__ ((unused))) {
324         txToAllOlsrInterfaces(OLSR);
325 }
326
327 /**
328  The uplink timer callback
329
330  @param context
331  unused
332  */
333 static void pud_uplink_timer_callback(void *context __attribute__ ((unused))) {
334         txToAllOlsrInterfaces(UPLINK);
335 }
336
337 /**
338  Detemine whether we are moving by comparing fields from the average
339  position against those of the last transmitted position.
340
341  MUST be called which the position average list locked.
342
343  @param avg
344  the average position
345  @param lastTx
346  the last transmitted position
347  @param result
348  the results of all movement criteria
349  */
350 static void detemineMoving(PositionUpdateEntry * avg,
351                 PositionUpdateEntry * lastTx, MovementType * result) {
352         /* avg field presence booleans */
353         bool avgHasSpeed;
354         bool avgHasPos;
355         bool avgHasHdop;
356         bool avgHasElv;
357         bool avgHasVdop;
358
359         /* lastTx field presence booleans */bool lastTxHasPos;
360         bool lastTxHasHdop;
361         bool lastTxHasElv;
362         bool lastTxHasVdop;
363
364         /* these have defaults */
365         double dopMultiplier;
366         double avgHdop;
367         double lastTxHdop;
368         double avgVdop;
369         double lastTxVdop;
370
371         /* calculated values and their validity booleans */
372         double hDistance;
373         double vDistance;
374         double hdopDistanceForOutside;
375         double hdopDistanceForInside;
376         double vdopDistanceForOutside;
377         double vdopDistanceForInside;
378         bool hDistanceValid;
379         bool hdopDistanceValid;
380         bool vDistanceValid;
381         bool vdopDistanceValid;
382
383         /* clear outputs */
384         result->moving = UNKNOWN;
385         result->overThresholds = UNKNOWN;
386         result->speedOverThreshold = UNKNOWN;
387         result->hDistanceOverThreshold = UNKNOWN;
388         result->vDistanceOverThreshold = UNKNOWN;
389         result->outside = UNKNOWN;
390         result->outsideHdop = UNKNOWN;
391         result->outsideVdop = UNKNOWN;
392         result->inside = UNKNOWN;
393         result->insideHdop = UNKNOWN;
394         result->insideVdop = UNKNOWN;
395
396         /*
397          * Validity
398          *
399          * avg  last  movingNow
400          *  0     0   UNKNOWN : can't determine whether we're moving
401          *  0     1   UNKNOWN : can't determine whether we're moving
402          *  1     0   MOVING  : always seen as movement
403          *  1     1   determine via other parameters
404          */
405
406         if (!positionValid(avg)) {
407                 /* force stationary when the position is invalid */
408                 result->moving = UNSET;
409                 return;
410         }
411
412         /* avg is valid here */
413
414         if (!positionValid(lastTx)) {
415                 /* the position just became valid: force moving */
416                 result->moving = SET;
417                 /* the rest is unknown */
418                 return;
419         }
420
421         /* both avg and lastTx are valid here */
422
423         /* avg field presence booleans */
424         avgHasSpeed = nmea_INFO_has_field(avg->nmeaInfo.smask, SPEED);
425         avgHasPos = nmea_INFO_has_field(avg->nmeaInfo.smask, LAT)
426                         && nmea_INFO_has_field(avg->nmeaInfo.smask, LON);
427         avgHasHdop = nmea_INFO_has_field(avg->nmeaInfo.smask, HDOP);
428         avgHasElv = nmea_INFO_has_field(avg->nmeaInfo.smask, ELV);
429         avgHasVdop = nmea_INFO_has_field(avg->nmeaInfo.smask, VDOP);
430
431         /* lastTx field presence booleans */
432         lastTxHasPos = nmea_INFO_has_field(lastTx->nmeaInfo.smask, LAT)
433                         && nmea_INFO_has_field(lastTx->nmeaInfo.smask, LON);
434         lastTxHasHdop = nmea_INFO_has_field(lastTx->nmeaInfo.smask, HDOP);
435         lastTxHasElv = nmea_INFO_has_field(lastTx->nmeaInfo.smask, ELV);
436         lastTxHasVdop = nmea_INFO_has_field(lastTx->nmeaInfo.smask, VDOP);
437
438         /* fill in some values _or_ defaults */
439         dopMultiplier = getDopMultiplier();
440         avgHdop = avgHasHdop ? avg->nmeaInfo.HDOP : getDefaultHdop();
441         lastTxHdop = lastTxHasHdop ? lastTx->nmeaInfo.HDOP : getDefaultHdop();
442         avgVdop = avgHasVdop ? avg->nmeaInfo.VDOP : getDefaultVdop();
443         lastTxVdop = lastTxHasVdop ? lastTx->nmeaInfo.VDOP : getDefaultVdop();
444
445         /*
446          * Calculations
447          */
448
449         /* hDistance */
450         if (avgHasPos && lastTxHasPos) {
451                 nmeaPOS avgPos;
452                 nmeaPOS lastTxPos;
453
454                 avgPos.lat = nmea_degree2radian(avg->nmeaInfo.lat);
455                 avgPos.lon = nmea_degree2radian(avg->nmeaInfo.lon);
456
457                 lastTxPos.lat = nmea_degree2radian(lastTx->nmeaInfo.lat);
458                 lastTxPos.lon = nmea_degree2radian(lastTx->nmeaInfo.lon);
459
460                 hDistance = nmea_distance_ellipsoid(&avgPos, &lastTxPos, NULL, NULL);
461                 hDistanceValid = true;
462         } else {
463                 hDistanceValid = false;
464         }
465
466         /* hdopDistance */
467         if (avgHasHdop || lastTxHasHdop) {
468                 hdopDistanceForOutside = dopMultiplier * (lastTxHdop + avgHdop);
469                 hdopDistanceForInside = dopMultiplier * (lastTxHdop - avgHdop);
470                 hdopDistanceValid = true;
471         } else {
472                 hdopDistanceValid = false;
473         }
474
475         /* vDistance */
476         if (avgHasElv && lastTxHasElv) {
477                 vDistance = fabs(lastTx->nmeaInfo.elv - avg->nmeaInfo.elv);
478                 vDistanceValid = true;
479         } else {
480                 vDistanceValid = false;
481         }
482
483         /* vdopDistance */
484         if (avgHasVdop || lastTxHasVdop) {
485                 vdopDistanceForOutside = dopMultiplier * (lastTxVdop + avgVdop);
486                 vdopDistanceForInside = dopMultiplier * (lastTxVdop - avgVdop);
487                 vdopDistanceValid = true;
488         } else {
489                 vdopDistanceValid = false;
490         }
491
492         /*
493          * Moving Criteria Evaluation Start
494          * We compare the average position against the last transmitted position.
495          */
496
497         /* Speed */
498         if (avgHasSpeed) {
499                 if (avg->nmeaInfo.speed >= getMovingSpeedThreshold()) {
500                         result->speedOverThreshold = SET;
501                 } else {
502                         result->speedOverThreshold = UNSET;
503                 }
504         }
505
506         /*
507          * Position
508          *
509          * avg  last  hDistanceMoving
510          *  0     0   determine via other parameters
511          *  0     1   determine via other parameters
512          *  1     0   MOVING
513          *  1     1   determine via distance threshold and HDOP
514          */
515         if (avgHasPos && !lastTxHasPos) {
516                 result->hDistanceOverThreshold = SET;
517         } else if (hDistanceValid) {
518                 if (hDistance >= getMovingDistanceThreshold()) {
519                         result->hDistanceOverThreshold = SET;
520                 } else {
521                         result->hDistanceOverThreshold = UNSET;
522                 }
523
524                 /*
525                  * Position with HDOP
526                  *
527                  * avg  last  movingNow
528                  *  0     0   determine via other parameters
529                  *  0     1   determine via position with HDOP (avg has default HDOP)
530                  *  1     0   determine via position with HDOP (lastTx has default HDOP)
531                  *  1     1   determine via position with HDOP
532                  */
533                 if (hdopDistanceValid) {
534                         /* we are outside the HDOP when the HDOPs no longer overlap */
535                         if (hDistance > hdopDistanceForOutside) {
536                                 result->outsideHdop = SET;
537                         } else {
538                                 result->outsideHdop = UNSET;
539                         }
540
541                         /* we are inside the HDOP when the HDOPs fully overlap */
542                         if (hDistance <= hdopDistanceForInside) {
543                                 result->insideHdop = SET;
544                         } else {
545                                 result->insideHdop = UNSET;
546                         }
547                 }
548         }
549
550         /*
551          * Elevation
552          *
553          * avg  last  movingNow
554          *  0     0   determine via other parameters
555          *  0     1   determine via other parameters
556          *  1     0   MOVING
557          *  1     1   determine via distance threshold and VDOP
558          */
559         if (avgHasElv && !lastTxHasElv) {
560                 result->vDistanceOverThreshold = SET;
561         } else if (vDistanceValid) {
562                 if (vDistance >= getMovingDistanceThreshold()) {
563                         result->vDistanceOverThreshold = SET;
564                 } else {
565                         result->vDistanceOverThreshold = UNSET;
566                 }
567
568                 /*
569                  * Elevation with VDOP
570                  *
571                  * avg  last  movingNow
572                  *  0     0   determine via other parameters
573                  *  0     1   determine via elevation with VDOP (avg has default VDOP)
574                  *  1     0   determine via elevation with VDOP (lastTx has default VDOP)
575                  *  1     1   determine via elevation with VDOP
576                  */
577                 if (vdopDistanceValid) {
578                         /* we are outside the VDOP when the VDOPs no longer overlap */
579                         if (vDistance > vdopDistanceForOutside) {
580                                 result->outsideVdop = SET;
581                         } else {
582                                 result->outsideVdop = UNSET;
583                         }
584
585                         /* we are inside the VDOP when the VDOPs fully overlap */
586                         if (vDistance <= vdopDistanceForInside) {
587                                 result->insideVdop = SET;
588                         } else {
589                                 result->insideVdop = UNSET;
590                         }
591                 }
592         }
593
594         /*
595          * Moving Criteria Evaluation End
596          */
597
598         /* accumulate inside criteria */
599         if ((result->insideHdop == SET) && (result->insideVdop == SET)) {
600                 result->inside = SET;
601         } else if ((result->insideHdop == UNSET) || (result->insideVdop == UNSET)) {
602                 result->inside = UNSET;
603         }
604
605         /* accumulate outside criteria */
606         if ((result->outsideHdop == SET) || (result->outsideVdop == SET)) {
607                 result->outside = SET;
608         } else if ((result->outsideHdop == UNSET)
609                         || (result->outsideVdop == UNSET)) {
610                 result->outside = UNSET;
611         }
612
613         /* accumulate threshold criteria */
614         if ((result->speedOverThreshold == SET)
615                         || (result->hDistanceOverThreshold == SET)
616                         || (result->vDistanceOverThreshold == SET)) {
617                 result->overThresholds = SET;
618         } else if ((result->speedOverThreshold == UNSET)
619                         || (result->hDistanceOverThreshold == UNSET)
620                         || (result->vDistanceOverThreshold == UNSET)) {
621                 result->overThresholds = UNSET;
622         }
623
624         /* accumulate moving criteria */
625         if ((result->overThresholds == SET) || (result->outside == SET)) {
626                 result->moving = SET;
627         } else if ((result->overThresholds == UNSET)
628                         && (result->outside == UNSET)) {
629                 result->moving = UNSET;
630         }
631
632         return;
633 }
634
635 /**
636  Update the latest GPS information. This function is called when a packet is
637  received from a rxNonOlsr interface, containing one or more NMEA strings with
638  GPS information.
639
640  @param rxBuffer
641  the receive buffer with the received NMEA string(s)
642  @param rxCount
643  the number of bytes in the receive buffer
644
645  @return
646  - false on failure
647  - true otherwise
648  */
649 bool receiverUpdateGpsInformation(unsigned char * rxBuffer, size_t rxCount) {
650         static const char * rxBufferPrefix = "$GP";
651         static const size_t rxBufferPrefixLength = 3;
652
653         bool retval = false;
654         PositionUpdateEntry * incomingEntry;
655         MovementState newState = STATIONARY;
656         PositionUpdateEntry * posAvgEntry;
657         MovementType movementResult;
658         TristateBoolean movingNow;
659         bool internalStateChange = false;
660         bool externalStateChange = false;
661         bool updateTransmitGpsInformation = false;
662
663         /* do not process when the message does not start with $GP */
664         if ((rxCount < rxBufferPrefixLength) || (strncmp((char *) rxBuffer,
665                         rxBufferPrefix, rxBufferPrefixLength) != 0)) {
666                 return true;
667         }
668
669         (void) pthread_mutex_lock(&positionAverageList.mutex);
670
671         /* parse all NMEA strings in the rxBuffer into the incoming entry */
672         incomingEntry = getPositionAverageEntry(&positionAverageList, INCOMING);
673         nmea_zero_INFO(&incomingEntry->nmeaInfo);
674         nmea_parse(&nmeaParser, (char *) rxBuffer, rxCount,
675                         &incomingEntry->nmeaInfo);
676
677 #if defined(PUD_DUMP_AVERAGING)
678         dump_nmeaInfo(&incomingEntry->nmeaInfo,
679                         "receiverUpdateGpsInformation: incoming entry");
680 #endif /* PUD_DUMP_AVERAGING */
681
682         /* ignore when no useful information */
683         if (incomingEntry->nmeaInfo.smask == GPNON) {
684                 retval = true;
685                 goto end;
686         }
687
688         nmea_INFO_sanitise(&incomingEntry->nmeaInfo);
689
690 #if defined(PUD_DUMP_AVERAGING)
691         dump_nmeaInfo(&incomingEntry->nmeaInfo,
692                         "receiverUpdateGpsInformation: incoming entry after sanitise");
693 #endif /* PUD_DUMP_AVERAGING */
694
695         /* we always work with latitude, longitude in degrees and DOPs in meters */
696         nmea_INFO_unit_conversion(&incomingEntry->nmeaInfo);
697
698 #if defined(PUD_DUMP_AVERAGING)
699         dump_nmeaInfo(&incomingEntry->nmeaInfo,
700                         "receiverUpdateGpsInformation: incoming entry after unit conversion");
701 #endif /* PUD_DUMP_AVERAGING */
702
703         /*
704          * Averageing
705          */
706
707         if (state.internalState == MOVING) {
708                 /* flush average: keep only the incoming entry */
709                 flushPositionAverageList(&positionAverageList);
710         }
711         addNewPositionToAverage(&positionAverageList, incomingEntry);
712         posAvgEntry = getPositionAverageEntry(&positionAverageList, AVERAGE);
713
714         /*
715          * Movement detection
716          */
717
718         detemineMoving(posAvgEntry, &txPosition, &movementResult);
719         movingNow = movementResult.moving;
720
721 #if defined(PUD_DUMP_AVERAGING)
722         olsr_printf(0, "receiverUpdateGpsInformation: internalState = %s\n",
723                         MovementStateToString(state.internalState));
724         olsr_printf(0, "receiverUpdateGpsInformation: movingNow     = %s\n",
725                         TristateBooleanToString(movingNow));
726 #endif /* PUD_DUMP_AVERAGING */
727
728         /*
729          * Internal State
730          */
731
732         if (movingNow == SET) {
733                 newState = MOVING;
734         } else if (movingNow == UNSET) {
735                 newState = STATIONARY;
736         }
737         internalStateChange = (state.internalState != newState);
738         state.internalState = newState;
739
740         /*
741          * External State (+ hysteresis)
742          */
743
744         if (internalStateChange) {
745                 /* restart hysteresis for external state change when we have an internal
746                  * state change */
747                 state.hysteresisCounter = 0;
748         }
749
750         /* when internal state and external state are not the same we need to
751          * perform hysteresis before we can propagate the internal state to the
752          * external state */
753         newState = state.externalState;
754         if (state.internalState != state.externalState) {
755                 switch (state.internalState) {
756                         case STATIONARY:
757                                 /* external state is MOVING */
758
759                                 /* delay going to stationary a bit */
760                                 state.hysteresisCounter++;
761
762                                 if (state.hysteresisCounter
763                                                 >= getHysteresisCountToStationary()) {
764                                         /* outside the hysteresis range, go to stationary */
765                                         newState = STATIONARY;
766                                 }
767                                 break;
768
769                         case MOVING:
770                                 /* external state is STATIONARY */
771
772                                 /* delay going to moving a bit */
773                                 state.hysteresisCounter++;
774
775                                 if (state.hysteresisCounter >= getHysteresisCountToMoving()) {
776                                         /* outside the hysteresis range, go to moving */
777                                         newState = MOVING;
778                                 }
779                                 break;
780
781                         default:
782                                 /* when unknown do just as if we transition into stationary */
783                                 newState = STATIONARY;
784                                 break;
785                 }
786         }
787         externalStateChange = (state.externalState != newState);
788         state.externalState = newState;
789
790 #if defined(PUD_DUMP_AVERAGING)
791         olsr_printf(0, "receiverUpdateGpsInformation: newState = %s\n",
792                         MovementStateToString(newState));
793         dump_nmeaInfo(&posAvgEntry->nmeaInfo,
794                         "receiverUpdateGpsInformation: posAvgEntry");
795 #endif /* PUD_DUMP_AVERAGING */
796
797         /*
798          * Update transmitGpsInformation
799          */
800
801         updateTransmitGpsInformation = externalStateChange
802                         || (positionValid(posAvgEntry) && !positionValid(&txPosition))
803                         || (movementResult.inside == SET);
804
805         if ((state.externalState == MOVING) || updateTransmitGpsInformation) {
806                 memcpy(&txPosition.nmeaInfo, &posAvgEntry->nmeaInfo, sizeof(nmeaINFO));
807                 (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
808                 memcpy(&transmitGpsInformation.txPosition.nmeaInfo,
809                                 &posAvgEntry->nmeaInfo, sizeof(nmeaINFO));
810                 transmitGpsInformation.updated = true;
811
812 #if defined(PUD_DUMP_AVERAGING)
813                 dump_nmeaInfo(&transmitGpsInformation.txPosition.nmeaInfo,
814                         "receiverUpdateGpsInformation: transmitGpsInformation");
815 #endif /* PUD_DUMP_AVERAGING */
816
817                 (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
818         }
819
820         if (updateTransmitGpsInformation) {
821                 TimedTxInterface interfaces = OLSR; /* always send over olsr */
822                 if (!restartOlsrTxTimer(
823                                 (state.externalState == STATIONARY) ? getUpdateIntervalStationary()
824                                 : getUpdateIntervalMoving(), &pud_olsr_tx_timer_callback)) {
825                         pudError(0, "Could not restart OLSR tx timer, no periodic"
826                                         " position updates will be sent to the OLSR network");
827                 }
828
829                 if (isUplinkAddrSet()) {
830                         interfaces |= UPLINK;
831                         if (!restartUplinkTxTimer(
832                                         (state.externalState == STATIONARY) ? getUplinkUpdateIntervalStationary()
833                                         : getUplinkUpdateIntervalMoving(), &pud_uplink_timer_callback)
834                                         ) {
835                                 pudError(0, "Could not restart uplink timer, no periodic"
836                                                 " position updates will be uplinked");
837                         }
838                 }
839
840                 /* do an immediate transmit */
841                 txToAllOlsrInterfaces(interfaces);
842         }
843
844         retval = true;
845
846         end: (void) pthread_mutex_unlock(&positionAverageList.mutex);
847         return retval;
848 }
849
850 /*
851  * Receiver start/stop
852  */
853
854 /**
855  Start the receiver
856
857  @return
858  - false on failure
859  - true otherwise
860  */
861 bool startReceiver(void) {
862         pthread_mutexattr_t attr;
863         if (pthread_mutexattr_init(&attr)) {
864                 return false;
865         }
866         if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE_NP)) {
867                 return false;
868         }
869         if (pthread_mutex_init(&transmitGpsInformation.mutex, &attr)) {
870                 return false;
871         }
872
873         if (!nmea_parser_init(&nmeaParser)) {
874                 pudError(false, "Could not initialise NMEA parser");
875                 return false;
876         }
877
878         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
879         transmitGpsInformation.updated = false;
880
881         nmea_zero_INFO(&txPosition.nmeaInfo);
882
883         state.internalState = STATIONARY;
884         state.externalState = STATIONARY;
885         state.hysteresisCounter = 0;
886
887         initPositionAverageList(&positionAverageList, getAverageDepth());
888
889         if (!initOlsrTxTimer()) {
890                 stopReceiver();
891                 return false;
892         }
893
894         if (!initUplinkTxTimer()) {
895                 stopReceiver();
896                 return false;
897         }
898
899         return true;
900 }
901
902 /**
903  Stop the receiver
904  */
905 void stopReceiver(void) {
906         destroyUplinkTxTimer();
907         destroyOlsrTxTimer();
908
909         destroyPositionAverageList(&positionAverageList);
910
911         state.hysteresisCounter = 0;
912         state.externalState = STATIONARY;
913         state.internalState = STATIONARY;
914
915         nmea_zero_INFO(&txPosition.nmeaInfo);
916
917         transmitGpsInformation.updated = false;
918         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
919
920         nmea_parser_destroy(&nmeaParser);
921
922         (void) pthread_mutex_destroy(&transmitGpsInformation.mutex);
923 }