PUD: force STATIONARY when the position just became valid
[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  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         /*
183          * convert nmeaINFO to wireformat olsr message (always, since we always try
184          * to send the position)
185          */
186         (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
187
188         /* only get the position when it is valid (no tx on invalid position) */
189         if (positionValid(&transmitGpsInformation.txPosition)) {
190                 /* fixup timestamp when the position was not updated */
191                 if (!transmitGpsInformation.updated) {
192                         nmea_time_now(&transmitGpsInformation.txPosition.nmeaInfo.utc);
193                 }
194
195                 txBufferBytesUsed += sizeof(UplinkHeader); /* keep before txBufferSpaceFree usage */
196                 pu_size = gpsToOlsr(&transmitGpsInformation.txPosition.nmeaInfo, pu, txBufferBytesFree,
197                                 ((state.externalState == MOVING) ? getUpdateIntervalMoving() : getUpdateIntervalStationary()));
198                 txBufferBytesUsed += pu_size;
199         }
200
201         transmitGpsInformation.updated = false;
202         (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
203
204         /*
205          * push out to all OLSR interfaces
206          */
207         if (((interfaces & OLSR) != 0) && (pu_size > 0)) {
208                 int r;
209                 struct interface *ifn;
210                 for (ifn = ifnet; ifn; ifn = ifn->int_next) {
211                         r = net_outbuffer_push(ifn, pu, pu_size);
212                         if (r != (int) pu_size) {
213                                 pudError(
214                                                 false,
215                                                 "Could not send to OLSR interface %s: %s (size=%u, r=%d)",
216                                                 ifn->int_name,
217                                                 ((r == -1) ? "no buffer was found" :
218                                                         (r == 0) ? "there was not enough room in the buffer" : "unknown reason"), pu_size, r);
219                         }
220 #ifdef PUD_DUMP_GPS_PACKETS_TX_OLSR
221                         else {
222                                 olsr_printf(0, "%s: packet sent to OLSR interface %s (%d bytes)\n",
223                                                 PUD_PLUGIN_ABBR, ifn->int_name, pu_size);
224                                 dump_packet((unsigned char *)pu, pu_size);
225                         }
226 #endif
227                 }
228
229                 /* loopback to tx interface when so configured */
230                 if (getUseLoopback()) {
231                         (void) packetReceivedFromOlsr(pu, NULL, NULL);
232                 }
233         }
234
235         /* push out over uplink when an uplink is configured */
236         if (((interfaces & UPLINK) != 0) && isUplinkAddrSet()) {
237                 int fd = getDownlinkSocketFd();
238                 if (fd != -1) {
239                         union olsr_sockaddr * uplink_addr = getUplinkAddr();
240                         union olsr_ip_addr * gw_addr = getBestUplinkGateway();
241
242                         UplinkMessage * cl_uplink = (UplinkMessage *) &txBuffer[txBufferBytesUsed];
243                         UplinkClusterLeader * cl = &cl_uplink->msg.clusterLeader;
244                         union olsr_ip_addr * cl_originator = getClusterLeaderOriginator(olsr_cnf->ip_version, cl);
245                         union olsr_ip_addr * cl_clusterLeader = getClusterLeaderClusterLeader(olsr_cnf->ip_version, cl);
246                         unsigned int cl_size =
247                                         sizeof(UplinkClusterLeader) - sizeof(cl->leader)
248                                                         + ((olsr_cnf->ip_version == AF_INET) ? sizeof(cl->leader.v4) :
249                                                                         sizeof(cl->leader.v6));
250
251                         /*
252                          * position update message (pu)
253                          */
254
255                         /* set header fields in position update uplink message and adjust
256                          * the validity time to the uplink validity time */
257                         if (pu_size > 0) {
258                                 PudOlsrPositionUpdate * pu_gpsMessage = getOlsrMessagePayload(olsr_cnf->ip_version, pu);
259
260                                 setUplinkMessageType(&pu_uplink->header, POSITION);
261                                 setUplinkMessageLength(&pu_uplink->header, pu_size);
262                                 setUplinkMessageIPv6(&pu_uplink->header, (olsr_cnf->ip_version != AF_INET));
263                                 setUplinkMessagePadding(&pu_uplink->header, 0);
264
265                                 /* fixup validity time */
266                                 setValidityTime(
267                                                 &pu_gpsMessage->validityTime,
268                                                 (state.externalState == MOVING) ?
269                                                                 getUplinkUpdateIntervalMoving() : getUplinkUpdateIntervalStationary());
270                         }
271
272                         /*
273                          * cluster leader message (cl)
274                          */
275
276                         /* set cl_uplink header fields */
277                         setUplinkMessageType(&cl_uplink->header, CLUSTERLEADER);
278                         setUplinkMessageLength(&cl_uplink->header, cl_size);
279                         setUplinkMessageIPv6(&cl_uplink->header, (olsr_cnf->ip_version != AF_INET));
280                         setUplinkMessagePadding(&cl_uplink->header, 0);
281
282                         /* setup cl */
283                         setClusterLeaderVersion(cl, PUD_WIRE_FORMAT_VERSION);
284                         setValidityTime(
285                                         &cl->validityTime,
286                                         (state.externalState == MOVING) ?
287                                                         getUplinkUpdateIntervalMoving() : getUplinkUpdateIntervalStationary());
288
289                         memcpy(cl_originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
290                         memcpy(cl_clusterLeader, gw_addr, olsr_cnf->ipsize);
291
292                         txBufferBytesUsed += sizeof(UplinkHeader);
293                         txBufferBytesUsed += cl_size;
294
295                         errno = 0;
296                         if (sendto(fd, &txBuffer, txBufferBytesUsed, 0, (struct sockaddr *) &uplink_addr->in,
297                                         sizeof(uplink_addr->in)) < 0) {
298                                 pudError(true, "Could not send to uplink (size=%u)", txBufferBytesUsed);
299                         }
300 #ifdef PUD_DUMP_GPS_PACKETS_TX_UPLINK
301                         else {
302                                 olsr_printf(0, "%s: packet sent to uplink (%d bytes)\n",
303                                                 PUD_PLUGIN_ABBR, pu_size);
304                                 dump_packet((unsigned char *)&txBuffer, txBufferBytesUsed);
305                         }
306 #endif
307                 }
308         }
309 }
310
311 /*
312  * Timer Callbacks
313  */
314
315 /**
316  The OLSR tx timer callback
317
318  @param context
319  unused
320  */
321 static void pud_olsr_tx_timer_callback(void *context __attribute__ ((unused))) {
322         txToAllOlsrInterfaces(OLSR);
323 }
324
325 /**
326  The uplink timer callback
327
328  @param context
329  unused
330  */
331 static void pud_uplink_timer_callback(void *context __attribute__ ((unused))) {
332         txToAllOlsrInterfaces(UPLINK);
333 }
334
335 /**
336  Detemine whether we are moving by comparing fields from the average
337  position against those of the last transmitted position.
338
339  MUST be called which the position average list locked.
340
341  @param avg
342  the average position
343  @param lastTx
344  the last transmitted position
345  @param result
346  the results of all movement criteria
347  */
348 static void detemineMoving(PositionUpdateEntry * avg,
349                 PositionUpdateEntry * lastTx, MovementType * result) {
350         /* avg field presence booleans */
351         bool avgHasSpeed;
352         bool avgHasPos;
353         bool avgHasHdop;
354         bool avgHasElv;
355         bool avgHasVdop;
356
357         /* lastTx field presence booleans */bool lastTxHasPos;
358         bool lastTxHasHdop;
359         bool lastTxHasElv;
360         bool lastTxHasVdop;
361
362         /* these have defaults */
363         double dopMultiplier;
364         double avgHdop;
365         double lastTxHdop;
366         double avgVdop;
367         double lastTxVdop;
368
369         /* calculated values and their validity booleans */
370         double hDistance;
371         double vDistance;
372         double hdopDistanceForOutside;
373         double hdopDistanceForInside;
374         double vdopDistanceForOutside;
375         double vdopDistanceForInside;
376         bool hDistanceValid;
377         bool hdopDistanceValid;
378         bool vDistanceValid;
379         bool vdopDistanceValid;
380
381         /* clear outputs */
382         result->moving = UNKNOWN;
383         result->overThresholds = UNKNOWN;
384         result->speedOverThreshold = UNKNOWN;
385         result->hDistanceOverThreshold = UNKNOWN;
386         result->vDistanceOverThreshold = UNKNOWN;
387         result->outside = UNKNOWN;
388         result->outsideHdop = UNKNOWN;
389         result->outsideVdop = UNKNOWN;
390         result->inside = UNKNOWN;
391         result->insideHdop = UNKNOWN;
392         result->insideVdop = UNKNOWN;
393
394         /*
395          * Validity
396          *
397          * avg  last  movingNow
398          *  0     0   STATIONARY : can't determine whether we're moving, force STATIONARY
399          *  0     1   STATIONARY : can't determine whether we're moving, force STATIONARY
400          *  1     0   STATIONARY : position just became valid, force STATIONARY to do averageing since we
401          *                         might be dealing with bad signal quality. we will go into moving when
402          *                         signal quality is good and we're actually moving.
403          *  1     1   determine via other parameters
404          */
405
406         if (!positionValid(avg)) {
407                 /* force stationary when the position is invalid */
408                 result->moving = UNSET;
409                 return;
410         }
411
412         /* avg is valid here */
413
414         if (!positionValid(lastTx)) {
415                 /* the position just became valid: force stationary */
416                 result->moving = UNSET;
417                 /* the rest is unknown */
418                 return;
419         }
420
421         /* both avg and lastTx are valid here */
422
423         /* avg field presence booleans */
424         avgHasSpeed = nmea_INFO_has_field(avg->nmeaInfo.smask, SPEED);
425         avgHasPos = nmea_INFO_has_field(avg->nmeaInfo.smask, LAT)
426                         && nmea_INFO_has_field(avg->nmeaInfo.smask, LON);
427         avgHasHdop = nmea_INFO_has_field(avg->nmeaInfo.smask, HDOP);
428         avgHasElv = nmea_INFO_has_field(avg->nmeaInfo.smask, ELV);
429         avgHasVdop = nmea_INFO_has_field(avg->nmeaInfo.smask, VDOP);
430
431         /* lastTx field presence booleans */
432         lastTxHasPos = nmea_INFO_has_field(lastTx->nmeaInfo.smask, LAT)
433                         && nmea_INFO_has_field(lastTx->nmeaInfo.smask, LON);
434         lastTxHasHdop = nmea_INFO_has_field(lastTx->nmeaInfo.smask, HDOP);
435         lastTxHasElv = nmea_INFO_has_field(lastTx->nmeaInfo.smask, ELV);
436         lastTxHasVdop = nmea_INFO_has_field(lastTx->nmeaInfo.smask, VDOP);
437
438         /* fill in some values _or_ defaults */
439         dopMultiplier = getDopMultiplier();
440         avgHdop = avgHasHdop ? avg->nmeaInfo.HDOP : getDefaultHdop();
441         lastTxHdop = lastTxHasHdop ? lastTx->nmeaInfo.HDOP : getDefaultHdop();
442         avgVdop = avgHasVdop ? avg->nmeaInfo.VDOP : getDefaultVdop();
443         lastTxVdop = lastTxHasVdop ? lastTx->nmeaInfo.VDOP : getDefaultVdop();
444
445         /*
446          * Calculations
447          */
448
449         /* hDistance */
450         if (avgHasPos && lastTxHasPos) {
451                 nmeaPOS avgPos;
452                 nmeaPOS lastTxPos;
453
454                 avgPos.lat = nmea_degree2radian(avg->nmeaInfo.lat);
455                 avgPos.lon = nmea_degree2radian(avg->nmeaInfo.lon);
456
457                 lastTxPos.lat = nmea_degree2radian(lastTx->nmeaInfo.lat);
458                 lastTxPos.lon = nmea_degree2radian(lastTx->nmeaInfo.lon);
459
460                 hDistance = nmea_distance_ellipsoid(&avgPos, &lastTxPos, NULL, NULL);
461                 hDistanceValid = true;
462         } else {
463                 hDistanceValid = false;
464         }
465
466         /* hdopDistance */
467         if (avgHasHdop || lastTxHasHdop) {
468                 hdopDistanceForOutside = dopMultiplier * (lastTxHdop + avgHdop);
469                 hdopDistanceForInside = dopMultiplier * (lastTxHdop - avgHdop);
470                 hdopDistanceValid = true;
471         } else {
472                 hdopDistanceValid = false;
473         }
474
475         /* vDistance */
476         if (avgHasElv && lastTxHasElv) {
477                 vDistance = fabs(lastTx->nmeaInfo.elv - avg->nmeaInfo.elv);
478                 vDistanceValid = true;
479         } else {
480                 vDistanceValid = false;
481         }
482
483         /* vdopDistance */
484         if (avgHasVdop || lastTxHasVdop) {
485                 vdopDistanceForOutside = dopMultiplier * (lastTxVdop + avgVdop);
486                 vdopDistanceForInside = dopMultiplier * (lastTxVdop - avgVdop);
487                 vdopDistanceValid = true;
488         } else {
489                 vdopDistanceValid = false;
490         }
491
492         /*
493          * Moving Criteria Evaluation Start
494          * We compare the average position against the last transmitted position.
495          */
496
497         /* Speed */
498         if (avgHasSpeed) {
499                 if (avg->nmeaInfo.speed >= getMovingSpeedThreshold()) {
500                         result->speedOverThreshold = SET;
501                 } else {
502                         result->speedOverThreshold = UNSET;
503                 }
504         }
505
506         /*
507          * Position
508          *
509          * avg  last  hDistanceMoving
510          *  0     0   determine via other parameters
511          *  0     1   determine via other parameters
512          *  1     0   MOVING
513          *  1     1   determine via distance threshold and HDOP
514          */
515         if (avgHasPos && !lastTxHasPos) {
516                 result->hDistanceOverThreshold = SET;
517         } else if (hDistanceValid) {
518                 if (hDistance >= getMovingDistanceThreshold()) {
519                         result->hDistanceOverThreshold = SET;
520                 } else {
521                         result->hDistanceOverThreshold = UNSET;
522                 }
523
524                 /*
525                  * Position with HDOP
526                  *
527                  * avg  last  movingNow
528                  *  0     0   determine via other parameters
529                  *  0     1   determine via position with HDOP (avg has default HDOP)
530                  *  1     0   determine via position with HDOP (lastTx has default HDOP)
531                  *  1     1   determine via position with HDOP
532                  */
533                 if (hdopDistanceValid) {
534                         /* we are outside the HDOP when the HDOPs no longer overlap */
535                         if (hDistance > hdopDistanceForOutside) {
536                                 result->outsideHdop = SET;
537                         } else {
538                                 result->outsideHdop = UNSET;
539                         }
540
541                         /* we are inside the HDOP when the HDOPs fully overlap */
542                         if (hDistance <= hdopDistanceForInside) {
543                                 result->insideHdop = SET;
544                         } else {
545                                 result->insideHdop = UNSET;
546                         }
547                 }
548         }
549
550         /*
551          * Elevation
552          *
553          * avg  last  movingNow
554          *  0     0   determine via other parameters
555          *  0     1   determine via other parameters
556          *  1     0   MOVING
557          *  1     1   determine via distance threshold and VDOP
558          */
559         if (avgHasElv && !lastTxHasElv) {
560                 result->vDistanceOverThreshold = SET;
561         } else if (vDistanceValid) {
562                 if (vDistance >= getMovingDistanceThreshold()) {
563                         result->vDistanceOverThreshold = SET;
564                 } else {
565                         result->vDistanceOverThreshold = UNSET;
566                 }
567
568                 /*
569                  * Elevation with VDOP
570                  *
571                  * avg  last  movingNow
572                  *  0     0   determine via other parameters
573                  *  0     1   determine via elevation with VDOP (avg has default VDOP)
574                  *  1     0   determine via elevation with VDOP (lastTx has default VDOP)
575                  *  1     1   determine via elevation with VDOP
576                  */
577                 if (vdopDistanceValid) {
578                         /* we are outside the VDOP when the VDOPs no longer overlap */
579                         if (vDistance > vdopDistanceForOutside) {
580                                 result->outsideVdop = SET;
581                         } else {
582                                 result->outsideVdop = UNSET;
583                         }
584
585                         /* we are inside the VDOP when the VDOPs fully overlap */
586                         if (vDistance <= vdopDistanceForInside) {
587                                 result->insideVdop = SET;
588                         } else {
589                                 result->insideVdop = UNSET;
590                         }
591                 }
592         }
593
594         /*
595          * Moving Criteria Evaluation End
596          */
597
598         /* accumulate inside criteria */
599         if ((result->insideHdop == SET) && (result->insideVdop == SET)) {
600                 result->inside = SET;
601         } else if ((result->insideHdop == UNSET) || (result->insideVdop == UNSET)) {
602                 result->inside = UNSET;
603         }
604
605         /* accumulate outside criteria */
606         if ((result->outsideHdop == SET) || (result->outsideVdop == SET)) {
607                 result->outside = SET;
608         } else if ((result->outsideHdop == UNSET)
609                         || (result->outsideVdop == UNSET)) {
610                 result->outside = UNSET;
611         }
612
613         /* accumulate threshold criteria */
614         if ((result->speedOverThreshold == SET)
615                         || (result->hDistanceOverThreshold == SET)
616                         || (result->vDistanceOverThreshold == SET)) {
617                 result->overThresholds = SET;
618         } else if ((result->speedOverThreshold == UNSET)
619                         || (result->hDistanceOverThreshold == UNSET)
620                         || (result->vDistanceOverThreshold == UNSET)) {
621                 result->overThresholds = UNSET;
622         }
623
624         /* accumulate moving criteria */
625         if ((result->overThresholds == SET) || (result->outside == SET)) {
626                 result->moving = SET;
627         } else if ((result->overThresholds == UNSET)
628                         && (result->outside == UNSET)) {
629                 result->moving = UNSET;
630         }
631
632         return;
633 }
634
635 /**
636  Restart the OLSR tx timer
637  */
638 static void restartOlsrTimer(void) {
639         if (!restartOlsrTxTimer(
640                         (state.externalState == STATIONARY) ? getUpdateIntervalStationary() :
641                                         getUpdateIntervalMoving(), &pud_olsr_tx_timer_callback)) {
642                 pudError(0, "Could not restart OLSR tx timer, no periodic"
643                                 " position updates will be sent to the OLSR network");
644         }
645 }
646
647 /**
648  Restart the uplink tx timer
649  */
650 static void restartUplinkTimer(void) {
651         if (!restartUplinkTxTimer(
652                         (state.externalState == STATIONARY) ? getUplinkUpdateIntervalStationary() :
653                                         getUplinkUpdateIntervalMoving(),
654                         &pud_uplink_timer_callback)) {
655                 pudError(0, "Could not restart uplink timer, no periodic"
656                                 " position updates will be uplinked");
657         }
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 = STATIONARY;
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 stationary */
808                                 newState = STATIONARY;
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 (externalStateChange) {
846                 TimedTxInterface interfaces = OLSR; /* always send over olsr */
847                 restartOlsrTimer();
848
849                 if (isUplinkAddrSet()) {
850                         interfaces |= UPLINK;
851                         restartUplinkTimer();
852                 }
853
854                 /* do an immediate transmit */
855                 txToAllOlsrInterfaces(interfaces);
856         }
857
858         retval = true;
859
860         end: (void) pthread_mutex_unlock(&positionAverageList.mutex);
861         return retval;
862 }
863
864 /*
865  * Receiver start/stop
866  */
867
868 /**
869  Start the receiver
870
871  @return
872  - false on failure
873  - true otherwise
874  */
875 bool startReceiver(void) {
876         pthread_mutexattr_t attr;
877         if (pthread_mutexattr_init(&attr)) {
878                 return false;
879         }
880         if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE_NP)) {
881                 return false;
882         }
883         if (pthread_mutex_init(&transmitGpsInformation.mutex, &attr)) {
884                 return false;
885         }
886
887         if (!nmea_parser_init(&nmeaParser)) {
888                 pudError(false, "Could not initialise NMEA parser");
889                 return false;
890         }
891
892         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
893         transmitGpsInformation.updated = false;
894
895         nmea_zero_INFO(&txPosition.nmeaInfo);
896
897         state.internalState = STATIONARY;
898         state.externalState = STATIONARY;
899         state.hysteresisCounter = 0;
900
901         initPositionAverageList(&positionAverageList, getAverageDepth());
902
903         if (!initOlsrTxTimer()) {
904                 stopReceiver();
905                 return false;
906         }
907
908         if (!initUplinkTxTimer()) {
909                 stopReceiver();
910                 return false;
911         }
912
913         restartOlsrTimer();
914         restartUplinkTimer();
915
916         return true;
917 }
918
919 /**
920  Stop the receiver
921  */
922 void stopReceiver(void) {
923         destroyUplinkTxTimer();
924         destroyOlsrTxTimer();
925
926         destroyPositionAverageList(&positionAverageList);
927
928         state.hysteresisCounter = 0;
929         state.externalState = STATIONARY;
930         state.internalState = STATIONARY;
931
932         nmea_zero_INFO(&txPosition.nmeaInfo);
933
934         transmitGpsInformation.updated = false;
935         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
936
937         nmea_parser_destroy(&nmeaParser);
938
939         (void) pthread_mutex_destroy(&transmitGpsInformation.mutex);
940 }