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