3f8fd2a9efc7167cb368b4d8f598ad002314f503
[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         (void) pthread_mutex_lock(&positionAverageList.mutex);
728
729         /* parse all NMEA strings in the rxBuffer into the incoming entry */
730         incomingEntry = getPositionAverageEntry(&positionAverageList, INCOMING);
731         nmea_zero_INFO(&incomingEntry->nmeaInfo);
732         nmea_parse(&nmeaParser, (char *) rxBuffer, rxCount,
733                         &incomingEntry->nmeaInfo);
734
735 #if defined(PUD_DUMP_AVERAGING)
736         dump_nmeaInfo(&incomingEntry->nmeaInfo,
737                         "receiverUpdateGpsInformation: incoming entry");
738 #endif /* PUD_DUMP_AVERAGING */
739
740         /* ignore when no useful information */
741         if (incomingEntry->nmeaInfo.smask == GPNON) {
742                 retval = true;
743                 goto end;
744         }
745
746         nmea_INFO_sanitise(&incomingEntry->nmeaInfo);
747
748 #if defined(PUD_DUMP_AVERAGING)
749         dump_nmeaInfo(&incomingEntry->nmeaInfo,
750                         "receiverUpdateGpsInformation: incoming entry after sanitise");
751 #endif /* PUD_DUMP_AVERAGING */
752
753         /* we always work with latitude, longitude in degrees and DOPs in meters */
754         nmea_INFO_unit_conversion(&incomingEntry->nmeaInfo);
755
756 #if defined(PUD_DUMP_AVERAGING)
757         dump_nmeaInfo(&incomingEntry->nmeaInfo,
758                         "receiverUpdateGpsInformation: incoming entry after unit conversion");
759 #endif /* PUD_DUMP_AVERAGING */
760
761         /*
762          * Averageing
763          */
764
765         if (state.internalState == MOVING) {
766                 /* flush average: keep only the incoming entry */
767                 flushPositionAverageList(&positionAverageList);
768         }
769         addNewPositionToAverage(&positionAverageList, incomingEntry);
770         posAvgEntry = getPositionAverageEntry(&positionAverageList, AVERAGE);
771
772         /*
773          * Movement detection
774          */
775
776         bestGateway = getBestUplinkGateway();
777         clearMovementType(&movementResult);
778         detemineMovingFromGateway(bestGateway,&txGateway, &movementResult);
779         if (movementResult.moving != SET) {
780                 detemineMovingFromPosition(posAvgEntry, &txPosition, &movementResult);
781         }
782         movingNow = movementResult.moving;
783
784 #if defined(PUD_DUMP_AVERAGING)
785         olsr_printf(0, "receiverUpdateGpsInformation: internalState = %s\n",
786                         MovementStateToString(state.internalState));
787         olsr_printf(0, "receiverUpdateGpsInformation: movingNow     = %s\n",
788                         TristateBooleanToString(movingNow));
789 #endif /* PUD_DUMP_AVERAGING */
790
791         /*
792          * Internal State
793          */
794
795         if (movingNow == SET) {
796                 newState = MOVING;
797         } else if (movingNow == UNSET) {
798                 newState = STATIONARY;
799         }
800         internalStateChange = (state.internalState != newState);
801         state.internalState = newState;
802
803         /*
804          * External State (+ hysteresis)
805          */
806
807         if (internalStateChange) {
808                 /* restart hysteresis for external state change when we have an internal
809                  * state change */
810                 state.hysteresisCounter = 0;
811         }
812
813         /* when internal state and external state are not the same we need to
814          * perform hysteresis before we can propagate the internal state to the
815          * external state */
816         newState = state.externalState;
817         if (state.internalState != state.externalState) {
818                 switch (state.internalState) {
819                         case STATIONARY:
820                                 /* external state is MOVING */
821
822                                 /* delay going to stationary a bit */
823                                 state.hysteresisCounter++;
824
825                                 if (state.hysteresisCounter
826                                                 >= getHysteresisCountToStationary()) {
827                                         /* outside the hysteresis range, go to stationary */
828                                         newState = STATIONARY;
829                                 }
830                                 break;
831
832                         case MOVING:
833                                 /* external state is STATIONARY */
834
835                                 /* delay going to moving a bit */
836                                 state.hysteresisCounter++;
837
838                                 if (state.hysteresisCounter >= getHysteresisCountToMoving()) {
839                                         /* outside the hysteresis range, go to moving */
840                                         newState = MOVING;
841                                 }
842                                 break;
843
844                         default:
845                                 /* when unknown do just as if we transition into stationary */
846                                 newState = STATIONARY;
847                                 break;
848                 }
849         }
850         externalStateChange = (state.externalState != newState);
851         state.externalState = newState;
852
853 #if defined(PUD_DUMP_AVERAGING)
854         olsr_printf(0, "receiverUpdateGpsInformation: newState = %s\n",
855                         MovementStateToString(newState));
856         dump_nmeaInfo(&posAvgEntry->nmeaInfo,
857                         "receiverUpdateGpsInformation: posAvgEntry");
858 #endif /* PUD_DUMP_AVERAGING */
859
860         /*
861          * Update transmitGpsInformation
862          */
863
864         updateTransmitGpsInformation = externalStateChange
865                         || (positionValid(posAvgEntry) && !positionValid(&txPosition))
866                         || (movementResult.inside == SET);
867
868         if ((state.externalState == MOVING) || updateTransmitGpsInformation) {
869                 (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
870                 memcpy(&transmitGpsInformation.txPosition.nmeaInfo, &posAvgEntry->nmeaInfo, sizeof(nmeaINFO));
871                 memcpy(&transmitGpsInformation.txGateway, bestGateway, olsr_cnf->ipsize);
872                 transmitGpsInformation.updated = true;
873                 (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
874
875 #if defined(PUD_DUMP_AVERAGING)
876                 dump_nmeaInfo(&posAvgEntry->nmeaInfo,
877                         "receiverUpdateGpsInformation: transmitGpsInformation");
878 #endif /* PUD_DUMP_AVERAGING */
879         }
880
881         if (externalStateChange) {
882                 TimedTxInterface interfaces = OLSR; /* always send over olsr */
883                 restartOlsrTimer();
884
885                 if (isUplinkAddrSet()) {
886                         interfaces |= UPLINK;
887                         restartUplinkTimer();
888                 }
889
890                 /* do an immediate transmit */
891                 txToAllOlsrInterfaces(interfaces);
892         }
893
894         retval = true;
895
896         end: (void) pthread_mutex_unlock(&positionAverageList.mutex);
897         return retval;
898 }
899
900 /*
901  * Receiver start/stop
902  */
903
904 /**
905  Start the receiver
906
907  @return
908  - false on failure
909  - true otherwise
910  */
911 bool startReceiver(void) {
912         pthread_mutexattr_t attr;
913         if (pthread_mutexattr_init(&attr)) {
914                 return false;
915         }
916         if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE_NP)) {
917                 return false;
918         }
919         if (pthread_mutex_init(&transmitGpsInformation.mutex, &attr)) {
920                 return false;
921         }
922
923         if (!nmea_parser_init(&nmeaParser)) {
924                 pudError(false, "Could not initialise NMEA parser");
925                 return false;
926         }
927
928         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
929         memcpy(&transmitGpsInformation.txGateway, &olsr_cnf->main_addr, olsr_cnf->ipsize);
930         transmitGpsInformation.updated = false;
931
932         state.internalState = MOVING;
933         state.externalState = MOVING;
934         state.hysteresisCounter = 0;
935
936         initPositionAverageList(&positionAverageList, getAverageDepth());
937
938         if (!initOlsrTxTimer()) {
939                 stopReceiver();
940                 return false;
941         }
942
943         if (!initUplinkTxTimer()) {
944                 stopReceiver();
945                 return false;
946         }
947
948         restartOlsrTimer();
949         restartUplinkTimer();
950
951         return true;
952 }
953
954 /**
955  Stop the receiver
956  */
957 void stopReceiver(void) {
958         destroyUplinkTxTimer();
959         destroyOlsrTxTimer();
960
961         destroyPositionAverageList(&positionAverageList);
962
963         state.hysteresisCounter = 0;
964         state.externalState = MOVING;
965         state.internalState = MOVING;
966
967         transmitGpsInformation.updated = false;
968         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
969         memcpy(&transmitGpsInformation.txGateway, &olsr_cnf->main_addr, olsr_cnf->ipsize);
970
971         nmea_parser_destroy(&nmeaParser);
972
973         (void) pthread_mutex_destroy(&transmitGpsInformation.mutex);
974 }