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