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