f0dfac3bb9152cdd8f8a60dbf115cd80c18b6ef4
[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         /*
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   UNKNOWN : can't determine whether we're moving
399          *  0     1   UNKNOWN : can't determine whether we're moving
400          *  1     0   UNKNOWN : can't determine whether we're moving
401          *  1     1   determine via other parameters
402          */
403
404         if (!positionValid(avg)) {
405                 result->moving = UNKNOWN;
406                 return;
407         }
408
409         /* avg is valid here */
410
411         if (!positionValid(lastTx)) {
412                 result->moving = UNKNOWN;
413                 return;
414         }
415
416         /* both avg and lastTx are valid here */
417
418         /* avg field presence booleans */
419         avgHasSpeed = nmea_INFO_has_field(avg->nmeaInfo.smask, SPEED);
420         avgHasPos = nmea_INFO_has_field(avg->nmeaInfo.smask, LAT)
421                         && nmea_INFO_has_field(avg->nmeaInfo.smask, LON);
422         avgHasHdop = nmea_INFO_has_field(avg->nmeaInfo.smask, HDOP);
423         avgHasElv = nmea_INFO_has_field(avg->nmeaInfo.smask, ELV);
424         avgHasVdop = nmea_INFO_has_field(avg->nmeaInfo.smask, VDOP);
425
426         /* lastTx field presence booleans */
427         lastTxHasPos = nmea_INFO_has_field(lastTx->nmeaInfo.smask, LAT)
428                         && nmea_INFO_has_field(lastTx->nmeaInfo.smask, LON);
429         lastTxHasHdop = nmea_INFO_has_field(lastTx->nmeaInfo.smask, HDOP);
430         lastTxHasElv = nmea_INFO_has_field(lastTx->nmeaInfo.smask, ELV);
431         lastTxHasVdop = nmea_INFO_has_field(lastTx->nmeaInfo.smask, VDOP);
432
433         /* fill in some values _or_ defaults */
434         dopMultiplier = getDopMultiplier();
435         avgHdop = avgHasHdop ? avg->nmeaInfo.HDOP : getDefaultHdop();
436         lastTxHdop = lastTxHasHdop ? lastTx->nmeaInfo.HDOP : getDefaultHdop();
437         avgVdop = avgHasVdop ? avg->nmeaInfo.VDOP : getDefaultVdop();
438         lastTxVdop = lastTxHasVdop ? lastTx->nmeaInfo.VDOP : getDefaultVdop();
439
440         /*
441          * Calculations
442          */
443
444         /* hDistance */
445         if (avgHasPos && lastTxHasPos) {
446                 nmeaPOS avgPos;
447                 nmeaPOS lastTxPos;
448
449                 avgPos.lat = nmea_degree2radian(avg->nmeaInfo.lat);
450                 avgPos.lon = nmea_degree2radian(avg->nmeaInfo.lon);
451
452                 lastTxPos.lat = nmea_degree2radian(lastTx->nmeaInfo.lat);
453                 lastTxPos.lon = nmea_degree2radian(lastTx->nmeaInfo.lon);
454
455                 hDistance = nmea_distance_ellipsoid(&avgPos, &lastTxPos, NULL, NULL);
456                 hDistanceValid = true;
457         } else {
458                 hDistanceValid = false;
459         }
460
461         /* hdopDistance */
462         if (avgHasHdop || lastTxHasHdop) {
463                 hdopDistanceForOutside = dopMultiplier * (lastTxHdop + avgHdop);
464                 hdopDistanceForInside = dopMultiplier * (lastTxHdop - avgHdop);
465                 hdopDistanceValid = true;
466         } else {
467                 hdopDistanceValid = false;
468         }
469
470         /* vDistance */
471         if (avgHasElv && lastTxHasElv) {
472                 vDistance = fabs(lastTx->nmeaInfo.elv - avg->nmeaInfo.elv);
473                 vDistanceValid = true;
474         } else {
475                 vDistanceValid = false;
476         }
477
478         /* vdopDistance */
479         if (avgHasVdop || lastTxHasVdop) {
480                 vdopDistanceForOutside = dopMultiplier * (lastTxVdop + avgVdop);
481                 vdopDistanceForInside = dopMultiplier * (lastTxVdop - avgVdop);
482                 vdopDistanceValid = true;
483         } else {
484                 vdopDistanceValid = false;
485         }
486
487         /*
488          * Moving Criteria Evaluation Start
489          * We compare the average position against the last transmitted position.
490          */
491
492         /* Speed */
493         if (avgHasSpeed) {
494                 if (avg->nmeaInfo.speed >= getMovingSpeedThreshold()) {
495                         result->speedOverThreshold = SET;
496                 } else {
497                         result->speedOverThreshold = UNSET;
498                 }
499         }
500
501         /*
502          * Position
503          *
504          * avg  last  hDistanceMoving
505          *  0     0   determine via other parameters
506          *  0     1   determine via other parameters
507          *  1     0   MOVING
508          *  1     1   determine via distance threshold and HDOP
509          */
510         if (avgHasPos && !lastTxHasPos) {
511                 result->hDistanceOverThreshold = SET;
512         } else if (hDistanceValid) {
513                 if (hDistance >= getMovingDistanceThreshold()) {
514                         result->hDistanceOverThreshold = SET;
515                 } else {
516                         result->hDistanceOverThreshold = UNSET;
517                 }
518
519                 /*
520                  * Position with HDOP
521                  *
522                  * avg  last  movingNow
523                  *  0     0   determine via other parameters
524                  *  0     1   determine via position with HDOP (avg has default HDOP)
525                  *  1     0   determine via position with HDOP (lastTx has default HDOP)
526                  *  1     1   determine via position with HDOP
527                  */
528                 if (hdopDistanceValid) {
529                         /* we are outside the HDOP when the HDOPs no longer overlap */
530                         if (hDistance > hdopDistanceForOutside) {
531                                 result->outsideHdop = SET;
532                         } else {
533                                 result->outsideHdop = UNSET;
534                         }
535
536                         /* we are inside the HDOP when the HDOPs fully overlap */
537                         if (hDistance <= hdopDistanceForInside) {
538                                 result->insideHdop = SET;
539                         } else {
540                                 result->insideHdop = UNSET;
541                         }
542                 }
543         }
544
545         /*
546          * Elevation
547          *
548          * avg  last  movingNow
549          *  0     0   determine via other parameters
550          *  0     1   determine via other parameters
551          *  1     0   MOVING
552          *  1     1   determine via distance threshold and VDOP
553          */
554         if (avgHasElv && !lastTxHasElv) {
555                 result->vDistanceOverThreshold = SET;
556         } else if (vDistanceValid) {
557                 if (vDistance >= getMovingDistanceThreshold()) {
558                         result->vDistanceOverThreshold = SET;
559                 } else {
560                         result->vDistanceOverThreshold = UNSET;
561                 }
562
563                 /*
564                  * Elevation with VDOP
565                  *
566                  * avg  last  movingNow
567                  *  0     0   determine via other parameters
568                  *  0     1   determine via elevation with VDOP (avg has default VDOP)
569                  *  1     0   determine via elevation with VDOP (lastTx has default VDOP)
570                  *  1     1   determine via elevation with VDOP
571                  */
572                 if (vdopDistanceValid) {
573                         /* we are outside the VDOP when the VDOPs no longer overlap */
574                         if (vDistance > vdopDistanceForOutside) {
575                                 result->outsideVdop = SET;
576                         } else {
577                                 result->outsideVdop = UNSET;
578                         }
579
580                         /* we are inside the VDOP when the VDOPs fully overlap */
581                         if (vDistance <= vdopDistanceForInside) {
582                                 result->insideVdop = SET;
583                         } else {
584                                 result->insideVdop = UNSET;
585                         }
586                 }
587         }
588
589         /*
590          * Moving Criteria Evaluation End
591          */
592
593         /* accumulate inside criteria */
594         if ((result->insideHdop == SET) && (result->insideVdop == SET)) {
595                 result->inside = SET;
596         } else if ((result->insideHdop == UNSET) || (result->insideVdop == UNSET)) {
597                 result->inside = UNSET;
598         }
599
600         /* accumulate outside criteria */
601         if ((result->outsideHdop == SET) || (result->outsideVdop == SET)) {
602                 result->outside = SET;
603         } else if ((result->outsideHdop == UNSET)
604                         || (result->outsideVdop == UNSET)) {
605                 result->outside = UNSET;
606         }
607
608         /* accumulate threshold criteria */
609         if ((result->speedOverThreshold == SET)
610                         || (result->hDistanceOverThreshold == SET)
611                         || (result->vDistanceOverThreshold == SET)) {
612                 result->overThresholds = SET;
613         } else if ((result->speedOverThreshold == UNSET)
614                         || (result->hDistanceOverThreshold == UNSET)
615                         || (result->vDistanceOverThreshold == UNSET)) {
616                 result->overThresholds = UNSET;
617         }
618
619         /* accumulate moving criteria */
620         if ((result->overThresholds == SET) || (result->outside == SET)) {
621                 result->moving = SET;
622         } else if ((result->overThresholds == UNSET)
623                         && (result->outside == UNSET)) {
624                 result->moving = UNSET;
625         }
626
627         return;
628 }
629
630 /**
631  Restart the OLSR tx timer
632  */
633 static void restartOlsrTimer(void) {
634         if (!restartOlsrTxTimer(
635                         (state.externalState == STATIONARY) ? getUpdateIntervalStationary() :
636                                         getUpdateIntervalMoving(), &pud_olsr_tx_timer_callback)) {
637                 pudError(0, "Could not restart OLSR tx timer, no periodic"
638                                 " position updates will be sent to the OLSR network");
639         }
640 }
641
642 /**
643  Restart the uplink tx timer
644  */
645 static void restartUplinkTimer(void) {
646         if (!restartUplinkTxTimer(
647                         (state.externalState == STATIONARY) ? getUplinkUpdateIntervalStationary() :
648                                         getUplinkUpdateIntervalMoving(),
649                         &pud_uplink_timer_callback)) {
650                 pudError(0, "Could not restart uplink timer, no periodic"
651                                 " position updates will be uplinked");
652         }
653 }
654
655 /**
656  Update the latest GPS information. This function is called when a packet is
657  received from a rxNonOlsr interface, containing one or more NMEA strings with
658  GPS information.
659
660  @param rxBuffer
661  the receive buffer with the received NMEA string(s)
662  @param rxCount
663  the number of bytes in the receive buffer
664
665  @return
666  - false on failure
667  - true otherwise
668  */
669 bool receiverUpdateGpsInformation(unsigned char * rxBuffer, size_t rxCount) {
670         static const char * rxBufferPrefix = "$GP";
671         static const size_t rxBufferPrefixLength = 3;
672
673         bool retval = false;
674         PositionUpdateEntry * incomingEntry;
675         MovementState newState = STATIONARY;
676         PositionUpdateEntry * posAvgEntry;
677         MovementType movementResult;
678         TristateBoolean movingNow;
679         bool internalStateChange = false;
680         bool externalStateChange = false;
681         bool updateTransmitGpsInformation = false;
682
683         /* do not process when the message does not start with $GP */
684         if ((rxCount < rxBufferPrefixLength) || (strncmp((char *) rxBuffer,
685                         rxBufferPrefix, rxBufferPrefixLength) != 0)) {
686                 return true;
687         }
688
689         (void) pthread_mutex_lock(&positionAverageList.mutex);
690
691         /* parse all NMEA strings in the rxBuffer into the incoming entry */
692         incomingEntry = getPositionAverageEntry(&positionAverageList, INCOMING);
693         nmea_zero_INFO(&incomingEntry->nmeaInfo);
694         nmea_parse(&nmeaParser, (char *) rxBuffer, rxCount,
695                         &incomingEntry->nmeaInfo);
696
697 #if defined(PUD_DUMP_AVERAGING)
698         dump_nmeaInfo(&incomingEntry->nmeaInfo,
699                         "receiverUpdateGpsInformation: incoming entry");
700 #endif /* PUD_DUMP_AVERAGING */
701
702         /* ignore when no useful information */
703         if (incomingEntry->nmeaInfo.smask == GPNON) {
704                 retval = true;
705                 goto end;
706         }
707
708         nmea_INFO_sanitise(&incomingEntry->nmeaInfo);
709
710 #if defined(PUD_DUMP_AVERAGING)
711         dump_nmeaInfo(&incomingEntry->nmeaInfo,
712                         "receiverUpdateGpsInformation: incoming entry after sanitise");
713 #endif /* PUD_DUMP_AVERAGING */
714
715         /* we always work with latitude, longitude in degrees and DOPs in meters */
716         nmea_INFO_unit_conversion(&incomingEntry->nmeaInfo);
717
718 #if defined(PUD_DUMP_AVERAGING)
719         dump_nmeaInfo(&incomingEntry->nmeaInfo,
720                         "receiverUpdateGpsInformation: incoming entry after unit conversion");
721 #endif /* PUD_DUMP_AVERAGING */
722
723         /*
724          * Averageing
725          */
726
727         if (state.internalState == MOVING) {
728                 /* flush average: keep only the incoming entry */
729                 flushPositionAverageList(&positionAverageList);
730         }
731         addNewPositionToAverage(&positionAverageList, incomingEntry);
732         posAvgEntry = getPositionAverageEntry(&positionAverageList, AVERAGE);
733
734         /*
735          * Movement detection
736          */
737
738         detemineMoving(posAvgEntry, &txPosition, &movementResult);
739         movingNow = movementResult.moving;
740
741 #if defined(PUD_DUMP_AVERAGING)
742         olsr_printf(0, "receiverUpdateGpsInformation: internalState = %s\n",
743                         MovementStateToString(state.internalState));
744         olsr_printf(0, "receiverUpdateGpsInformation: movingNow     = %s\n",
745                         TristateBooleanToString(movingNow));
746 #endif /* PUD_DUMP_AVERAGING */
747
748         /*
749          * Internal State
750          */
751
752         if (movingNow == SET) {
753                 newState = MOVING;
754         } else if (movingNow == UNSET) {
755                 newState = STATIONARY;
756         }
757         internalStateChange = (state.internalState != newState);
758         state.internalState = newState;
759
760         /*
761          * External State (+ hysteresis)
762          */
763
764         if (internalStateChange) {
765                 /* restart hysteresis for external state change when we have an internal
766                  * state change */
767                 state.hysteresisCounter = 0;
768         }
769
770         /* when internal state and external state are not the same we need to
771          * perform hysteresis before we can propagate the internal state to the
772          * external state */
773         newState = state.externalState;
774         if (state.internalState != state.externalState) {
775                 switch (state.internalState) {
776                         case STATIONARY:
777                                 /* external state is MOVING */
778
779                                 /* delay going to stationary a bit */
780                                 state.hysteresisCounter++;
781
782                                 if (state.hysteresisCounter
783                                                 >= getHysteresisCountToStationary()) {
784                                         /* outside the hysteresis range, go to stationary */
785                                         newState = STATIONARY;
786                                 }
787                                 break;
788
789                         case MOVING:
790                                 /* external state is STATIONARY */
791
792                                 /* delay going to moving a bit */
793                                 state.hysteresisCounter++;
794
795                                 if (state.hysteresisCounter >= getHysteresisCountToMoving()) {
796                                         /* outside the hysteresis range, go to moving */
797                                         newState = MOVING;
798                                 }
799                                 break;
800
801                         default:
802                                 /* when unknown do just as if we transition into stationary */
803                                 newState = STATIONARY;
804                                 break;
805                 }
806         }
807         externalStateChange = (state.externalState != newState);
808         state.externalState = newState;
809
810 #if defined(PUD_DUMP_AVERAGING)
811         olsr_printf(0, "receiverUpdateGpsInformation: newState = %s\n",
812                         MovementStateToString(newState));
813         dump_nmeaInfo(&posAvgEntry->nmeaInfo,
814                         "receiverUpdateGpsInformation: posAvgEntry");
815 #endif /* PUD_DUMP_AVERAGING */
816
817         /*
818          * Update transmitGpsInformation
819          */
820
821         updateTransmitGpsInformation = externalStateChange
822                         || (positionValid(posAvgEntry) && !positionValid(&txPosition))
823                         || (movementResult.inside == SET);
824
825         if ((state.externalState == MOVING) || updateTransmitGpsInformation) {
826                 memcpy(&txPosition.nmeaInfo, &posAvgEntry->nmeaInfo, sizeof(nmeaINFO));
827                 (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
828                 memcpy(&transmitGpsInformation.txPosition.nmeaInfo,
829                                 &posAvgEntry->nmeaInfo, sizeof(nmeaINFO));
830                 transmitGpsInformation.updated = true;
831
832 #if defined(PUD_DUMP_AVERAGING)
833                 dump_nmeaInfo(&transmitGpsInformation.txPosition.nmeaInfo,
834                         "receiverUpdateGpsInformation: transmitGpsInformation");
835 #endif /* PUD_DUMP_AVERAGING */
836
837                 (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
838         }
839
840         if (externalStateChange) {
841                 TimedTxInterface interfaces = OLSR; /* always send over olsr */
842                 restartOlsrTimer();
843
844                 if (isUplinkAddrSet()) {
845                         interfaces |= UPLINK;
846                         restartUplinkTimer();
847                 }
848
849                 /* do an immediate transmit */
850                 txToAllOlsrInterfaces(interfaces);
851         }
852
853         retval = true;
854
855         end: (void) pthread_mutex_unlock(&positionAverageList.mutex);
856         return retval;
857 }
858
859 /*
860  * Receiver start/stop
861  */
862
863 /**
864  Start the receiver
865
866  @return
867  - false on failure
868  - true otherwise
869  */
870 bool startReceiver(void) {
871         pthread_mutexattr_t attr;
872         if (pthread_mutexattr_init(&attr)) {
873                 return false;
874         }
875         if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE_NP)) {
876                 return false;
877         }
878         if (pthread_mutex_init(&transmitGpsInformation.mutex, &attr)) {
879                 return false;
880         }
881
882         if (!nmea_parser_init(&nmeaParser)) {
883                 pudError(false, "Could not initialise NMEA parser");
884                 return false;
885         }
886
887         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
888         transmitGpsInformation.updated = false;
889
890         nmea_zero_INFO(&txPosition.nmeaInfo);
891
892         state.internalState = STATIONARY;
893         state.externalState = STATIONARY;
894         state.hysteresisCounter = 0;
895
896         initPositionAverageList(&positionAverageList, getAverageDepth());
897
898         if (!initOlsrTxTimer()) {
899                 stopReceiver();
900                 return false;
901         }
902
903         if (!initUplinkTxTimer()) {
904                 stopReceiver();
905                 return false;
906         }
907
908         restartOlsrTimer();
909         restartUplinkTimer();
910
911         return true;
912 }
913
914 /**
915  Stop the receiver
916  */
917 void stopReceiver(void) {
918         destroyUplinkTxTimer();
919         destroyOlsrTxTimer();
920
921         destroyPositionAverageList(&positionAverageList);
922
923         state.hysteresisCounter = 0;
924         state.externalState = STATIONARY;
925         state.internalState = STATIONARY;
926
927         nmea_zero_INFO(&txPosition.nmeaInfo);
928
929         transmitGpsInformation.updated = false;
930         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
931
932         nmea_parser_destroy(&nmeaParser);
933
934         (void) pthread_mutex_destroy(&transmitGpsInformation.mutex);
935 }