lib/pud/src/receiver.c

   1 #include "receiver.h"
   2
   3 /* Plugin includes */
   4 #include "pud.h"
   5 #include "state.h"
   6 #include "posAvg.h"
   7 #include "configuration.h"
   8 #include "gpsConversion.h"
   9 #include "networkInterfaces.h"
  10 #include "timers.h"
  11 #include "uplinkGateway.h"
  12 #include "dump.h"
  13
  14 /* OLSRD includes */
  15 #include "olsr_types.h"
  16 #include "net_olsr.h"
  17
  18 /* System includes */
  19 #include <pthread.h>
  20 #include <nmea/parser.h>
  21 #include <nmea/gmath.h>
  22 #include <nmea/sentence.h>
  23 #include <OlsrdPudWireFormat/wireFormat.h>
  24
  25 /* Debug includes */
  26 #if defined(PUD_DUMP_GPS_PACKETS_TX_OLSR) || \
  27         defined(PUD_DUMP_GPS_PACKETS_TX_UPLINK) || \
  28         defined(PUD_DUMP_AVERAGING)
  29 #include "olsr.h"
  30 #endif
  31
  32 /*
  33  * NMEA parser
  34  */
  35
  36 /** The NMEA string parser */
  37 static nmeaPARSER nmeaParser;
  38
  39 /*
  40  * State
  41  */
  42
  43 /** Type describing movement calculations */
  44 typedef struct _MovementType {
  45         TristateBoolean moving; /**< SET: we are moving */
  46
  47         TristateBoolean overThresholds; /**< SET: at least 1 threshold state is set */
  48         TristateBoolean speedOverThreshold; /**< SET: speed is over threshold */
  49         TristateBoolean hDistanceOverThreshold; /**< SET: horizontal distance is outside threshold */
  50         TristateBoolean vDistanceOverThreshold; /**< SET: vertical distance is outside threshold */
  51
  52         TristateBoolean outside; /**< SET: at least 1 outside state is SET */
  53         TristateBoolean outsideHdop; /**< SET: avg is outside lastTx HDOP */
  54         TristateBoolean outsideVdop; /**< SET: avg is outside lastTx VDOP */
  55
  56         TristateBoolean inside; /**< SET: all inside states are SET */
  57         TristateBoolean insideHdop; /**< SET: avg is inside lastTx HDOP */
  58         TristateBoolean insideVdop; /**< SET: avg is inside lastTx VDOP */
  59 } MovementType;
  60
  61 /*
  62  * Averaging
  63  */
  64
  65 /** The average position with its administration */
  66 static PositionAverageList positionAverageList;
  67
  68 /*
  69  * TX to OLSR
  70  */
  71
  72 typedef enum _TimedTxInterface {
  73         TX_INTERFACE_OLSR = 1,
  74         TX_INTERFACE_UPLINK = 2
  75 } TimedTxInterface;
  76
  77 /** Structure of the latest GPS information that is transmitted */
  78 typedef struct _TransmitGpsInformation {
  79         pthread_mutex_t mutex; /**< access mutex */
  80         bool updated; /**< true when the information was updated */
  81         PositionUpdateEntry txPosition; /**< The last transmitted position */
  82         union olsr_ip_addr txGateway; /**< the best gateway */
  83 } TransmitGpsInformation;
  84
  85 /** The latest position information that is transmitted */
  86 static TransmitGpsInformation transmitGpsInformation;
  87
  88 /** The size of the buffer in which the OLSR and uplink messages are assembled */
  89 #define TX_BUFFER_SIZE_FOR_OLSR 1024
  90
  91 /*
  92  * Functions
  93  */
  94
  95 /**
  96  Clear the MovementType
  97  * @param result a pointer to the MovementType
  98  */
  99 static void clearMovementType(MovementType * result) {
 100         /* clear outputs */
 101         result->moving = TRISTATE_BOOLEAN_UNKNOWN;
 102         result->overThresholds = TRISTATE_BOOLEAN_UNKNOWN;
 103         result->speedOverThreshold = TRISTATE_BOOLEAN_UNKNOWN;
 104         result->hDistanceOverThreshold = TRISTATE_BOOLEAN_UNKNOWN;
 105         result->vDistanceOverThreshold = TRISTATE_BOOLEAN_UNKNOWN;
 106         result->outside = TRISTATE_BOOLEAN_UNKNOWN;
 107         result->outsideHdop = TRISTATE_BOOLEAN_UNKNOWN;
 108         result->outsideVdop = TRISTATE_BOOLEAN_UNKNOWN;
 109         result->inside = TRISTATE_BOOLEAN_UNKNOWN;
 110         result->insideHdop = TRISTATE_BOOLEAN_UNKNOWN;
 111         result->insideVdop = TRISTATE_BOOLEAN_UNKNOWN;
 112 }
 113
 114 /**
 115  Determine whether s position is valid.
 116
 117  @param position
 118  a pointer to a position
 119
 120  @return
 121  - true when valid
 122  - false otherwise
 123  */
 124 static bool positionValid(PositionUpdateEntry * position) {
 125         return (nmea_INFO_has_field(position->nmeaInfo.smask, FIX)
 126                         && (position->nmeaInfo.fix != NMEA_FIX_BAD));
 127 }
 128
 129 /**
 130  Send the transmit buffer out over all designated interfaces, called as a
 131  timer callback and also immediately on an external state change.
 132
 133  @param interfaces
 134  a bitmap defining which interfaces to send over
 135  */
 136 static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
 137         /** txBuffer is used to concatenate the position update and cluster leader messages in */
 138         unsigned char txBuffer[TX_BUFFER_SIZE_FOR_OLSR];
 139         unsigned int txBufferBytesUsed = 0;
 140         #define txBufferBytesFree       (sizeof(txBuffer) - txBufferBytesUsed)
 141
 142         /*
 143          * The first message in txBuffer is an OLSR position update.
 144          *
 145          * The position update is not present when the position is not valid.
 146          * Otherwise it is always present: when we transmit onto the OLSR network
 147          * and/or when we transmit onto the uplink.
 148          *
 149          * The second message is the cluster leader message, but only when uplink
 150          * was requested and correctly configured.
 151          */
 152
 153         UplinkMessage * pu_uplink = (UplinkMessage *) &txBuffer[0];
 154         union olsr_message * pu = &pu_uplink->msg.olsrMessage;
 155         unsigned int pu_size = 0;
 156         union olsr_ip_addr gateway;
 157         unsigned long long updateInterval;
 158
 159         updateInterval =
 160                         (getExternalState() == MOVEMENT_STATE_MOVING) ? getUpdateIntervalMoving() : getUpdateIntervalStationary();
 161
 162         (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
 163
 164         /* only fixup timestamp when the position is valid _and_ when the position was not updated */
 165         if (positionValid(&transmitGpsInformation.txPosition) && !transmitGpsInformation.updated) {
 166                 nmea_time_now(&transmitGpsInformation.txPosition.nmeaInfo.utc);
 167         }
 168
 169         /* convert nmeaINFO to wireformat olsr message */
 170         txBufferBytesUsed += sizeof(UplinkHeader); /* keep before txBufferSpaceFree usage */
 171         pu_size = gpsToOlsr(&transmitGpsInformation.txPosition.nmeaInfo, pu, txBufferBytesFree, updateInterval);
 172         txBufferBytesUsed += pu_size;
 173         gateway = transmitGpsInformation.txGateway;
 174
 175         transmitGpsInformation.updated = false;
 176         (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
 177
 178         /*
 179          * push out to all OLSR interfaces
 180          */
 181         if (((interfaces & TX_INTERFACE_OLSR) != 0) && (pu_size > 0)) {
 182                 int r;
 183                 struct interface *ifn;
 184                 for (ifn = ifnet; ifn; ifn = ifn->int_next) {
 185                         r = net_outbuffer_push(ifn, pu, pu_size);
 186                         if (r != (int) pu_size) {
 187                                 pudError(
 188                                                 false,
 189                                                 "Could not send to OLSR interface %s: %s (size=%u, r=%d)",
 190                                                 ifn->int_name,
 191                                                 ((r == -1) ? "no buffer was found" :
 192                                                         (r == 0) ? "there was not enough room in the buffer" : "unknown reason"), pu_size, r);
 193                         }
 194 #ifdef PUD_DUMP_GPS_PACKETS_TX_OLSR
 195                         else {
 196                                 olsr_printf(0, "%s: packet sent to OLSR interface %s (%d bytes)\n",
 197                                                 PUD_PLUGIN_ABBR, ifn->int_name, pu_size);
 198                                 dump_packet((unsigned char *)pu, pu_size);
 199                         }
 200 #endif
 201                 }
 202
 203                 /* loopback to tx interface when so configured */
 204                 if (getUseLoopback()) {
 205                         (void) packetReceivedFromOlsr(pu, NULL, NULL);
 206                 }
 207         }
 208
 209         /* push out over uplink when an uplink is configured */
 210         if (((interfaces & TX_INTERFACE_UPLINK) != 0) && isUplinkAddrSet()) {
 211                 int fd = getDownlinkSocketFd();
 212                 if (fd != -1) {
 213                         union olsr_sockaddr * uplink_addr = getUplinkAddr();
 214
 215                         UplinkMessage * cl_uplink = (UplinkMessage *) &txBuffer[txBufferBytesUsed];
 216                         UplinkClusterLeader * cl = &cl_uplink->msg.clusterLeader;
 217                         union olsr_ip_addr * cl_originator = getClusterLeaderOriginator(olsr_cnf->ip_version, cl);
 218                         union olsr_ip_addr * cl_clusterLeader = getClusterLeaderClusterLeader(olsr_cnf->ip_version, cl);
 219                         unsigned int cl_size =
 220                                         sizeof(UplinkClusterLeader) - sizeof(cl->leader)
 221                                                         + ((olsr_cnf->ip_version == AF_INET) ? sizeof(cl->leader.v4) :
 222                                                                         sizeof(cl->leader.v6));
 223
 224                         /*
 225                          * position update message (pu)
 226                          */
 227
 228                         /* set header fields in position update uplink message and adjust
 229                          * the validity time to the uplink validity time */
 230                         if (pu_size > 0) {
 231                                 PudOlsrPositionUpdate * pu_gpsMessage = getOlsrMessagePayload(olsr_cnf->ip_version, pu);
 232
 233                                 setUplinkMessageType(&pu_uplink->header, POSITION);
 234                                 setUplinkMessageLength(&pu_uplink->header, pu_size);
 235                                 setUplinkMessageIPv6(&pu_uplink->header, (olsr_cnf->ip_version != AF_INET));
 236                                 setUplinkMessagePadding(&pu_uplink->header, 0);
 237
 238                                 /* fixup validity time */
 239                                 setValidityTime(&pu_gpsMessage->validityTime, updateInterval);
 240                         }
 241
 242                         /*
 243                          * cluster leader message (cl)
 244                          */
 245
 246                         /* set cl_uplink header fields */
 247                         setUplinkMessageType(&cl_uplink->header, CLUSTERLEADER);
 248                         setUplinkMessageLength(&cl_uplink->header, cl_size);
 249                         setUplinkMessageIPv6(&cl_uplink->header, (olsr_cnf->ip_version != AF_INET));
 250                         setUplinkMessagePadding(&cl_uplink->header, 0);
 251
 252                         /* setup cl */
 253                         setClusterLeaderVersion(cl, PUD_WIRE_FORMAT_VERSION);
 254                         setValidityTime(&cl->validityTime, updateInterval);
 255
 256                         /* really need 2 memcpy's here because of olsr_cnf->ipsize */
 257                         memcpy(cl_originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
 258                         memcpy(cl_clusterLeader, &gateway, olsr_cnf->ipsize);
 259
 260                         txBufferBytesUsed += sizeof(UplinkHeader);
 261                         txBufferBytesUsed += cl_size;
 262
 263                         errno = 0;
 264                         if (sendto(fd, &txBuffer, txBufferBytesUsed, 0, (struct sockaddr *) &uplink_addr->in,
 265                                         sizeof(uplink_addr->in)) < 0) {
 266                                 pudError(true, "Could not send to uplink (size=%u)", txBufferBytesUsed);
 267                         }
 268 #ifdef PUD_DUMP_GPS_PACKETS_TX_UPLINK
 269                         else {
 270                                 olsr_printf(0, "%s: packet sent to uplink (%d bytes)\n",
 271                                                 PUD_PLUGIN_ABBR, pu_size);
 272                                 dump_packet((unsigned char *)&txBuffer, txBufferBytesUsed);
 273                         }
 274 #endif
 275                 }
 276         }
 277 }
 278
 279 /*
 280  * Timer Callbacks
 281  */
 282
 283 /**
 284  The OLSR tx timer callback
 285
 286  @param context
 287  unused
 288  */
 289 static void pud_olsr_tx_timer_callback(void *context __attribute__ ((unused))) {
 290         txToAllOlsrInterfaces(TX_INTERFACE_OLSR);
 291 }
 292
 293 /**
 294  The uplink timer callback
 295
 296  @param context
 297  unused
 298  */
 299 static void pud_uplink_timer_callback(void *context __attribute__ ((unused))) {
 300         txToAllOlsrInterfaces(TX_INTERFACE_UPLINK);
 301 }
 302
 303 /**
 304  Restart the OLSR tx timer
 305  */
 306 static void restartOlsrTimer(MovementState externalState) {
 307         if (!restartOlsrTxTimer(
 308                         (externalState == MOVEMENT_STATE_STATIONARY) ? getUpdateIntervalStationary() :
 309                                         getUpdateIntervalMoving(), &pud_olsr_tx_timer_callback)) {
 310                 pudError(0, "Could not restart OLSR tx timer, no periodic"
 311                                 " position updates will be sent to the OLSR network");
 312         }
 313 }
 314
 315 /**
 316  Restart the uplink tx timer
 317  */
 318 static void restartUplinkTimer(MovementState externalState) {
 319         if (!restartUplinkTxTimer(
 320                         (externalState == MOVEMENT_STATE_STATIONARY) ? getUplinkUpdateIntervalStationary() :
 321                                         getUplinkUpdateIntervalMoving(),
 322                         &pud_uplink_timer_callback)) {
 323                 pudError(0, "Could not restart uplink timer, no periodic"
 324                                 " position updates will be uplinked");
 325         }
 326 }
 327
 328 static void doImmediateTransmit(MovementState externalState, bool externalStateChange) {
 329         if (externalStateChange) {
 330                 TimedTxInterface interfaces = TX_INTERFACE_OLSR; /* always send over olsr */
 331                 restartOlsrTimer(externalState);
 332
 333                 if (isUplinkAddrSet()) {
 334                         interfaces |= TX_INTERFACE_UPLINK;
 335                         restartUplinkTimer(externalState);
 336                 }
 337
 338                 /* do an immediate transmit */
 339                 txToAllOlsrInterfaces(interfaces);
 340         }
 341 }
 342
 343 /**
 344  The gateway timer callback
 345
 346  @param context
 347  unused
 348  */
 349 static void pud_gateway_timer_callback(void *context __attribute__ ((unused))) {
 350         union olsr_ip_addr bestGateway;
 351         bool subStateExternalStateChange;
 352         bool externalStateChange;
 353         MovementState externalState;
 354         TristateBoolean movingNow = TRISTATE_BOOLEAN_UNSET;
 355
 356         getBestUplinkGateway(&bestGateway);
 357
 358         (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
 359
 360         /*
 361          * Movement detection
 362          */
 363
 364         if (!ipequal(&bestGateway, &transmitGpsInformation.txGateway)) {
 365                 movingNow = TRISTATE_BOOLEAN_SET;
 366         }
 367
 368         /*
 369          * State Determination
 370          */
 371
 372         determineStateWithHysteresis(SUBSTATE_GATEWAY, movingNow, &externalState, &externalStateChange,
 373                         &subStateExternalStateChange);
 374
 375         /*
 376          * Update transmitGpsInformation
 377          */
 378
 379         if ((externalState == MOVEMENT_STATE_MOVING) || subStateExternalStateChange) {
 380                 transmitGpsInformation.txGateway = bestGateway;
 381         }
 382
 383         (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
 384
 385         doImmediateTransmit(externalState, externalStateChange);
 386 }
 387
 388 /**
 389  Detemine whether we are moving from the position, by comparing fields from the
 390  average position against those of the last transmitted position.
 391
 392  MUST be called which the position average list locked.
 393
 394  @param avg
 395  the average position
 396  @param lastTx
 397  the last transmitted position
 398  @param result
 399  the results of all movement criteria
 400  */
 401 static void detemineMovingFromPosition(PositionUpdateEntry * avg, PositionUpdateEntry * lastTx, MovementType * result) {
 402         /* avg field presence booleans */
 403         bool avgHasSpeed;
 404         bool avgHasPos;
 405         bool avgHasHdop;
 406         bool avgHasElv;
 407         bool avgHasVdop;
 408
 409         /* lastTx field presence booleans */bool lastTxHasPos;
 410         bool lastTxHasHdop;
 411         bool lastTxHasElv;
 412         bool lastTxHasVdop;
 413
 414         /* these have defaults */
 415         double dopMultiplier;
 416         double avgHdop;
 417         double lastTxHdop;
 418         double avgVdop;
 419         double lastTxVdop;
 420
 421         /* calculated values and their validity booleans */
 422         double hDistance;
 423         double vDistance;
 424         double hdopDistanceForOutside;
 425         double hdopDistanceForInside;
 426         double vdopDistanceForOutside;
 427         double vdopDistanceForInside;
 428         bool hDistanceValid;
 429         bool hdopDistanceValid;
 430         bool vDistanceValid;
 431         bool vdopDistanceValid;
 432
 433         /*
 434          * Validity
 435          *
 436          * avg  last  movingNow
 437          *  0     0   UNKNOWN : can't determine whether we're moving
 438          *  0     1   UNKNOWN : can't determine whether we're moving
 439          *  1     0   UNKNOWN : can't determine whether we're moving
 440          *  1     1   determine via other parameters
 441          */
 442
 443         if (!positionValid(avg)) {
 444                 result->moving = TRISTATE_BOOLEAN_UNKNOWN;
 445                 return;
 446         }
 447
 448         /* avg is valid here */
 449
 450         if (!positionValid(lastTx)) {
 451                 result->moving = TRISTATE_BOOLEAN_UNKNOWN;
 452                 return;
 453         }
 454
 455         /* both avg and lastTx are valid here */
 456
 457         /* avg field presence booleans */
 458         avgHasSpeed = nmea_INFO_has_field(avg->nmeaInfo.smask, SPEED);
 459         avgHasPos = nmea_INFO_has_field(avg->nmeaInfo.smask, LAT)
 460                         && nmea_INFO_has_field(avg->nmeaInfo.smask, LON);
 461         avgHasHdop = nmea_INFO_has_field(avg->nmeaInfo.smask, HDOP);
 462         avgHasElv = nmea_INFO_has_field(avg->nmeaInfo.smask, ELV);
 463         avgHasVdop = nmea_INFO_has_field(avg->nmeaInfo.smask, VDOP);
 464
 465         /* lastTx field presence booleans */
 466         lastTxHasPos = nmea_INFO_has_field(lastTx->nmeaInfo.smask, LAT)
 467                         && nmea_INFO_has_field(lastTx->nmeaInfo.smask, LON);
 468         lastTxHasHdop = nmea_INFO_has_field(lastTx->nmeaInfo.smask, HDOP);
 469         lastTxHasElv = nmea_INFO_has_field(lastTx->nmeaInfo.smask, ELV);
 470         lastTxHasVdop = nmea_INFO_has_field(lastTx->nmeaInfo.smask, VDOP);
 471
 472         /* fill in some values _or_ defaults */
 473         dopMultiplier = getDopMultiplier();
 474         avgHdop = avgHasHdop ? avg->nmeaInfo.HDOP : getDefaultHdop();
 475         lastTxHdop = lastTxHasHdop ? lastTx->nmeaInfo.HDOP : getDefaultHdop();
 476         avgVdop = avgHasVdop ? avg->nmeaInfo.VDOP : getDefaultVdop();
 477         lastTxVdop = lastTxHasVdop ? lastTx->nmeaInfo.VDOP : getDefaultVdop();
 478
 479         /*
 480          * Calculations
 481          */
 482
 483         /* hDistance */
 484         if (avgHasPos && lastTxHasPos) {
 485                 nmeaPOS avgPos;
 486                 nmeaPOS lastTxPos;
 487
 488                 avgPos.lat = nmea_degree2radian(avg->nmeaInfo.lat);
 489                 avgPos.lon = nmea_degree2radian(avg->nmeaInfo.lon);
 490
 491                 lastTxPos.lat = nmea_degree2radian(lastTx->nmeaInfo.lat);
 492                 lastTxPos.lon = nmea_degree2radian(lastTx->nmeaInfo.lon);
 493
 494                 hDistance = nmea_distance_ellipsoid(&avgPos, &lastTxPos, NULL, NULL);
 495                 hDistanceValid = true;
 496         } else {
 497                 hDistanceValid = false;
 498         }
 499
 500         /* hdopDistance */
 501         if (avgHasHdop || lastTxHasHdop) {
 502                 hdopDistanceForOutside = dopMultiplier * (lastTxHdop + avgHdop);
 503                 hdopDistanceForInside = dopMultiplier * (lastTxHdop - avgHdop);
 504                 hdopDistanceValid = true;
 505         } else {
 506                 hdopDistanceValid = false;
 507         }
 508
 509         /* vDistance */
 510         if (avgHasElv && lastTxHasElv) {
 511                 vDistance = fabs(lastTx->nmeaInfo.elv - avg->nmeaInfo.elv);
 512                 vDistanceValid = true;
 513         } else {
 514                 vDistanceValid = false;
 515         }
 516
 517         /* vdopDistance */
 518         if (avgHasVdop || lastTxHasVdop) {
 519                 vdopDistanceForOutside = dopMultiplier * (lastTxVdop + avgVdop);
 520                 vdopDistanceForInside = dopMultiplier * (lastTxVdop - avgVdop);
 521                 vdopDistanceValid = true;
 522         } else {
 523                 vdopDistanceValid = false;
 524         }
 525
 526         /*
 527          * Moving Criteria Evaluation Start
 528          * We compare the average position against the last transmitted position.
 529          */
 530
 531         /* Speed */
 532         if (avgHasSpeed) {
 533                 if (avg->nmeaInfo.speed >= getMovingSpeedThreshold()) {
 534                         result->speedOverThreshold = TRISTATE_BOOLEAN_SET;
 535                 } else {
 536                         result->speedOverThreshold = TRISTATE_BOOLEAN_UNSET;
 537                 }
 538         }
 539
 540         /*
 541          * Position
 542          *
 543          * avg  last  hDistanceMoving
 544          *  0     0   determine via other parameters
 545          *  0     1   determine via other parameters
 546          *  1     0   MOVING
 547          *  1     1   determine via distance threshold and HDOP
 548          */
 549         if (avgHasPos && !lastTxHasPos) {
 550                 result->hDistanceOverThreshold = TRISTATE_BOOLEAN_SET;
 551         } else if (hDistanceValid) {
 552                 if (hDistance >= getMovingDistanceThreshold()) {
 553                         result->hDistanceOverThreshold = TRISTATE_BOOLEAN_SET;
 554                 } else {
 555                         result->hDistanceOverThreshold = TRISTATE_BOOLEAN_UNSET;
 556                 }
 557
 558                 /*
 559                  * Position with HDOP
 560                  *
 561                  * avg  last  movingNow
 562                  *  0     0   determine via other parameters
 563                  *  0     1   determine via position with HDOP (avg has default HDOP)
 564                  *  1     0   determine via position with HDOP (lastTx has default HDOP)
 565                  *  1     1   determine via position with HDOP
 566                  */
 567                 if (hdopDistanceValid) {
 568                         /* we are outside the HDOP when the HDOPs no longer overlap */
 569                         if (hDistance > hdopDistanceForOutside) {
 570                                 result->outsideHdop = TRISTATE_BOOLEAN_SET;
 571                         } else {
 572                                 result->outsideHdop = TRISTATE_BOOLEAN_UNSET;
 573                         }
 574
 575                         /* we are inside the HDOP when the HDOPs fully overlap */
 576                         if (hDistance <= hdopDistanceForInside) {
 577                                 result->insideHdop = TRISTATE_BOOLEAN_SET;
 578                         } else {
 579                                 result->insideHdop = TRISTATE_BOOLEAN_UNSET;
 580                         }
 581                 }
 582         }
 583
 584         /*
 585          * Elevation
 586          *
 587          * avg  last  movingNow
 588          *  0     0   determine via other parameters
 589          *  0     1   determine via other parameters
 590          *  1     0   MOVING
 591          *  1     1   determine via distance threshold and VDOP
 592          */
 593         if (avgHasElv && !lastTxHasElv) {
 594                 result->vDistanceOverThreshold = TRISTATE_BOOLEAN_SET;
 595         } else if (vDistanceValid) {
 596                 if (vDistance >= getMovingDistanceThreshold()) {
 597                         result->vDistanceOverThreshold = TRISTATE_BOOLEAN_SET;
 598                 } else {
 599                         result->vDistanceOverThreshold = TRISTATE_BOOLEAN_UNSET;
 600                 }
 601
 602                 /*
 603                  * Elevation with VDOP
 604                  *
 605                  * avg  last  movingNow
 606                  *  0     0   determine via other parameters
 607                  *  0     1   determine via elevation with VDOP (avg has default VDOP)
 608                  *  1     0   determine via elevation with VDOP (lastTx has default VDOP)
 609                  *  1     1   determine via elevation with VDOP
 610                  */
 611                 if (vdopDistanceValid) {
 612                         /* we are outside the VDOP when the VDOPs no longer overlap */
 613                         if (vDistance > vdopDistanceForOutside) {
 614                                 result->outsideVdop = TRISTATE_BOOLEAN_SET;
 615                         } else {
 616                                 result->outsideVdop = TRISTATE_BOOLEAN_UNSET;
 617                         }
 618
 619                         /* we are inside the VDOP when the VDOPs fully overlap */
 620                         if (vDistance <= vdopDistanceForInside) {
 621                                 result->insideVdop = TRISTATE_BOOLEAN_SET;
 622                         } else {
 623                                 result->insideVdop = TRISTATE_BOOLEAN_UNSET;
 624                         }
 625                 }
 626         }
 627
 628         /*
 629          * Moving Criteria Evaluation End
 630          */
 631
 632         /* accumulate inside criteria */
 633         if ((result->insideHdop == TRISTATE_BOOLEAN_SET) && (result->insideVdop == TRISTATE_BOOLEAN_SET)) {
 634                 result->inside = TRISTATE_BOOLEAN_SET;
 635         } else if ((result->insideHdop == TRISTATE_BOOLEAN_UNSET) || (result->insideVdop == TRISTATE_BOOLEAN_UNSET)) {
 636                 result->inside = TRISTATE_BOOLEAN_UNSET;
 637         }
 638
 639         /* accumulate outside criteria */
 640         if ((result->outsideHdop == TRISTATE_BOOLEAN_SET) || (result->outsideVdop == TRISTATE_BOOLEAN_SET)) {
 641                 result->outside = TRISTATE_BOOLEAN_SET;
 642         } else if ((result->outsideHdop == TRISTATE_BOOLEAN_UNSET)
 643                         || (result->outsideVdop == TRISTATE_BOOLEAN_UNSET)) {
 644                 result->outside = TRISTATE_BOOLEAN_UNSET;
 645         }
 646
 647         /* accumulate threshold criteria */
 648         if ((result->speedOverThreshold == TRISTATE_BOOLEAN_SET)
 649                         || (result->hDistanceOverThreshold == TRISTATE_BOOLEAN_SET)
 650                         || (result->vDistanceOverThreshold == TRISTATE_BOOLEAN_SET)) {
 651                 result->overThresholds = TRISTATE_BOOLEAN_SET;
 652         } else if ((result->speedOverThreshold == TRISTATE_BOOLEAN_UNSET)
 653                         || (result->hDistanceOverThreshold == TRISTATE_BOOLEAN_UNSET)
 654                         || (result->vDistanceOverThreshold == TRISTATE_BOOLEAN_UNSET)) {
 655                 result->overThresholds = TRISTATE_BOOLEAN_UNSET;
 656         }
 657
 658         /* accumulate moving criteria */
 659         if ((result->overThresholds == TRISTATE_BOOLEAN_SET) || (result->outside == TRISTATE_BOOLEAN_SET)) {
 660                 result->moving = TRISTATE_BOOLEAN_SET;
 661         } else if ((result->overThresholds == TRISTATE_BOOLEAN_UNSET)
 662                         && (result->outside == TRISTATE_BOOLEAN_UNSET)) {
 663                 result->moving = TRISTATE_BOOLEAN_UNSET;
 664         }
 665
 666         return;
 667 }
 668
 669 /**
 670  Update the latest GPS information. This function is called when a packet is
 671  received from a rxNonOlsr interface, containing one or more NMEA strings with
 672  GPS information.
 673
 674  @param rxBuffer
 675  the receive buffer with the received NMEA string(s)
 676  @param rxCount
 677  the number of bytes in the receive buffer
 678
 679  @return
 680  - false on failure
 681  - true otherwise
 682  */
 683 bool receiverUpdateGpsInformation(unsigned char * rxBuffer, size_t rxCount) {
 684         static const char * rxBufferPrefix = "$GP";
 685         static const size_t rxBufferPrefixLength = 3;
 686
 687         bool retval = false;
 688         PositionUpdateEntry * incomingEntry;
 689         PositionUpdateEntry * posAvgEntry;
 690         MovementType movementResult;
 691         bool subStateExternalStateChange;
 692         bool externalStateChange;
 693         bool updateTransmitGpsInformation = false;
 694         PositionUpdateEntry txPosition;
 695         MovementState externalState;
 696
 697         /* do not process when the message does not start with $GP */
 698         if ((rxCount < rxBufferPrefixLength) || (strncmp((char *) rxBuffer,
 699                         rxBufferPrefix, rxBufferPrefixLength) != 0)) {
 700                 return true;
 701         }
 702
 703         /* parse all NMEA strings in the rxBuffer into the incoming entry */
 704         incomingEntry = getPositionAverageEntry(&positionAverageList, INCOMING);
 705         nmea_zero_INFO(&incomingEntry->nmeaInfo);
 706         nmea_parse(&nmeaParser, (char *) rxBuffer, rxCount,
 707                         &incomingEntry->nmeaInfo);
 708
 709 #if defined(PUD_DUMP_AVERAGING)
 710         dump_nmeaInfo(&incomingEntry->nmeaInfo,
 711                         "receiverUpdateGpsInformation: incoming entry");
 712 #endif /* PUD_DUMP_AVERAGING */
 713
 714         /* ignore when no useful information */
 715         if (incomingEntry->nmeaInfo.smask == GPNON) {
 716                 retval = true;
 717                 goto end;
 718         }
 719
 720         nmea_INFO_sanitise(&incomingEntry->nmeaInfo);
 721
 722 #if defined(PUD_DUMP_AVERAGING)
 723         dump_nmeaInfo(&incomingEntry->nmeaInfo,
 724                         "receiverUpdateGpsInformation: incoming entry after sanitise");
 725 #endif /* PUD_DUMP_AVERAGING */
 726
 727         /* we always work with latitude, longitude in degrees and DOPs in meters */
 728         nmea_INFO_unit_conversion(&incomingEntry->nmeaInfo);
 729
 730 #if defined(PUD_DUMP_AVERAGING)
 731         dump_nmeaInfo(&incomingEntry->nmeaInfo,
 732                         "receiverUpdateGpsInformation: incoming entry after unit conversion");
 733 #endif /* PUD_DUMP_AVERAGING */
 734
 735         /*
 736          * Averaging
 737          */
 738
 739         if (getInternalState(SUBSTATE_POSITION) == MOVEMENT_STATE_MOVING) {
 740                 /* flush average: keep only the incoming entry */
 741                 flushPositionAverageList(&positionAverageList);
 742         }
 743         addNewPositionToAverage(&positionAverageList, incomingEntry);
 744         posAvgEntry = getPositionAverageEntry(&positionAverageList, AVERAGE);
 745
 746 #if defined(PUD_DUMP_AVERAGING)
 747         dump_nmeaInfo(&posAvgEntry->nmeaInfo,
 748                         "receiverUpdateGpsInformation: posAvgEntry");
 749 #endif /* PUD_DUMP_AVERAGING */
 750
 751         /*
 752          * Movement detection
 753          */
 754
 755         clearMovementType(&movementResult);
 756
 757         (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
 758         txPosition = transmitGpsInformation.txPosition;
 759
 760         detemineMovingFromPosition(posAvgEntry, &txPosition, &movementResult);
 761
 762         /*
 763          * State Determination
 764          */
 765
 766         determineStateWithHysteresis(SUBSTATE_POSITION, movementResult.moving, &externalState, &externalStateChange,
 767                         &subStateExternalStateChange);
 768
 769         /*
 770          * Update transmitGpsInformation
 771          */
 772
 773         updateTransmitGpsInformation = subStateExternalStateChange
 774                         || (positionValid(posAvgEntry) && !positionValid(&txPosition))
 775                         || (movementResult.inside == TRISTATE_BOOLEAN_SET);
 776
 777         if ((externalState == MOVEMENT_STATE_MOVING) || updateTransmitGpsInformation) {
 778                 transmitGpsInformation.txPosition.nmeaInfo = posAvgEntry->nmeaInfo;
 779                 transmitGpsInformation.updated = true;
 780
 781 #if defined(PUD_DUMP_AVERAGING)
 782                 dump_nmeaInfo(&posAvgEntry->nmeaInfo,
 783                         "receiverUpdateGpsInformation: transmitGpsInformation");
 784 #endif /* PUD_DUMP_AVERAGING */
 785         }
 786
 787         (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
 788
 789         doImmediateTransmit(externalState, externalStateChange);
 790
 791         retval = true;
 792
 793         end:
 794         return retval;
 795 }
 796
 797 /**
 798  Start the receiver
 799
 800  @return
 801  - false on failure
 802  - true otherwise
 803  */
 804 bool startReceiver(void) {
 805         MovementState externalState;
 806
 807         pthread_mutexattr_t attr;
 808         if (pthread_mutexattr_init(&attr)) {
 809                 return false;
 810         }
 811         if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE_NP)) {
 812                 return false;
 813         }
 814         if (pthread_mutex_init(&transmitGpsInformation.mutex, &attr)) {
 815                 return false;
 816         }
 817
 818         if (!nmea_parser_init(&nmeaParser)) {
 819                 pudError(false, "Could not initialise NMEA parser");
 820                 return false;
 821         }
 822
 823         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
 824         transmitGpsInformation.txGateway = olsr_cnf->main_addr;
 825         transmitGpsInformation.updated = false;
 826
 827         initPositionAverageList(&positionAverageList, getAverageDepth());
 828
 829         if (!initOlsrTxTimer()) {
 830                 stopReceiver();
 831                 return false;
 832         }
 833
 834         if (!initUplinkTxTimer()) {
 835                 stopReceiver();
 836                 return false;
 837         }
 838
 839         if (!initGatewayTimer()) {
 840                 stopReceiver();
 841                 return false;
 842         }
 843
 844         externalState = getExternalState();
 845         restartOlsrTimer(externalState);
 846         restartUplinkTimer(externalState);
 847         if (!restartGatewayTimer(getGatewayDeterminationInterval(), &pud_gateway_timer_callback)) {
 848                 pudError(0, "Could not start gateway timer");
 849                 stopReceiver();
 850                 return false;
 851         }
 852
 853         return true;
 854 }
 855
 856 /**
 857  Stop the receiver
 858  */
 859 void stopReceiver(void) {
 860         destroyGatewayTimer();
 861         destroyUplinkTxTimer();
 862         destroyOlsrTxTimer();
 863
 864         destroyPositionAverageList(&positionAverageList);
 865
 866         (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
 867         nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
 868         transmitGpsInformation.txGateway = olsr_cnf->main_addr;
 869         transmitGpsInformation.updated = false;
 870         (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
 871
 872         nmea_parser_destroy(&nmeaParser);
 873
 874         (void) pthread_mutex_destroy(&transmitGpsInformation.mutex);
 875 }