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