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