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