PUD: add downlink port to clusterleader message
[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
17 /* System includes */
18 #include <stddef.h>
19 #include <nmea/parser.h>
20 #include <nmea/info.h>
21 #include <pthread.h>
22 #include <nmea/info.h>
23 #include <string.h>
24 #include <nmea/gmath.h>
25 #include <nmea/sentence.h>
26 #include <math.h>
27 #include <net/if.h>
28 #include <assert.h>
29
30 /* Debug includes */
31 #if defined(PUD_DUMP_GPS_PACKETS_TX_OLSR) || \
32         defined(PUD_DUMP_GPS_PACKETS_TX_UPLINK) || \
33         defined(PUD_DUMP_AVERAGING)
34 #include "olsr.h"
35 #endif
36
37 /*
38  * NMEA parser
39  */
40
41 /** The NMEA string parser */
42 static nmeaPARSER nmeaParser;
43
44 /*
45  * State
46  */
47
48 /** Type describing a tri-state boolean */
49 typedef enum _TristateBoolean {
50         UNKNOWN = 0,
51         UNSET = 1,
52         SET = 2
53 } TristateBoolean;
54
55 #define TristateBooleanToString(s)      ((s == SET) ? "set" : \
56                                                                          (s == UNSET) ? "unset" : \
57                                                                          "unknown")
58
59 /** Type describing movement state */
60 typedef enum _MovementState {
61         STATIONARY = 0,
62         MOVING = 1
63 } MovementState;
64
65 #define MovementStateToString(s)        ((s == MOVING) ? "moving" : \
66                                                                          "stationary")
67
68 /** Type describing state */
69 typedef struct _StateType {
70         MovementState internalState; /**< the internal movement state */
71         MovementState externalState; /**< the externally visible movement state */
72         unsigned long long hysteresisCounter; /**< the hysteresis counter external state changes */
73 } StateType;
74
75 /** The state */
76 static StateType state = {
77                 .internalState = MOVING,
78                 .externalState = MOVING,
79                 .hysteresisCounter = 0
80 };
81
82 /** Type describing movement calculations */
83 typedef struct _MovementType {
84         TristateBoolean moving; /**< SET: we are moving */
85
86         TristateBoolean overThresholds; /**< SET: at least 1 threshold state is set */
87         TristateBoolean speedOverThreshold; /**< SET: speed is over threshold */
88         TristateBoolean hDistanceOverThreshold; /**< SET: horizontal distance is outside threshold */
89         TristateBoolean vDistanceOverThreshold; /**< SET: vertical distance is outside threshold */
90
91         TristateBoolean outside; /**< SET: at least 1 outside state is SET */
92         TristateBoolean outsideHdop; /**< SET: avg is outside lastTx HDOP */
93         TristateBoolean outsideVdop; /**< SET: avg is outside lastTx VDOP */
94
95         TristateBoolean inside; /**< SET: all inside states are SET */
96         TristateBoolean insideHdop; /**< SET: avg is inside lastTx HDOP */
97         TristateBoolean insideVdop; /**< SET: avg is inside lastTx VDOP */
98 } MovementType;
99
100 /*
101  * Averaging
102  */
103
104 /** The average position with its administration */
105 static PositionAverageList positionAverageList;
106
107 /*
108  * TX to OLSR
109  */
110
111 typedef enum _TimedTxInterface {
112         OLSR = 1,
113         UPLINK = 2
114 } TimedTxInterface;
115
116 /** Structure of the latest GPS information that is transmitted */
117 typedef struct _TransmitGpsInformation {
118         pthread_mutex_t mutex; /**< access mutex */
119         bool updated; /**< true when the information was updated */
120         PositionUpdateEntry txPosition; /**< The last transmitted position */
121 } TransmitGpsInformation;
122
123 /** The latest position information that is transmitted */
124 static TransmitGpsInformation transmitGpsInformation;
125
126 /** The last transmitted position.
127  * The same as transmitGpsInformation.txPosition.
128  * We keep this because then we can access the information without locking
129  * mutexes. */
130 static PositionUpdateEntry txPosition;
131
132 /** The size of the buffer in which the OLSR and uplink messages are assembled */
133 #define TX_BUFFER_SIZE_FOR_OLSR 1024
134
135 /*
136  * Functions
137  */
138
139 /**
140  This function is called every time before a message is sent on a specific
141  interface. It can manipulate the outgoing message.
142  Note that a change to the outgoing messages is carried over to the message
143  that goes out on the next interface when the message is _not_ reset
144  before it is sent out on the next interface.
145
146  @param olsrMessage
147  A pointer to the outgoing message
148  @param ifn
149  A pointer to the OLSR interface structure
150  */
151 static void nodeIdPreTransmitHook(union olsr_message *olsrMessage,
152                 struct interface *ifn) {
153         /* set the MAC address in the message when needed */
154         if (unlikely(getNodeIdTypeNumber() == PUD_NODEIDTYPE_MAC)) {
155                 TOLSRNetworkInterface * olsrIf = getOlsrNetworkInterface(ifn);
156                 PudOlsrPositionUpdate * olsrGpsMessage =
157                                 getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
158
159                 if (likely(olsrIf != NULL)) {
160                         setPositionUpdateNodeId(olsrGpsMessage, &olsrIf->hwAddress[0],
161                                         PUD_NODEIDTYPE_MAC_BYTES, false);
162                 } else {
163                         unsigned char buffer[PUD_NODEIDTYPE_MAC_BYTES] = { 0 };
164                         setPositionUpdateNodeId(olsrGpsMessage, &buffer[0],
165                                         PUD_NODEIDTYPE_MAC_BYTES, false);
166
167                         pudError(false, "Could not find OLSR interface %s, cleared its"
168                                 " MAC address in the OLSR message\n", ifn->int_name);
169                 }
170         }
171 }
172
173 /**
174  Determine whether s position is valid.
175
176  @param position
177  a pointer to a position
178
179  @return
180  - true when valid
181  - false otherwise
182  */
183 static bool positionValid(PositionUpdateEntry * position) {
184         return (nmea_INFO_has_field(position->nmeaInfo.smask, FIX)
185                         && (position->nmeaInfo.fix != NMEA_FIX_BAD));
186 }
187
188 /** Send the transmit buffer out over all designated interfaces, called as a
189  * timer callback and also immediately on an external state change.
190
191  @param interfaces
192  a bitmap defining which interfaces to send over
193  */
194 static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
195         /** buffer used to fill in the OLSR and uplink messages */
196         unsigned char txBuffer[TX_BUFFER_SIZE_FOR_OLSR];
197         UplinkMessage * message1 = (UplinkMessage *) &txBuffer[0];
198
199         unsigned int txBufferSpaceTaken = 0;
200         #define txBufferSpaceFree       (sizeof(txBuffer) - txBufferSpaceTaken)
201
202         /* the first message in the buffer is an OLSR position update */
203         union olsr_message * olsrMessage =
204                         (union olsr_message *) &message1->msg.olsrMessage;
205         unsigned int aligned_size = 0;
206
207         /* convert nmeaINFO to wireformat olsr message */
208         (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
209         if (!transmitGpsInformation.updated
210                         && positionValid(&transmitGpsInformation.txPosition)) {
211                 nmea_time_now(&transmitGpsInformation.txPosition.nmeaInfo.utc);
212         }
213
214         txBufferSpaceTaken += sizeof(UplinkHeader);
215         aligned_size = gpsToOlsr(&transmitGpsInformation.txPosition.nmeaInfo,
216                         olsrMessage, txBufferSpaceFree,
217                         ((state.externalState == MOVING) ? getUpdateIntervalMoving()
218                                                 : getUpdateIntervalStationary()));
219         txBufferSpaceTaken += aligned_size;
220         transmitGpsInformation.updated = false;
221         (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
222
223         if (aligned_size > 0) {
224                 /* push out to all OLSR interfaces */
225                 if ((interfaces & OLSR) != 0) {
226                         int r;
227                         struct interface *ifn;
228                         for (ifn = ifnet; ifn; ifn = ifn->int_next) {
229                                 nodeIdPreTransmitHook(olsrMessage, ifn);
230                                 r = net_outbuffer_push(ifn, olsrMessage, aligned_size);
231                                 if (r != (int) aligned_size) {
232                                         pudError(
233                                                         false,
234                                                         "Could not send to OLSR interface %s: %s"
235                                                                         " (aligned_size=%u, r=%d)",
236                                                         ifn->int_name,
237                                                         ((r == -1) ? "no buffer was found"
238                                                         : (r == 0) ? "there was not enough room in the buffer"
239                                                         : "unknown reason"), aligned_size, r);
240                                 }
241 #ifdef PUD_DUMP_GPS_PACKETS_TX_OLSR
242                                 else {
243                                         olsr_printf(0, "%s: packet sent to OLSR interface %s (%d bytes)\n",
244                                                         PUD_PLUGIN_ABBR, ifn->int_name, aligned_size);
245                                         dump_packet((unsigned char *)olsrMessage, aligned_size);
246                                 }
247 #endif
248                         }
249
250                         /* loopback to tx interface when so configured */
251                         if (getUseLoopback()) {
252                                 (void) packetReceivedFromOlsr(olsrMessage, NULL, NULL);
253                         }
254                 }
255
256                 /* push out over uplink when an uplink is configured */
257                 if (((interfaces & UPLINK) != 0) && isUplinkAddrSet()) {
258                         int fd = getUplinkSocketFd();
259                         if (fd != -1) {
260                                 union olsr_ip_addr * gwAddr = getBestUplinkGateway();
261
262                                 UplinkMessage * message2 =
263                                                 (UplinkMessage *) &txBuffer[aligned_size
264                                                                 + sizeof(UplinkHeader)];
265                                 UplinkClusterLeader * clusterLeaderMessage =
266                                                 &message2->msg.clusterLeader;
267                                 unsigned int message2Size;
268                                 union olsr_ip_addr * clOriginator;
269                                 union olsr_ip_addr * clClusterLeader;
270
271                                 /*
272                                  * position update message (message1)
273                                  */
274
275                                 union olsr_sockaddr * address = getUplinkAddr();
276                                 PudOlsrPositionUpdate * olsrGpsMessage = getOlsrMessagePayload(
277                                                 olsr_cnf->ip_version, olsrMessage);
278
279                                 /* set header fields */
280                                 setUplinkMessageType(&message1->header, POSITION);
281                                 setUplinkMessageLength(&message1->header, aligned_size);
282                                 setUplinkMessageIPv6(&message1->header,
283                                                 (olsr_cnf->ip_version != AF_INET));
284                                 setUplinkMessagePadding(&message1->header, 0);
285
286                                 /* fixup validity time */
287                                 setValidityTime(&olsrGpsMessage->validityTime,
288                                                 (state.externalState == MOVING) ?
289                                                 getUplinkUpdateIntervalMoving() :
290                                                 getUplinkUpdateIntervalStationary());
291
292                                 /*
293                                  * cluster leader message (message2)
294                                  */
295
296                                 clOriginator = getClusterLeaderOriginator(olsr_cnf->ip_version,
297                                                 clusterLeaderMessage);
298                                 clClusterLeader = getClusterLeaderClusterLeader(
299                                                 olsr_cnf->ip_version, clusterLeaderMessage);
300                                 message2Size = sizeof(UplinkClusterLeader)
301                                                 - sizeof(clusterLeaderMessage->leader);
302                                 if (olsr_cnf->ip_version == AF_INET) {
303                                         message2Size += sizeof(clusterLeaderMessage->leader.v4);
304                                 } else {
305                                         message2Size = sizeof(clusterLeaderMessage->leader.v6);
306                                 }
307
308                                 /* set header fields */
309                                 setUplinkMessageType(&message2->header, CLUSTERLEADER);
310                                 setUplinkMessageLength(&message2->header, message2Size);
311                                 setUplinkMessageIPv6(&message2->header,
312                                                 (olsr_cnf->ip_version != AF_INET));
313                                 setUplinkMessagePadding(&message2->header, 0);
314                                 txBufferSpaceTaken += sizeof(UplinkHeader);
315
316                                 /* setup validity time */
317                                 setClusterLeaderVersion(clusterLeaderMessage, PUD_WIRE_FORMAT_VERSION);
318                                 setValidityTime(&clusterLeaderMessage->validityTime,
319                                                 (state.externalState == MOVING) ?
320                                                 getUplinkUpdateIntervalMoving() :
321                                                 getUplinkUpdateIntervalStationary());
322                                 setClusterLeaderDownlinkPort(clusterLeaderMessage,
323                                                 getDownlinkPort());
324
325                                 memcpy(clOriginator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
326                                 memcpy(clClusterLeader, gwAddr, olsr_cnf->ipsize);
327
328                                 txBufferSpaceTaken += message2Size;
329
330                                 errno = 0;
331                                 if (sendto(fd, &txBuffer, txBufferSpaceTaken, 0,
332                                         (struct sockaddr *) &address->in, sizeof(address->in)) < 0) {
333                                         pudError(true, "Could not send to uplink"
334                                                         " (aligned_size=%u)", txBufferSpaceTaken);
335                                 }
336 #ifdef PUD_DUMP_GPS_PACKETS_TX_UPLINK
337                                 else {
338                                         olsr_printf(0, "%s: packet sent to uplink (%d bytes)\n",
339                                                         PUD_PLUGIN_ABBR, aligned_size);
340                                         dump_packet((unsigned char *)&txBuffer,
341                                                         (sizeof(txBuffer) -
342                                                          sizeof(txBuffer.msg)) + aligned_size);
343                                 }
344 #endif
345                         }
346                 }
347         }
348 }
349
350 /*
351  * Timer Callbacks
352  */
353
354 /**
355  The OLSR tx timer callback
356
357  @param context
358  unused
359  */
360 static void pud_olsr_tx_timer_callback(void *context __attribute__ ((unused))) {
361         txToAllOlsrInterfaces(OLSR);
362 }
363
364 /**
365  The uplink timer callback
366
367  @param context
368  unused
369  */
370 static void pud_uplink_timer_callback(void *context __attribute__ ((unused))) {
371         txToAllOlsrInterfaces(UPLINK);
372 }
373
374 /**
375  Detemine whether we are moving by comparing fields from the average
376  position against those of the last transmitted position.
377
378  MUST be called which the position average list locked.
379
380  @param avg
381  the average position
382  @param lastTx
383  the last transmitted position
384  @param result
385  the results of all movement criteria
386  */
387 static void detemineMoving(PositionUpdateEntry * avg,
388                 PositionUpdateEntry * lastTx, MovementType * result) {
389         /* avg field presence booleans */
390         bool avgHasSpeed;
391         bool avgHasPos;
392         bool avgHasHdop;
393         bool avgHasElv;
394         bool avgHasVdop;
395
396         /* lastTx field presence booleans */bool lastTxHasPos;
397         bool lastTxHasHdop;
398         bool lastTxHasElv;
399         bool lastTxHasVdop;
400
401         /* these have defaults */
402         double dopMultiplier;
403         double avgHdop;
404         double lastTxHdop;
405         double avgVdop;
406         double lastTxVdop;
407
408         /* calculated values and their validity booleans */
409         double hDistance;
410         double vDistance;
411         double hdopDistanceForOutside;
412         double hdopDistanceForInside;
413         double vdopDistanceForOutside;
414         double vdopDistanceForInside;
415         bool hDistanceValid;
416         bool hdopDistanceValid;
417         bool vDistanceValid;
418         bool vdopDistanceValid;
419
420         /* clear outputs */
421         memset(result, UNKNOWN, sizeof(MovementType));
422
423         /*
424          * Validity
425          *
426          * avg  last  movingNow
427          *  0     0   UNKNOWN : can't determine whether we're moving
428          *  0     1   UNKNOWN : can't determine whether we're moving
429          *  1     0   MOVING  : always seen as movement
430          *  1     1   determine via other parameters
431          */
432
433         if (!positionValid(avg)) {
434                 /* everything is unknown */
435                 return;
436         }
437
438         /* avg is valid here */
439
440         if (!positionValid(lastTx)) {
441                 result->moving = SET;
442                 /* the rest is 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  Update the latest GPS information. This function is called when a packet is
662  received from a rxNonOlsr interface, containing one or more NMEA strings with
663  GPS information.
664
665  @param rxBuffer
666  the receive buffer with the received NMEA string(s)
667  @param rxCount
668  the number of bytes in the receive buffer
669
670  @return
671  - false on failure
672  - true otherwise
673  */
674 bool receiverUpdateGpsInformation(unsigned char * rxBuffer, size_t rxCount) {
675         static const char * rxBufferPrefix = "$GP";
676         static const size_t rxBufferPrefixLength = 3;
677
678         bool retval = false;
679         PositionUpdateEntry * incomingEntry;
680         MovementState newState = MOVING;
681         PositionUpdateEntry * posAvgEntry;
682         MovementType movementResult;
683         TristateBoolean movingNow;
684         bool internalStateChange = false;
685         bool externalStateChange = false;
686         bool updateTransmitGpsInformation = false;
687
688         /* do not process when the message does not start with $GP */
689         if ((rxCount < rxBufferPrefixLength) || (strncmp((char *) rxBuffer,
690                         rxBufferPrefix, rxBufferPrefixLength) != 0)) {
691                 return true;
692         }
693
694         (void) pthread_mutex_lock(&positionAverageList.mutex);
695
696         /* parse all NMEA strings in the rxBuffer into the incoming entry */
697         incomingEntry = getPositionAverageEntry(&positionAverageList, INCOMING);
698         nmea_zero_INFO(&incomingEntry->nmeaInfo);
699         nmea_parse(&nmeaParser, (char *) rxBuffer, rxCount,
700                         &incomingEntry->nmeaInfo);
701
702 #if defined(PUD_DUMP_AVERAGING)
703         dump_nmeaInfo(&incomingEntry->nmeaInfo,
704                         "receiverUpdateGpsInformation: incoming entry");
705 #endif /* PUD_DUMP_AVERAGING */
706
707         /* ignore when no useful information */
708         if (incomingEntry->nmeaInfo.smask == GPNON) {
709                 retval = true;
710                 goto end;
711         }
712
713         nmea_INFO_sanitise(&incomingEntry->nmeaInfo);
714
715 #if defined(PUD_DUMP_AVERAGING)
716         dump_nmeaInfo(&incomingEntry->nmeaInfo,
717                         "receiverUpdateGpsInformation: incoming entry after sanitise");
718 #endif /* PUD_DUMP_AVERAGING */
719
720         /* we always work with latitude, longitude in degrees and DOPs in meters */
721         nmea_INFO_unit_conversion(&incomingEntry->nmeaInfo);
722
723 #if defined(PUD_DUMP_AVERAGING)
724         dump_nmeaInfo(&incomingEntry->nmeaInfo,
725                         "receiverUpdateGpsInformation: incoming entry after unit conversion");
726 #endif /* PUD_DUMP_AVERAGING */
727
728         /*
729          * Averageing
730          */
731
732         if (state.internalState == MOVING) {
733                 /* flush average: keep only the incoming entry */
734                 flushPositionAverageList(&positionAverageList);
735         }
736         addNewPositionToAverage(&positionAverageList, incomingEntry);
737         posAvgEntry = getPositionAverageEntry(&positionAverageList, AVERAGE);
738
739         /*
740          * Movement detection
741          */
742
743         detemineMoving(posAvgEntry, &txPosition, &movementResult);
744         movingNow = movementResult.moving;
745
746 #if defined(PUD_DUMP_AVERAGING)
747         olsr_printf(0, "receiverUpdateGpsInformation: internalState = %s\n",
748                         MovementStateToString(state.internalState));
749         olsr_printf(0, "receiverUpdateGpsInformation: movingNow     = %s\n",
750                         TristateBooleanToString(movingNow));
751 #endif /* PUD_DUMP_AVERAGING */
752
753         /*
754          * Internal State
755          */
756
757         if (movingNow == SET) {
758                 newState = MOVING;
759         } else if (movingNow == UNSET) {
760                 newState = STATIONARY;
761         }
762         internalStateChange = (state.internalState != newState);
763         state.internalState = newState;
764
765         /*
766          * External State (+ hysteresis)
767          */
768
769         if (internalStateChange) {
770                 /* restart hysteresis for external state change when we have an internal
771                  * state change */
772                 state.hysteresisCounter = 0;
773         }
774
775         /* when internal state and external state are not the same we need to
776          * perform hysteresis before we can propagate the internal state to the
777          * external state */
778         newState = state.externalState;
779         if (state.internalState != state.externalState) {
780                 switch (state.internalState) {
781                         case STATIONARY:
782                                 /* external state is MOVING */
783
784                                 /* delay going to stationary a bit */
785                                 state.hysteresisCounter++;
786
787                                 if (state.hysteresisCounter
788                                                 >= getHysteresisCountToStationary()) {
789                                         /* outside the hysteresis range, go to stationary */
790                                         newState = STATIONARY;
791                                 }
792                                 break;
793
794                         case MOVING:
795                                 /* external state is STATIONARY */
796
797                                 /* delay going to moving a bit */
798                                 state.hysteresisCounter++;
799
800                                 if (state.hysteresisCounter >= getHysteresisCountToMoving()) {
801                                         /* outside the hysteresis range, go to moving */
802                                         newState = MOVING;
803                                 }
804                                 break;
805
806                         default:
807                                 /* when unknown do just as if we transition into moving */
808                                 newState = MOVING;
809                                 break;
810                 }
811         }
812         externalStateChange = (state.externalState != newState);
813         state.externalState = newState;
814
815 #if defined(PUD_DUMP_AVERAGING)
816         olsr_printf(0, "receiverUpdateGpsInformation: newState = %s\n",
817                         MovementStateToString(newState));
818         dump_nmeaInfo(&posAvgEntry->nmeaInfo,
819                         "receiverUpdateGpsInformation: posAvgEntry");
820 #endif /* PUD_DUMP_AVERAGING */
821
822         /*
823          * Update transmitGpsInformation
824          */
825
826         updateTransmitGpsInformation = externalStateChange
827                         || (positionValid(posAvgEntry) && !positionValid(&txPosition))
828                         || (movementResult.inside == SET);
829
830         if ((state.externalState == MOVING) || updateTransmitGpsInformation) {
831                 memcpy(&txPosition.nmeaInfo, &posAvgEntry->nmeaInfo, sizeof(nmeaINFO));
832                 (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
833                 memcpy(&transmitGpsInformation.txPosition.nmeaInfo,
834                                 &posAvgEntry->nmeaInfo, sizeof(nmeaINFO));
835                 transmitGpsInformation.updated = true;
836
837 #if defined(PUD_DUMP_AVERAGING)
838                 dump_nmeaInfo(&transmitGpsInformation.txPosition.nmeaInfo,
839                         "receiverUpdateGpsInformation: transmitGpsInformation");
840 #endif /* PUD_DUMP_AVERAGING */
841
842                 (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
843         }
844
845         if (updateTransmitGpsInformation) {
846                 TimedTxInterface interfaces = OLSR; /* always send over olsr */
847                 if (!restartOlsrTxTimer(
848                                 (state.externalState == STATIONARY) ? getUpdateIntervalStationary()
849                                 : getUpdateIntervalMoving(), &pud_olsr_tx_timer_callback)) {
850                         pudError(0, "Could not restart OLSR tx timer, no periodic"
851                                         " position updates will be sent to the OLSR network");
852                 }
853
854                 if (isUplinkAddrSet()) {
855                         interfaces |= UPLINK;
856                         if (!restartUplinkTxTimer(
857                                         (state.externalState == STATIONARY) ? getUplinkUpdateIntervalStationary()
858                                         : getUplinkUpdateIntervalMoving(), &pud_uplink_timer_callback)
859                                         ) {
860                                 pudError(0, "Could not restart uplink timer, no periodic"
861                                                 " position updates will be uplinked");
862                         }
863                 }
864
865                 /* do an immediate transmit */
866                 txToAllOlsrInterfaces(interfaces);
867         }
868
869         retval = true;
870
871         end: (void) pthread_mutex_unlock(&positionAverageList.mutex);
872         return retval;
873 }
874
875 /*
876  * Receiver start/stop
877  */
878
879 /**
880  Start the receiver
881
882  @return
883  - false on failure
884  - true otherwise
885  */
886 bool startReceiver(void) {
887         pthread_mutexattr_t attr;
888         if (pthread_mutexattr_init(&attr)) {
889                 return false;
890         }
891         if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE_NP)) {
892                 return false;
893         }
894         if (pthread_mutex_init(&transmitGpsInformation.mutex, &attr)) {
895                 return false;
896         }
897
898         if (!nmea_parser_init(&nmeaParser)) {
899                 pudError(false, "Could not initialise NMEA parser");
900                 return false;
901         }
902
903         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
904         transmitGpsInformation.updated = false;
905
906         nmea_zero_INFO(&txPosition.nmeaInfo);
907
908         state.internalState = MOVING;
909         state.externalState = MOVING;
910         state.hysteresisCounter = 0;
911
912         initPositionAverageList(&positionAverageList, getAverageDepth());
913
914         if (!initOlsrTxTimer()) {
915                 stopReceiver();
916                 return false;
917         }
918
919         if (!initUplinkTxTimer()) {
920                 stopReceiver();
921                 return false;
922         }
923
924         return true;
925 }
926
927 /**
928  Stop the receiver
929  */
930 void stopReceiver(void) {
931         destroyUplinkTxTimer();
932         destroyOlsrTxTimer();
933
934         destroyPositionAverageList(&positionAverageList);
935
936         state.hysteresisCounter = 0;
937         state.externalState = MOVING;
938         state.internalState = MOVING;
939
940         nmea_zero_INFO(&txPosition.nmeaInfo);
941
942         transmitGpsInformation.updated = false;
943         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
944
945         nmea_parser_destroy(&nmeaParser);
946
947         (void) pthread_mutex_destroy(&transmitGpsInformation.mutex);
948 }