55e77d35a8e04937876458478ea2d0d77c78f63c
[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                 /* force back to stationary for unknown movement */
729                 newState = MOVEMENT_STATE_STATIONARY;
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 do just as if we transition into stationary */
776                                 newState = MOVEMENT_STATE_STATIONARY;
777                                 break;
778                 }
779         }
780
781         externalStateChange = (subState->externalState != newState);
782         subState->externalState = newState;
783
784 #if defined(PUD_DUMP_AVERAGING)
785         olsr_printf(0, "determineStateWithHysteresis: externalState(%d) = %s\n", subStateIndex,
786                         MovementStateToString(subState->externalState));
787 #endif /* PUD_DUMP_AVERAGING */
788
789         /*
790          * external state may transition into MOVING when either one of the sub-states say so (OR), and
791          * may transition into STATIONARY when all of the sub-states say so (AND)
792          */
793         if (externalStateChange) {
794                 bool transition = false;
795
796                 if (newState == MOVEMENT_STATE_STATIONARY) {
797                         /* AND: all sub-states must agree on STATIONARY */
798                         int i = 0;
799                         for (i = 0; i < SUBSTATE_COUNT; i++) {
800                                 transition = transition && (state.substate[i].externalState == newState);
801                         }
802                 } else /* if (newState == MOVEMENT_STATE_MOVING) */{
803                         /* OR: one sub-state wanting MOVING is enough */
804                         int i = 0;
805                         for (i = 0; i < SUBSTATE_COUNT; i++) {
806                                 transition = transition || (state.substate[i].externalState == newState);
807                         }
808                 }
809
810                 if (transition) {
811                         externalStateChange = (state.externalState != newState);
812                         state.externalState = newState;
813                 }
814         }
815
816 #if defined(PUD_DUMP_AVERAGING)
817         olsr_printf(0, "determineStateWithHysteresis: externalState    = %s\n", subStateIndex,
818                         MovementStateToString(externalState));
819 #endif /* PUD_DUMP_AVERAGING */
820
821         return externalStateChange;
822 }
823
824 /**
825  Update the latest GPS information. This function is called when a packet is
826  received from a rxNonOlsr interface, containing one or more NMEA strings with
827  GPS information.
828
829  @param rxBuffer
830  the receive buffer with the received NMEA string(s)
831  @param rxCount
832  the number of bytes in the receive buffer
833
834  @return
835  - false on failure
836  - true otherwise
837  */
838 bool receiverUpdateGpsInformation(unsigned char * rxBuffer, size_t rxCount) {
839         static const char * rxBufferPrefix = "$GP";
840         static const size_t rxBufferPrefixLength = 3;
841
842         bool retval = false;
843         PositionUpdateEntry * incomingEntry;
844         PositionUpdateEntry * posAvgEntry;
845         MovementType movementResult;
846         bool externalStateChange = false;
847         bool updateTransmitGpsInformation = false;
848         union olsr_ip_addr bestGateway;
849         PositionUpdateEntry txPosition;
850         union olsr_ip_addr txGateway;
851
852         /* do not process when the message does not start with $GP */
853         if ((rxCount < rxBufferPrefixLength) || (strncmp((char *) rxBuffer,
854                         rxBufferPrefix, rxBufferPrefixLength) != 0)) {
855                 return true;
856         }
857
858         /* parse all NMEA strings in the rxBuffer into the incoming entry */
859         incomingEntry = getPositionAverageEntry(&positionAverageList, INCOMING);
860         nmea_zero_INFO(&incomingEntry->nmeaInfo);
861         nmea_parse(&nmeaParser, (char *) rxBuffer, rxCount,
862                         &incomingEntry->nmeaInfo);
863
864 #if defined(PUD_DUMP_AVERAGING)
865         dump_nmeaInfo(&incomingEntry->nmeaInfo,
866                         "receiverUpdateGpsInformation: incoming entry");
867 #endif /* PUD_DUMP_AVERAGING */
868
869         /* ignore when no useful information */
870         if (incomingEntry->nmeaInfo.smask == GPNON) {
871                 retval = true;
872                 goto end;
873         }
874
875         nmea_INFO_sanitise(&incomingEntry->nmeaInfo);
876
877 #if defined(PUD_DUMP_AVERAGING)
878         dump_nmeaInfo(&incomingEntry->nmeaInfo,
879                         "receiverUpdateGpsInformation: incoming entry after sanitise");
880 #endif /* PUD_DUMP_AVERAGING */
881
882         /* we always work with latitude, longitude in degrees and DOPs in meters */
883         nmea_INFO_unit_conversion(&incomingEntry->nmeaInfo);
884
885 #if defined(PUD_DUMP_AVERAGING)
886         dump_nmeaInfo(&incomingEntry->nmeaInfo,
887                         "receiverUpdateGpsInformation: incoming entry after unit conversion");
888 #endif /* PUD_DUMP_AVERAGING */
889
890         /*
891          * Averaging
892          */
893
894         if (state.substate[SUBSTATE_POSITION].internalState == MOVEMENT_STATE_MOVING) {
895                 /* flush average: keep only the incoming entry */
896                 flushPositionAverageList(&positionAverageList);
897         }
898         addNewPositionToAverage(&positionAverageList, incomingEntry);
899         posAvgEntry = getPositionAverageEntry(&positionAverageList, AVERAGE);
900
901 #if defined(PUD_DUMP_AVERAGING)
902         dump_nmeaInfo(&posAvgEntry->nmeaInfo,
903                         "receiverUpdateGpsInformation: posAvgEntry");
904 #endif /* PUD_DUMP_AVERAGING */
905
906         /*
907          * Movement detection
908          */
909
910         (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
911         txPosition = transmitGpsInformation.txPosition;
912         txGateway = transmitGpsInformation.txGateway;
913         bestGateway = transmitGpsInformation.bestGateway;
914         (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
915
916         clearMovementType(&movementResult);
917         detemineMovingFromGateway(&bestGateway, &txGateway, &movementResult);
918         detemineMovingFromPosition(posAvgEntry, &txPosition, &movementResult);
919
920         /*
921          * State Determination
922          */
923
924         externalStateChange = determineStateWithHysteresis(SUBSTATE_GATEWAY, movementResult.moving);
925         externalStateChange = externalStateChange || determineStateWithHysteresis(SUBSTATE_POSITION, movementResult.moving);
926
927         /*
928          * Update transmitGpsInformation
929          */
930
931         updateTransmitGpsInformation = externalStateChange
932                         || (positionValid(posAvgEntry) && !positionValid(&txPosition))
933                         || (movementResult.inside == TRISTATE_BOOLEAN_SET);
934
935         if ((state.externalState == MOVEMENT_STATE_MOVING) || updateTransmitGpsInformation) {
936                 (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
937                 transmitGpsInformation.txPosition.nmeaInfo = posAvgEntry->nmeaInfo;
938                 transmitGpsInformation.txGateway = bestGateway;
939                 transmitGpsInformation.updated = true;
940                 (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
941
942 #if defined(PUD_DUMP_AVERAGING)
943                 dump_nmeaInfo(&posAvgEntry->nmeaInfo,
944                         "receiverUpdateGpsInformation: transmitGpsInformation");
945 #endif /* PUD_DUMP_AVERAGING */
946         }
947
948         if (externalStateChange) {
949                 TimedTxInterface interfaces = TX_INTERFACE_OLSR; /* always send over olsr */
950                 restartOlsrTimer();
951
952                 if (isUplinkAddrSet()) {
953                         interfaces |= TX_INTERFACE_UPLINK;
954                         restartUplinkTimer();
955                 }
956
957                 /* do an immediate transmit */
958                 txToAllOlsrInterfaces(interfaces);
959         }
960
961         retval = true;
962
963         end:
964         return retval;
965 }
966
967 /*
968  * Receiver start/stop
969  */
970
971 static void initState(void) {
972         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
973         transmitGpsInformation.txGateway = olsr_cnf->main_addr;
974         transmitGpsInformation.updated = false;
975
976         state.substate[SUBSTATE_POSITION].internalState = MOVEMENT_STATE_MOVING;
977         state.substate[SUBSTATE_POSITION].hysteresisCounter = 0;
978         state.substate[SUBSTATE_POSITION].hysteresisCounterToStationary = getHysteresisCountToStationary();
979         state.substate[SUBSTATE_POSITION].hysteresisCounterToMoving = getHysteresisCountToMoving();
980         state.substate[SUBSTATE_POSITION].externalState = MOVEMENT_STATE_MOVING;
981         state.substate[SUBSTATE_GATEWAY].internalState = MOVEMENT_STATE_MOVING;
982         state.substate[SUBSTATE_GATEWAY].hysteresisCounter = 0;
983         state.substate[SUBSTATE_GATEWAY].hysteresisCounterToStationary = getGatewayHysteresisCountToStationary();
984         state.substate[SUBSTATE_GATEWAY].hysteresisCounterToMoving = getGatewayHysteresisCountToMoving();
985         state.substate[SUBSTATE_GATEWAY].externalState = MOVEMENT_STATE_MOVING;
986         state.externalState = MOVEMENT_STATE_MOVING;
987 }
988
989 /**
990  Start the receiver
991
992  @return
993  - false on failure
994  - true otherwise
995  */
996 bool startReceiver(void) {
997         pthread_mutexattr_t attr;
998         if (pthread_mutexattr_init(&attr)) {
999                 return false;
1000         }
1001         if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE_NP)) {
1002                 return false;
1003         }
1004         if (pthread_mutex_init(&transmitGpsInformation.mutex, &attr)) {
1005                 return false;
1006         }
1007
1008         if (!nmea_parser_init(&nmeaParser)) {
1009                 pudError(false, "Could not initialise NMEA parser");
1010                 return false;
1011         }
1012
1013         initState();
1014
1015         initPositionAverageList(&positionAverageList, getAverageDepth());
1016
1017         if (!initOlsrTxTimer()) {
1018                 stopReceiver();
1019                 return false;
1020         }
1021
1022         if (!initUplinkTxTimer()) {
1023                 stopReceiver();
1024                 return false;
1025         }
1026
1027         if (!initGatewayTimer()) {
1028                 stopReceiver();
1029                 return false;
1030         }
1031
1032         restartOlsrTimer();
1033         restartUplinkTimer();
1034         if (!restartGatewayTimer(getGatewayDeterminationInterval(), &pud_gateway_timer_callback)) {
1035                 pudError(0, "Could not start gateway timer");
1036                 stopReceiver();
1037                 return false;
1038         }
1039
1040         return true;
1041 }
1042
1043 /**
1044  Stop the receiver
1045  */
1046 void stopReceiver(void) {
1047         destroyGatewayTimer();
1048         destroyUplinkTxTimer();
1049         destroyOlsrTxTimer();
1050
1051         destroyPositionAverageList(&positionAverageList);
1052
1053         initState();
1054         (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
1055         (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
1056
1057         nmea_parser_destroy(&nmeaParser);
1058
1059         (void) pthread_mutex_destroy(&transmitGpsInformation.mutex);
1060 }