a7b8be20454cd79e81639d709351827fb9cf555f
[olsrd.git] / lib / pud / src / receiver.c
1 /*
2  * The olsr.org Optimized Link-State Routing daemon (olsrd)
3  *
4  * (c) by the OLSR project
5  *
6  * See our Git repository to find out who worked on this file
7  * and thus is a copyright holder on it.
8  *
9  * All rights reserved.
10  *
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions
13  * are met:
14  *
15  * * Redistributions of source code must retain the above copyright
16  *   notice, this list of conditions and the following disclaimer.
17  * * Redistributions in binary form must reproduce the above copyright
18  *   notice, this list of conditions and the following disclaimer in
19  *   the documentation and/or other materials provided with the
20  *   distribution.
21  * * Neither the name of olsr.org, olsrd nor the names of its
22  *   contributors may be used to endorse or promote products derived
23  *   from this software without specific prior written permission.
24  *
25  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  * POSSIBILITY OF SUCH DAMAGE.
37  *
38  * Visit http://www.olsr.org for more information.
39  *
40  * If you find this software useful feel free to make a donation
41  * to the project. For more information see the website or contact
42  * the copyright holders.
43  *
44  */
45
46 #include "receiver.h"
47
48 /* Plugin includes */
49 #include "pud.h"
50 #include "state.h"
51 #include "configuration.h"
52 #include "gpsConversion.h"
53 #include "networkInterfaces.h"
54 #include "timers.h"
55 #include "uplinkGateway.h"
56 #include "posFile.h"
57
58 /* OLSRD includes */
59 #include "net_olsr.h"
60
61 /* System includes */
62 #include <nmealib/context.h>
63 #include <nmealib/nmath.h>
64 #include <nmealib/parser.h>
65 #include <nmealib/sentence.h>
66 #include <OlsrdPudWireFormat/wireFormat.h>
67 #include <unistd.h>
68
69 static void receiverProcessIncomingEntry(PositionUpdateEntry * incomingEntry);
70
71 /*
72  * NMEA parser
73  */
74
75 /** The NMEA string parser */
76 static NmeaParser nmeaParser;
77
78 /*
79  * State
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         TX_INTERFACE_OLSR = 1,
113         TX_INTERFACE_UPLINK = 2
114 } TimedTxInterface;
115
116 /** The latest position information that is transmitted */
117 static TransmitGpsInformation transmitGpsInformation;
118
119 /** The size of the buffer in which the OLSR and uplink messages are assembled */
120 #define TX_BUFFER_SIZE_FOR_OLSR 1024
121
122 /** the prefix of all parameters written to the position output file */
123 #define PUD_POSOUT_FILE_PARAM_PREFIX "OLSRD_PUD_"
124
125 /** the position output file */
126 static char * positionOutputFile = NULL;
127
128 /** true when a postion output file error was already reported */
129 static bool positionOutputFileError = false;
130
131 /** gpsd daemon data */
132 struct gps_data_t gpsdata;
133
134 /** gpsd daemon connection tracking */
135 static struct GpsdConnectionState connectionTracking;
136
137 /*
138  * Functions
139  */
140
141 /**
142  Clear the MovementType
143  * @param result a pointer to the MovementType
144  */
145 static void clearMovementType(MovementType * result) {
146         /* clear outputs */
147         result->moving = TRISTATE_BOOLEAN_UNKNOWN;
148         result->overThresholds = TRISTATE_BOOLEAN_UNKNOWN;
149         result->speedOverThreshold = TRISTATE_BOOLEAN_UNKNOWN;
150         result->hDistanceOverThreshold = TRISTATE_BOOLEAN_UNKNOWN;
151         result->vDistanceOverThreshold = TRISTATE_BOOLEAN_UNKNOWN;
152         result->outside = TRISTATE_BOOLEAN_UNKNOWN;
153         result->outsideHdop = TRISTATE_BOOLEAN_UNKNOWN;
154         result->outsideVdop = TRISTATE_BOOLEAN_UNKNOWN;
155         result->inside = TRISTATE_BOOLEAN_UNKNOWN;
156         result->insideHdop = TRISTATE_BOOLEAN_UNKNOWN;
157         result->insideVdop = TRISTATE_BOOLEAN_UNKNOWN;
158 }
159
160 /**
161  Determine whether s position is valid.
162
163  @param position
164  a pointer to a position
165
166  @return
167  - true when valid
168  - false otherwise
169  */
170 static bool positionValid(PositionUpdateEntry * position) {
171         return (nmeaInfoIsPresentAll(position->nmeaInfo.present, NMEALIB_PRESENT_FIX)
172                         && (position->nmeaInfo.fix != NMEALIB_FIX_BAD));
173 }
174
175 /**
176  Send the transmit buffer out over all designated interfaces, called as a
177  timer callback and also immediately on an external state change.
178
179  @param interfaces
180  a bitmap defining which interfaces to send over
181  */
182 static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
183         /** txBuffer is used to concatenate the position update and cluster leader messages in */
184         unsigned char txBuffer[TX_BUFFER_SIZE_FOR_OLSR];
185         unsigned int txBufferBytesUsed = 0;
186         #define txBufferBytesFree       (sizeof(txBuffer) - txBufferBytesUsed)
187
188         /*
189          * The first message in txBuffer is an OLSR position update.
190          *
191          * The position update is always present.
192          *
193          * The second message is the cluster leader message, but only when uplink
194          * was requested and correctly configured.
195          */
196
197         UplinkMessage * pu_uplink = (UplinkMessage *) &txBuffer[0];
198         union olsr_message * pu = &pu_uplink->msg.olsrMessage;
199         unsigned int pu_size = 0;
200         union olsr_ip_addr gateway;
201         MovementState externalState;
202         NmeaInfo nmeaInfo;
203
204         externalState = getExternalState();
205
206         /* only update the timestamp when the position is invalid AND when the position was not updated */
207         if (!positionValid(&transmitGpsInformation.txPosition) && !transmitGpsInformation.positionUpdated) {
208           nmeaTimeSet(&transmitGpsInformation.txPosition.nmeaInfo.utc, &transmitGpsInformation.txPosition.nmeaInfo.present, NULL);
209         }
210
211         nmeaInfo = transmitGpsInformation.txPosition.nmeaInfo;
212         transmitGpsInformation.positionUpdated = false;
213         gateway = transmitGpsInformation.txGateway;
214
215         /* convert nmeaINFO to wireformat olsr message */
216         txBufferBytesUsed += sizeof(UplinkHeader); /* keep before txBufferSpaceFree usage */
217         pu_size = gpsToOlsr(&nmeaInfo, pu, txBufferBytesFree,
218                         ((externalState == MOVEMENT_STATE_STATIONARY) ? getUpdateIntervalStationary() : getUpdateIntervalMoving()));
219         txBufferBytesUsed += pu_size;
220
221         /*
222          * push out to all OLSR interfaces
223          */
224         if (((interfaces & TX_INTERFACE_OLSR) != 0) && getOlsrTtl() && (pu_size > 0)) {
225                 int r;
226                 struct interface_olsr *ifn;
227                 for (ifn = ifnet; ifn; ifn = ifn->int_next) {
228                         /* force the pending buffer out if there's not enough space for our message */
229                         if ((int)pu_size > net_outbuffer_bytes_left(ifn)) {
230                           net_output(ifn);
231                         }
232                         r = net_outbuffer_push(ifn, pu, pu_size);
233                         if (r != (int) pu_size) {
234                                 pudError(
235                                                 false,
236                                                 "Could not send to OLSR interface %s: %s (size=%u, r=%d)",
237                                                 ifn->int_name,
238                                                 ((r == -1) ? "no buffer was found" :
239                                                         (r == 0) ? "there was not enough room in the buffer" : "unknown reason"), pu_size, r);
240                         }
241                 }
242
243                 /* loopback to tx interface when so configured */
244                 if (getUseLoopback()) {
245                         (void) packetReceivedFromOlsr(pu, NULL, NULL);
246                 }
247         }
248
249         /* push out over uplink when an uplink is configured */
250         if (((interfaces & TX_INTERFACE_UPLINK) != 0) && isUplinkAddrSet()) {
251                 int fd = getDownlinkSocketFd();
252                 if (fd != -1) {
253                         union olsr_sockaddr * uplink_addr = getUplinkAddr();
254                         void * addr;
255                         socklen_t addrSize;
256
257                         UplinkMessage * cl_uplink = (UplinkMessage *) &txBuffer[txBufferBytesUsed];
258                         UplinkClusterLeader * cl = &cl_uplink->msg.clusterLeader;
259                         union olsr_ip_addr * cl_originator = getClusterLeaderOriginator(olsr_cnf->ip_version, cl);
260                         union olsr_ip_addr * cl_clusterLeader = getClusterLeaderClusterLeader(olsr_cnf->ip_version, cl);
261
262                         unsigned int cl_size =
263                                         sizeof(UplinkClusterLeader) - sizeof(cl->leader)
264                                                         + ((olsr_cnf->ip_version == AF_INET) ? sizeof(cl->leader.v4) :
265                                                                         sizeof(cl->leader.v6));
266
267                         unsigned long long uplinkUpdateInterval =
268                                         (externalState == MOVEMENT_STATE_STATIONARY) ? getUplinkUpdateIntervalStationary() : getUplinkUpdateIntervalMoving();
269
270                         if (uplink_addr->in.sa_family == AF_INET) {
271                                 addr = &uplink_addr->in4;
272                                 addrSize = sizeof(struct sockaddr_in);
273                         } else {
274                                 addr = &uplink_addr->in6;
275                                 addrSize = sizeof(struct sockaddr_in6);
276                         }
277
278                         /*
279                          * position update message (pu)
280                          */
281
282                         /* set header fields in position update uplink message and adjust
283                          * the validity time to the uplink validity time */
284                         if (pu_size > 0) {
285                                 PudOlsrPositionUpdate * pu_gpsMessage = getOlsrMessagePayload(olsr_cnf->ip_version, pu);
286
287                                 setUplinkMessageType(&pu_uplink->header, POSITION);
288                                 setUplinkMessageLength(&pu_uplink->header, pu_size);
289                                 setUplinkMessageIPv6(&pu_uplink->header, (olsr_cnf->ip_version != AF_INET));
290                                 setUplinkMessagePadding(&pu_uplink->header, 0);
291
292                                 /* fixup validity time */
293                                 setValidityTime(&pu_gpsMessage->validityTime, uplinkUpdateInterval);
294                         }
295
296                         /*
297                          * cluster leader message (cl)
298                          */
299
300                         /* set cl_uplink header fields */
301                         setUplinkMessageType(&cl_uplink->header, CLUSTERLEADER);
302                         setUplinkMessageLength(&cl_uplink->header, cl_size);
303                         setUplinkMessageIPv6(&cl_uplink->header, (olsr_cnf->ip_version != AF_INET));
304                         setUplinkMessagePadding(&cl_uplink->header, 0);
305
306                         /* setup cl */
307                         setClusterLeaderVersion(cl, PUD_WIRE_FORMAT_VERSION);
308                         setValidityTime(&cl->validityTime, uplinkUpdateInterval);
309
310                         /* really need 2 memcpy's here because of olsr_cnf->ipsize */
311                         memcpy(cl_originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
312                         memcpy(cl_clusterLeader, &gateway, olsr_cnf->ipsize);
313
314                         txBufferBytesUsed += sizeof(UplinkHeader);
315                         txBufferBytesUsed += cl_size;
316
317                         errno = 0;
318                         if (sendto(fd, &txBuffer, txBufferBytesUsed, 0, addr, addrSize) < 0) {
319                                 /* do not report send errors, they're not really relevant */
320                         }
321                 }
322         }
323 }
324
325 /**
326  * Write the position output file (if so configured)
327  *
328  * @return true on success
329  */
330 static bool writePositionOutputFile(void) {
331   FILE * fp = NULL;
332   NmeaInfo * nmeaInfo;
333   const char * signal;
334   const char * fix;
335
336   if (!positionOutputFile) {
337     return true;
338   }
339
340   fp = fopen(positionOutputFile, "w");
341   if (!fp) {
342     /* could not open the file */
343     if (!positionOutputFileError) {
344       pudError(true, "Could not write to the position output file \"%s\"", positionOutputFile);
345       positionOutputFileError = true;
346     }
347     return false;
348   }
349   positionOutputFileError = false;
350
351   nmeaInfo = &transmitGpsInformation.txPosition.nmeaInfo;
352
353   /* node id */
354   fprintf(fp, "%s%s=\"%s\"\n", PUD_POSOUT_FILE_PARAM_PREFIX, "NODE_ID", transmitGpsInformation.nodeId);
355
356   /* sig */
357   switch (nmeaInfo->sig) {
358     case 0:
359       signal = "INVALID";
360       break;
361
362     case 1:
363       signal = "FIX";
364       break;
365
366     case 2:
367       signal = "DIFFERENTIAL";
368       break;
369
370     case 3:
371       signal = "SENSITIVE";
372       break;
373
374     case 4:
375       signal = "REALTIME";
376       break;
377
378     case 5:
379       signal = "FLOAT";
380       break;
381
382     case 6:
383       signal = "ESTIMATED";
384       break;
385
386     case 7:
387       signal = "MANUAL";
388       break;
389
390     case 8:
391       signal = "SIMULATED";
392       break;
393
394     default:
395       signal = NULL;
396       break;
397   }
398   if (!signal) {
399     fprintf(fp, "%s%s=%d\n", PUD_POSOUT_FILE_PARAM_PREFIX, "SIGNAL", nmeaInfo->sig);
400   } else {
401     fprintf(fp, "%s%s=%s\n", PUD_POSOUT_FILE_PARAM_PREFIX, "SIGNAL", signal);
402   }
403
404   /* fix */
405   switch (nmeaInfo->fix) {
406     case 1:
407       fix = "NONE";
408       break;
409
410     case 2:
411       fix = "2D";
412       break;
413
414     case 3:
415       fix = "3D";
416       break;
417
418     default:
419       fix = NULL;
420       break;
421   }
422   if (!fix) {
423     fprintf(fp, "%s%s=%d\n", PUD_POSOUT_FILE_PARAM_PREFIX, "FIX", nmeaInfo->fix);
424   } else {
425     fprintf(fp, "%s%s=%s\n", PUD_POSOUT_FILE_PARAM_PREFIX, "FIX", fix);
426   }
427
428   /* utc */
429   fprintf(fp, "%s%s=%04d\n", PUD_POSOUT_FILE_PARAM_PREFIX, "YEAR", nmeaInfo->utc.year);
430   fprintf(fp, "%s%s=%02d\n", PUD_POSOUT_FILE_PARAM_PREFIX, "MONTH", nmeaInfo->utc.mon);
431   fprintf(fp, "%s%s=%02d\n", PUD_POSOUT_FILE_PARAM_PREFIX, "DAY", nmeaInfo->utc.day);
432   fprintf(fp, "%s%s=%02d\n", PUD_POSOUT_FILE_PARAM_PREFIX, "HOUR", nmeaInfo->utc.hour);
433   fprintf(fp, "%s%s=%02d\n", PUD_POSOUT_FILE_PARAM_PREFIX, "MINUTE", nmeaInfo->utc.min);
434   fprintf(fp, "%s%s=%02d\n", PUD_POSOUT_FILE_PARAM_PREFIX, "SECONDS", nmeaInfo->utc.sec);
435   fprintf(fp, "%s%s=%03d\n", PUD_POSOUT_FILE_PARAM_PREFIX, "MILLISECONDS", nmeaInfo->utc.hsec * 10);
436
437   /* DOPs */
438   fprintf(fp, "%s%s=%f\n", PUD_POSOUT_FILE_PARAM_PREFIX, "PDOP", nmeaInfo->pdop);
439   fprintf(fp, "%s%s=%f\n", PUD_POSOUT_FILE_PARAM_PREFIX, "HDOP", nmeaInfo->hdop);
440   fprintf(fp, "%s%s=%f\n", PUD_POSOUT_FILE_PARAM_PREFIX, "VDOP", nmeaInfo->vdop);
441
442   /* lat, lon, ... */
443   fprintf(fp, "%s%s=%f\n", PUD_POSOUT_FILE_PARAM_PREFIX, "LATTITUDE", nmeaInfo->latitude);
444   fprintf(fp, "%s%s=%f\n", PUD_POSOUT_FILE_PARAM_PREFIX, "LONGITUDE", nmeaInfo->longitude);
445   fprintf(fp, "%s%s=%f\n", PUD_POSOUT_FILE_PARAM_PREFIX, "ELEVATION", nmeaInfo->elevation);
446   fprintf(fp, "%s%s=%f\n", PUD_POSOUT_FILE_PARAM_PREFIX, "SPEED", nmeaInfo->speed);
447   fprintf(fp, "%s%s=%f\n", PUD_POSOUT_FILE_PARAM_PREFIX, "TRACK", nmeaInfo->track);
448   fprintf(fp, "%s%s=%f\n", PUD_POSOUT_FILE_PARAM_PREFIX, "MAGNETIC_TRACK", nmeaInfo->mtrack);
449   fprintf(fp, "%s%s=%f\n", PUD_POSOUT_FILE_PARAM_PREFIX, "MAGNETIC_VARIATION", nmeaInfo->magvar);
450
451   fclose(fp);
452   return true;
453 }
454
455 /*
456  * Timer Callbacks
457  */
458
459 /**
460  The gpsd fetch timer callback
461
462  @param context
463  unused
464  */
465 static void pud_gpsd_fetch_timer_callback(void *context __attribute__ ((unused))) {
466   static char cnt = 0;
467
468   PositionUpdateEntry * incomingEntry;
469   GpsDaemon *gpsd = getGpsd();
470   if (!gpsd) {
471     return;
472   }
473
474   /* fetch info from gpsd daemon into the incoming entry */
475   incomingEntry = getPositionAverageEntry(&positionAverageList, INCOMING);
476   if (!cnt) {
477     nmeaInfoClear(&incomingEntry->nmeaInfo);
478     nmeaTimeSet(&incomingEntry->nmeaInfo.utc, &incomingEntry->nmeaInfo.present, NULL);
479   }
480   readFromGpsd(gpsd, &gpsdata, &connectionTracking, &incomingEntry->nmeaInfo);
481
482   if (cnt >= (TIMER_GPSD_READS_PER_SEC - 1)) {
483     receiverProcessIncomingEntry(incomingEntry);
484     cnt = 0;
485   } else {
486     cnt++;
487   }
488 }
489
490 /**
491  The OLSR tx timer callback
492
493  @param context
494  unused
495  */
496 static void pud_olsr_tx_timer_callback(void *context __attribute__ ((unused))) {
497         txToAllOlsrInterfaces(TX_INTERFACE_OLSR);
498 }
499
500 /**
501  The uplink timer callback
502
503  @param context
504  unused
505  */
506 static void pud_uplink_timer_callback(void *context __attribute__ ((unused))) {
507         txToAllOlsrInterfaces(TX_INTERFACE_UPLINK);
508 }
509
510 /**
511  Restart the gpsd fetch timer
512  */
513 static void restartGpsdTimer(void) {
514   if (!getGpsd()) {
515     return;
516   }
517
518   if (!restartGpsdFetchTimer(&pud_gpsd_fetch_timer_callback)) {
519     pudError(0, "Could not restart gpsd fetch timer, no position information will be available");
520   }
521 }
522
523 /**
524  Restart the OLSR tx timer
525  */
526 static void restartOlsrTimer(MovementState externalState) {
527         if (!restartOlsrTxTimer(
528                         (externalState == MOVEMENT_STATE_STATIONARY) ? getUpdateIntervalStationary() :
529                                         getUpdateIntervalMoving(), &pud_olsr_tx_timer_callback)) {
530                 pudError(0, "Could not restart OLSR tx timer, no periodic"
531                                 " position updates will be sent to the OLSR network");
532         }
533 }
534
535 /**
536  Restart the uplink tx timer
537  */
538 static void restartUplinkTimer(MovementState externalState) {
539         if (!restartUplinkTxTimer(
540                         (externalState == MOVEMENT_STATE_STATIONARY) ? getUplinkUpdateIntervalStationary() :
541                                         getUplinkUpdateIntervalMoving(),
542                         &pud_uplink_timer_callback)) {
543                 pudError(0, "Could not restart uplink timer, no periodic"
544                                 " position updates will be uplinked");
545         }
546 }
547
548 static void doImmediateTransmit(MovementState externalState) {
549         TimedTxInterface interfaces = TX_INTERFACE_OLSR; /* always send over olsr */
550         restartOlsrTimer(externalState);
551
552         if (isUplinkAddrSet()) {
553                 interfaces |= TX_INTERFACE_UPLINK;
554                 restartUplinkTimer(externalState);
555         }
556
557         /* do an immediate transmit */
558         txToAllOlsrInterfaces(interfaces);
559
560         /* write the position output file */
561         writePositionOutputFile();
562 }
563
564 /**
565  The gateway timer callback
566
567  @param context
568  unused
569  */
570 static void pud_gateway_timer_callback(void *context __attribute__ ((unused))) {
571         union olsr_ip_addr bestGateway;
572         bool externalStateChange;
573         MovementState externalState;
574         TristateBoolean movingNow = TRISTATE_BOOLEAN_UNSET;
575
576         getBestUplinkGateway(&bestGateway);
577
578         /*
579          * Movement detection
580          */
581
582         if (!ipequal(&bestGateway, &transmitGpsInformation.txGateway)) {
583                 movingNow = TRISTATE_BOOLEAN_SET;
584         }
585
586         /*
587          * State Determination
588          */
589
590         determineStateWithHysteresis(SUBSTATE_GATEWAY, movingNow, &externalState, &externalStateChange, NULL, false);
591
592         /*
593          * Update transmitGpsInformation
594          */
595
596         if (movingNow == TRISTATE_BOOLEAN_SET) {
597                 transmitGpsInformation.txGateway = bestGateway;
598         }
599
600         if (externalStateChange) {
601                 doImmediateTransmit(externalState);
602         }
603 }
604
605 /**
606  Detemine whether we are moving from the position, by comparing fields from the
607  average position against those of the last transmitted position.
608
609  MUST be called which the position average list locked.
610
611  @param avg
612  the average position
613  @param lastTx
614  the last transmitted position
615  @param result
616  the results of all movement criteria
617  */
618 static void detemineMovingFromPosition(PositionUpdateEntry * avg, PositionUpdateEntry * lastTx, MovementType * result) {
619         /* avg field presence booleans */
620         bool avgHasSpeed;
621         bool avgHasPos;
622         bool avgHasHdop;
623         bool avgHasElv;
624         bool avgHasVdop;
625
626         /* lastTx field presence booleans */
627         bool lastTxHasPos;
628         bool lastTxHasHdop;
629         bool lastTxHasElv;
630         bool lastTxHasVdop;
631
632         /* these have defaults */
633         double dopMultiplier;
634         double avgHdop;
635         double lastTxHdop;
636         double avgVdop;
637         double lastTxVdop;
638
639         /* calculated values and their validity booleans */
640         double hDistance;
641         double vDistance;
642         double hdopDistanceForOutside;
643         double hdopDistanceForInside;
644         double vdopDistanceForOutside;
645         double vdopDistanceForInside;
646         bool hDistanceValid;
647         bool hdopDistanceValid;
648         bool vDistanceValid;
649         bool vdopDistanceValid;
650
651         /*
652          * Validity
653          *
654          * avg  last  movingNow
655          *  0     0   UNKNOWN : can't determine whether we're moving
656          *  0     1   UNKNOWN : can't determine whether we're moving
657          *  1     0   UNKNOWN : can't determine whether we're moving
658          *  1     1   determine via other parameters
659          */
660
661         if (!positionValid(avg)) {
662                 result->moving = TRISTATE_BOOLEAN_UNKNOWN;
663                 return;
664         }
665
666         /* avg is valid here */
667
668         if (!positionValid(lastTx)) {
669                 result->moving = TRISTATE_BOOLEAN_UNKNOWN;
670                 return;
671         }
672
673         /* both avg and lastTx are valid here */
674
675         /* avg field presence booleans */
676         avgHasSpeed = nmeaInfoIsPresentAll(avg->nmeaInfo.present, NMEALIB_PRESENT_SPEED);
677         avgHasPos = nmeaInfoIsPresentAll(avg->nmeaInfo.present, NMEALIB_PRESENT_LAT)
678                         && nmeaInfoIsPresentAll(avg->nmeaInfo.present, NMEALIB_PRESENT_LON);
679         avgHasHdop = nmeaInfoIsPresentAll(avg->nmeaInfo.present, NMEALIB_PRESENT_HDOP);
680         avgHasElv = nmeaInfoIsPresentAll(avg->nmeaInfo.present, NMEALIB_PRESENT_ELV);
681         avgHasVdop = nmeaInfoIsPresentAll(avg->nmeaInfo.present, NMEALIB_PRESENT_VDOP);
682
683         /* lastTx field presence booleans */
684         lastTxHasPos = nmeaInfoIsPresentAll(lastTx->nmeaInfo.present, NMEALIB_PRESENT_LAT)
685                         && nmeaInfoIsPresentAll(lastTx->nmeaInfo.present, NMEALIB_PRESENT_LON);
686         lastTxHasHdop = nmeaInfoIsPresentAll(lastTx->nmeaInfo.present, NMEALIB_PRESENT_HDOP);
687         lastTxHasElv = nmeaInfoIsPresentAll(lastTx->nmeaInfo.present, NMEALIB_PRESENT_ELV);
688         lastTxHasVdop = nmeaInfoIsPresentAll(lastTx->nmeaInfo.present, NMEALIB_PRESENT_VDOP);
689
690         /* fill in some values _or_ defaults */
691         dopMultiplier = getDopMultiplier();
692         avgHdop = avgHasHdop ? avg->nmeaInfo.hdop : getDefaultHdop();
693         lastTxHdop = lastTxHasHdop ? lastTx->nmeaInfo.hdop : getDefaultHdop();
694         avgVdop = avgHasVdop ? avg->nmeaInfo.vdop : getDefaultVdop();
695         lastTxVdop = lastTxHasVdop ? lastTx->nmeaInfo.vdop: getDefaultVdop();
696
697         /*
698          * Calculations
699          */
700
701         /* hDistance */
702         if (avgHasPos && lastTxHasPos) {
703                 NmeaPosition avgPos;
704                 NmeaPosition lastTxPos;
705
706                 avgPos.lat = nmeaMathDegreeToRadian(avg->nmeaInfo.latitude);
707                 avgPos.lon = nmeaMathDegreeToRadian(avg->nmeaInfo.longitude);
708
709                 lastTxPos.lat = nmeaMathDegreeToRadian(lastTx->nmeaInfo.latitude);
710                 lastTxPos.lon = nmeaMathDegreeToRadian(lastTx->nmeaInfo.longitude);
711
712                 hDistance = fabs(nmeaMathDistanceEllipsoid(&avgPos, &lastTxPos, NULL, NULL));
713                 hDistanceValid = true;
714         } else {
715                 hDistanceValid = false;
716         }
717
718         /* hdopDistance */
719         if (avgHasHdop || lastTxHasHdop) {
720                 hdopDistanceForOutside = dopMultiplier * (lastTxHdop + avgHdop);
721                 hdopDistanceForInside = dopMultiplier * (lastTxHdop - avgHdop);
722                 hdopDistanceValid = true;
723         } else {
724                 hdopDistanceValid = false;
725         }
726
727         /* vDistance */
728         if (avgHasElv && lastTxHasElv) {
729                 vDistance = fabs(lastTx->nmeaInfo.elevation - avg->nmeaInfo.elevation);
730                 vDistanceValid = true;
731         } else {
732                 vDistanceValid = false;
733         }
734
735         /* vdopDistance */
736         if (avgHasVdop || lastTxHasVdop) {
737                 vdopDistanceForOutside = dopMultiplier * (lastTxVdop + avgVdop);
738                 vdopDistanceForInside = dopMultiplier * (lastTxVdop - avgVdop);
739                 vdopDistanceValid = true;
740         } else {
741                 vdopDistanceValid = false;
742         }
743
744         /*
745          * Moving Criteria Evaluation Start
746          * We compare the average position against the last transmitted position.
747          */
748
749         /* Speed */
750         if (avgHasSpeed) {
751                 if (avg->nmeaInfo.speed >= getMovingSpeedThreshold()) {
752                         result->speedOverThreshold = TRISTATE_BOOLEAN_SET;
753                 } else {
754                         result->speedOverThreshold = TRISTATE_BOOLEAN_UNSET;
755                 }
756         }
757
758         /*
759          * Position
760          *
761          * avg  last  hDistanceMoving
762          *  0     0   determine via other parameters
763          *  0     1   determine via other parameters
764          *  1     0   MOVING
765          *  1     1   determine via distance threshold and HDOP
766          */
767         if (avgHasPos && !lastTxHasPos) {
768                 result->hDistanceOverThreshold = TRISTATE_BOOLEAN_SET;
769         } else if (hDistanceValid) {
770                 if (hDistance >= getMovingDistanceThreshold()) {
771                         result->hDistanceOverThreshold = TRISTATE_BOOLEAN_SET;
772                 } else {
773                         result->hDistanceOverThreshold = TRISTATE_BOOLEAN_UNSET;
774                 }
775
776                 /*
777                  * Position with HDOP
778                  *
779                  * avg  last  movingNow
780                  *  0     0   determine via other parameters
781                  *  0     1   determine via position with HDOP (avg has default HDOP)
782                  *  1     0   determine via position with HDOP (lastTx has default HDOP)
783                  *  1     1   determine via position with HDOP
784                  */
785                 if (hdopDistanceValid) {
786                         /* we are outside the HDOP when the HDOPs no longer overlap */
787                         if (hDistance > hdopDistanceForOutside) {
788                                 result->outsideHdop = TRISTATE_BOOLEAN_SET;
789                         } else {
790                                 result->outsideHdop = TRISTATE_BOOLEAN_UNSET;
791                         }
792
793                         /* we are inside the HDOP when the HDOPs fully overlap */
794                         if (hDistance <= hdopDistanceForInside) {
795                                 result->insideHdop = TRISTATE_BOOLEAN_SET;
796                         } else {
797                                 result->insideHdop = TRISTATE_BOOLEAN_UNSET;
798                         }
799                 }
800         }
801
802         /*
803          * Elevation
804          *
805          * avg  last  movingNow
806          *  0     0   determine via other parameters
807          *  0     1   determine via other parameters
808          *  1     0   MOVING
809          *  1     1   determine via distance threshold and VDOP
810          */
811         if (avgHasElv && !lastTxHasElv) {
812                 result->vDistanceOverThreshold = TRISTATE_BOOLEAN_SET;
813         } else if (vDistanceValid) {
814                 if (vDistance >= getMovingDistanceThreshold()) {
815                         result->vDistanceOverThreshold = TRISTATE_BOOLEAN_SET;
816                 } else {
817                         result->vDistanceOverThreshold = TRISTATE_BOOLEAN_UNSET;
818                 }
819
820                 /*
821                  * Elevation with VDOP
822                  *
823                  * avg  last  movingNow
824                  *  0     0   determine via other parameters
825                  *  0     1   determine via elevation with VDOP (avg has default VDOP)
826                  *  1     0   determine via elevation with VDOP (lastTx has default VDOP)
827                  *  1     1   determine via elevation with VDOP
828                  */
829                 if (vdopDistanceValid) {
830                         /* we are outside the VDOP when the VDOPs no longer overlap */
831                         if (vDistance > vdopDistanceForOutside) {
832                                 result->outsideVdop = TRISTATE_BOOLEAN_SET;
833                         } else {
834                                 result->outsideVdop = TRISTATE_BOOLEAN_UNSET;
835                         }
836
837                         /* we are inside the VDOP when the VDOPs fully overlap */
838                         if (vDistance <= vdopDistanceForInside) {
839                                 result->insideVdop = TRISTATE_BOOLEAN_SET;
840                         } else {
841                                 result->insideVdop = TRISTATE_BOOLEAN_UNSET;
842                         }
843                 }
844         }
845
846         /*
847          * Moving Criteria Evaluation End
848          */
849
850         /* accumulate inside criteria */
851         if ((result->insideHdop == TRISTATE_BOOLEAN_SET) && (result->insideVdop == TRISTATE_BOOLEAN_SET)) {
852                 result->inside = TRISTATE_BOOLEAN_SET;
853         } else if ((result->insideHdop == TRISTATE_BOOLEAN_UNSET) || (result->insideVdop == TRISTATE_BOOLEAN_UNSET)) {
854                 result->inside = TRISTATE_BOOLEAN_UNSET;
855         }
856
857         /* accumulate outside criteria */
858         if ((result->outsideHdop == TRISTATE_BOOLEAN_SET) || (result->outsideVdop == TRISTATE_BOOLEAN_SET)) {
859                 result->outside = TRISTATE_BOOLEAN_SET;
860         } else if ((result->outsideHdop == TRISTATE_BOOLEAN_UNSET)
861                         || (result->outsideVdop == TRISTATE_BOOLEAN_UNSET)) {
862                 result->outside = TRISTATE_BOOLEAN_UNSET;
863         }
864
865         /* accumulate threshold criteria */
866         if ((result->speedOverThreshold == TRISTATE_BOOLEAN_SET)
867                         || (result->hDistanceOverThreshold == TRISTATE_BOOLEAN_SET)
868                         || (result->vDistanceOverThreshold == TRISTATE_BOOLEAN_SET)) {
869                 result->overThresholds = TRISTATE_BOOLEAN_SET;
870         } else if ((result->speedOverThreshold == TRISTATE_BOOLEAN_UNSET)
871                         || (result->hDistanceOverThreshold == TRISTATE_BOOLEAN_UNSET)
872                         || (result->vDistanceOverThreshold == TRISTATE_BOOLEAN_UNSET)) {
873                 result->overThresholds = TRISTATE_BOOLEAN_UNSET;
874         }
875
876         /* accumulate moving criteria */
877         if ((result->overThresholds == TRISTATE_BOOLEAN_SET) || (result->outside == TRISTATE_BOOLEAN_SET)) {
878                 result->moving = TRISTATE_BOOLEAN_SET;
879         } else if ((result->overThresholds == TRISTATE_BOOLEAN_UNSET)
880                         && (result->outside == TRISTATE_BOOLEAN_UNSET)) {
881                 result->moving = TRISTATE_BOOLEAN_UNSET;
882         }
883
884         return;
885 }
886
887 static void receiverProcessIncomingEntry(PositionUpdateEntry * incomingEntry) {
888         static bool gpnonPrev = false;
889
890         PositionUpdateEntry * posAvgEntry;
891         MovementType movementResult;
892         bool subStateExternalStateChange;
893         bool externalStateChange;
894         bool updateTransmitGpsInformation = false;
895         MovementState externalState;
896         bool gpnon;
897         bool gpnonChanged;
898
899         /* we always work with latitude, longitude in degrees and DOPs in meters */
900         nmeaInfoUnitConversion(&incomingEntry->nmeaInfo, true);
901
902         gpnon = !nmeaInfoIsPresentAll(incomingEntry->nmeaInfo.present, NMEALIB_PRESENT_SMASK) || (incomingEntry->nmeaInfo.smask == NMEALIB_SENTENCE_GPNON);
903         gpnonChanged = gpnon != gpnonPrev;
904         gpnonPrev = gpnon;
905
906         /*
907          * Averaging
908          */
909
910         if ((getInternalState(SUBSTATE_POSITION) == MOVEMENT_STATE_MOVING) || gpnonChanged) {
911                 /* flush average: keep only the incoming entry */
912                 flushPositionAverageList(&positionAverageList);
913         }
914         addNewPositionToAverage(&positionAverageList, incomingEntry);
915         posAvgEntry = getPositionAverageEntry(&positionAverageList, AVERAGE);
916
917         /*
918          * Movement detection
919          */
920
921         clearMovementType(&movementResult);
922
923         detemineMovingFromPosition(posAvgEntry, &transmitGpsInformation.txPosition, &movementResult);
924
925         /*
926          * State Determination
927          */
928
929         determineStateWithHysteresis(SUBSTATE_POSITION, movementResult.moving, &externalState, &externalStateChange,
930                         &subStateExternalStateChange, gpnonChanged);
931
932         /*
933          * Update transmitGpsInformation
934          */
935
936         updateTransmitGpsInformation = subStateExternalStateChange
937                         || (positionValid(posAvgEntry) && !positionValid(&transmitGpsInformation.txPosition))
938                         || (movementResult.inside == TRISTATE_BOOLEAN_SET);
939
940         if ((externalState == MOVEMENT_STATE_MOVING) || updateTransmitGpsInformation) {
941                 transmitGpsInformation.txPosition = *posAvgEntry;
942                 transmitGpsInformation.positionUpdated = true;
943
944                 /*
945                  * When we're stationary:
946                  * - the track is not reliable or even invalid, so we must clear it.
947                  * - to avoid confusion in consumers of the data, we must clear the speed
948                  *   because it is possible to have a very low speed while moving.
949                  */
950                 if (externalState == MOVEMENT_STATE_STATIONARY) {
951                         transmitGpsInformation.txPosition.nmeaInfo.speed = (double)0.0;
952                         transmitGpsInformation.txPosition.nmeaInfo.track = (double)0.0;
953                 }
954         }
955
956         if (externalStateChange) {
957                 doImmediateTransmit(externalState);
958         }
959 }
960
961 /**
962  * Log nmea library errors as plugin errors
963  * @param str
964  * @param sz
965  */
966 static void nmea_errors(const char *s, size_t sz __attribute__((unused))) {
967         pudError(false, "NMEA library error: %s", s);
968 }
969
970 /**
971  * Helper function to read the position file into the transmit position
972  */
973 void updatePositionFromFile(void) {
974   char * positionFile = getPositionFile();
975   if (positionFile) {
976     readPositionFile(positionFile, &transmitGpsInformation.txPosition.nmeaInfo);
977   }
978 }
979
980 /**
981  Start the receiver
982
983  @return
984  - false on failure
985  - true otherwise
986  */
987 bool startReceiver(void) {
988         MovementState externalState;
989
990         if (!nmeaParserInit(&nmeaParser, 0)) {
991                 pudError(false, "Could not initialise NMEA parser");
992                 return false;
993         }
994
995         /* hook up the NMEA library error callback */
996         nmeaContextSetErrorFunction(&nmea_errors);
997
998         nmeaInfoClear(&transmitGpsInformation.txPosition.nmeaInfo);
999         nmeaTimeSet(&transmitGpsInformation.txPosition.nmeaInfo.utc, &transmitGpsInformation.txPosition.nmeaInfo.present, NULL);
1000         updatePositionFromFile();
1001
1002         transmitGpsInformation.txGateway = olsr_cnf->main_addr;
1003         transmitGpsInformation.positionUpdated = false;
1004         transmitGpsInformation.nodeId = getNodeId(NULL);
1005
1006         olsr_cnf->pud_position = &transmitGpsInformation;
1007
1008         initPositionAverageList(&positionAverageList, getAverageDepth());
1009
1010         memset(&gpsdata, 0, sizeof(gpsdata));
1011         gpsdata.gps_fd = -1;
1012         memset(&connectionTracking, 0, sizeof(connectionTracking));
1013
1014         if (!initGpsdFetchTimer()) {
1015           stopReceiver();
1016           return false;
1017         }
1018
1019         if (!initOlsrTxTimer()) {
1020                 stopReceiver();
1021                 return false;
1022         }
1023
1024         if (!initUplinkTxTimer()) {
1025                 stopReceiver();
1026                 return false;
1027         }
1028
1029         if (!initGatewayTimer()) {
1030                 stopReceiver();
1031                 return false;
1032         }
1033
1034         positionOutputFile = getPositionOutputFile();
1035         positionOutputFileError = false;
1036
1037         /* write the position output file */
1038         if (!writePositionOutputFile()) {
1039           return false;
1040         }
1041
1042         externalState = getExternalState();
1043         restartGpsdTimer();
1044         restartOlsrTimer(externalState);
1045         restartUplinkTimer(externalState);
1046         if (!restartGatewayTimer(getGatewayDeterminationInterval(), &pud_gateway_timer_callback)) {
1047                 pudError(0, "Could not start gateway timer");
1048                 stopReceiver();
1049                 return false;
1050         }
1051
1052         return true;
1053 }
1054
1055 /**
1056  Stop the receiver
1057  */
1058 void stopReceiver(void) {
1059         if (positionOutputFile) {
1060           unlink(positionOutputFile);
1061         }
1062
1063         destroyGatewayTimer();
1064         destroyUplinkTxTimer();
1065         destroyOlsrTxTimer();
1066         destroyGpsdFetchTimer();
1067
1068         destroyPositionAverageList(&positionAverageList);
1069
1070         gpsdDisconnect(&gpsdata, &connectionTracking);
1071         memset(&connectionTracking, 0, sizeof(connectionTracking));
1072         memset(&gpsdata, 0, sizeof(gpsdata));
1073         gpsdata.gps_fd = -1;
1074
1075         nmeaInfoClear(&transmitGpsInformation.txPosition.nmeaInfo);
1076         transmitGpsInformation.txGateway = olsr_cnf->main_addr;
1077         transmitGpsInformation.positionUpdated = false;
1078
1079         nmeaParserDestroy(&nmeaParser);
1080 }