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