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