lib/pud/src/receiver.c

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