update comments on all ifdefs
[olsrd.git] / lib / pud / src / receiver.c
index 06610ec..86295af 100644 (file)
@@ -3,22 +3,21 @@
 /* Plugin includes */
 #include "pud.h"
 #include "state.h"
-#include "posAvg.h"
 #include "configuration.h"
 #include "gpsConversion.h"
 #include "networkInterfaces.h"
 #include "timers.h"
 #include "uplinkGateway.h"
+#include "posFile.h"
 
 /* OLSRD includes */
-#include "olsr_types.h"
 #include "net_olsr.h"
 
 /* System includes */
-#include <pthread.h>
 #include <nmea/parser.h>
 #include <nmea/gmath.h>
 #include <nmea/sentence.h>
+#include <nmea/context.h>
 #include <OlsrdPudWireFormat/wireFormat.h>
 
 /*
@@ -66,14 +65,6 @@ typedef enum _TimedTxInterface {
        TX_INTERFACE_UPLINK = 2
 } TimedTxInterface;
 
-/** Structure of the latest GPS information that is transmitted */
-typedef struct _TransmitGpsInformation {
-       pthread_mutex_t mutex; /**< access mutex */
-       bool positionUpdated; /**< true when the position information was updated */
-       PositionUpdateEntry txPosition; /**< The last transmitted position */
-       union olsr_ip_addr txGateway; /**< the best gateway */
-} TransmitGpsInformation;
-
 /** The latest position information that is transmitted */
 static TransmitGpsInformation transmitGpsInformation;
 
@@ -114,7 +105,7 @@ static void clearMovementType(MovementType * result) {
  - false otherwise
  */
 static bool positionValid(PositionUpdateEntry * position) {
-       return (nmea_INFO_has_field(position->nmeaInfo.smask, FIX)
+       return (nmea_INFO_is_present(position->nmeaInfo.present, FIX)
                        && (position->nmeaInfo.fix != NMEA_FIX_BAD));
 }
 
@@ -149,19 +140,15 @@ static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
 
        externalState = getExternalState();
 
-       (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
-
        /* only fixup timestamp when the position is valid _and_ when the position was not updated */
        if (positionValid(&transmitGpsInformation.txPosition) && !transmitGpsInformation.positionUpdated) {
-               nmea_time_now(&transmitGpsInformation.txPosition.nmeaInfo.utc);
+               nmea_time_now(&transmitGpsInformation.txPosition.nmeaInfo.utc, &transmitGpsInformation.txPosition.nmeaInfo.present);
        }
 
        nmeaInfo = transmitGpsInformation.txPosition.nmeaInfo;
        transmitGpsInformation.positionUpdated = false;
        gateway = transmitGpsInformation.txGateway;
 
-       (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
-
        /* convert nmeaINFO to wireformat olsr message */
        txBufferBytesUsed += sizeof(UplinkHeader); /* keep before txBufferSpaceFree usage */
        pu_size = gpsToOlsr(&nmeaInfo, pu, txBufferBytesFree,
@@ -175,6 +162,10 @@ static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
                int r;
                struct interface *ifn;
                for (ifn = ifnet; ifn; ifn = ifn->int_next) {
+                       /* force the pending buffer out if there's not enough space for our message */
+                       if ((int)pu_size > net_outbuffer_bytes_left(ifn)) {
+                         net_output(ifn);
+                       }
                        r = net_outbuffer_push(ifn, pu, pu_size);
                        if (r != (int) pu_size) {
                                pudError(
@@ -197,11 +188,14 @@ static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
                int fd = getDownlinkSocketFd();
                if (fd != -1) {
                        union olsr_sockaddr * uplink_addr = getUplinkAddr();
+                       void * addr;
+                       socklen_t addrSize;
 
                        UplinkMessage * cl_uplink = (UplinkMessage *) &txBuffer[txBufferBytesUsed];
                        UplinkClusterLeader * cl = &cl_uplink->msg.clusterLeader;
                        union olsr_ip_addr * cl_originator = getClusterLeaderOriginator(olsr_cnf->ip_version, cl);
                        union olsr_ip_addr * cl_clusterLeader = getClusterLeaderClusterLeader(olsr_cnf->ip_version, cl);
+
                        unsigned int cl_size =
                                        sizeof(UplinkClusterLeader) - sizeof(cl->leader)
                                                        + ((olsr_cnf->ip_version == AF_INET) ? sizeof(cl->leader.v4) :
@@ -210,6 +204,14 @@ static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
                        unsigned long long uplinkUpdateInterval =
                                        (externalState == MOVEMENT_STATE_STATIONARY) ? getUplinkUpdateIntervalStationary() : getUplinkUpdateIntervalMoving();
 
+                       if (uplink_addr->in.sa_family == AF_INET) {
+                               addr = &uplink_addr->in4;
+                               addrSize = sizeof(struct sockaddr_in);
+                       } else {
+                               addr = &uplink_addr->in6;
+                               addrSize = sizeof(struct sockaddr_in6);
+                       }
+
                        /*
                         * position update message (pu)
                         */
@@ -250,8 +252,7 @@ static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
                        txBufferBytesUsed += cl_size;
 
                        errno = 0;
-                       if (sendto(fd, &txBuffer, txBufferBytesUsed, 0, (struct sockaddr *) &uplink_addr->in,
-                                       sizeof(uplink_addr->in)) < 0) {
+                       if (sendto(fd, &txBuffer, txBufferBytesUsed, 0, addr, addrSize) < 0) {
                                pudError(true, "Could not send to uplink (size=%u)", txBufferBytesUsed);
                        }
                }
@@ -334,8 +335,6 @@ static void pud_gateway_timer_callback(void *context __attribute__ ((unused))) {
 
        getBestUplinkGateway(&bestGateway);
 
-       (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
-
        /*
         * Movement detection
         */
@@ -358,8 +357,6 @@ static void pud_gateway_timer_callback(void *context __attribute__ ((unused))) {
                transmitGpsInformation.txGateway = bestGateway;
        }
 
-       (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
-
        if (externalStateChange) {
                doImmediateTransmit(externalState);
        }
@@ -435,19 +432,19 @@ static void detemineMovingFromPosition(PositionUpdateEntry * avg, PositionUpdate
        /* both avg and lastTx are valid here */
 
        /* avg field presence booleans */
-       avgHasSpeed = nmea_INFO_has_field(avg->nmeaInfo.smask, SPEED);
-       avgHasPos = nmea_INFO_has_field(avg->nmeaInfo.smask, LAT)
-                       && nmea_INFO_has_field(avg->nmeaInfo.smask, LON);
-       avgHasHdop = nmea_INFO_has_field(avg->nmeaInfo.smask, HDOP);
-       avgHasElv = nmea_INFO_has_field(avg->nmeaInfo.smask, ELV);
-       avgHasVdop = nmea_INFO_has_field(avg->nmeaInfo.smask, VDOP);
+       avgHasSpeed = nmea_INFO_is_present(avg->nmeaInfo.present, SPEED);
+       avgHasPos = nmea_INFO_is_present(avg->nmeaInfo.present, LAT)
+                       && nmea_INFO_is_present(avg->nmeaInfo.present, LON);
+       avgHasHdop = nmea_INFO_is_present(avg->nmeaInfo.present, HDOP);
+       avgHasElv = nmea_INFO_is_present(avg->nmeaInfo.present, ELV);
+       avgHasVdop = nmea_INFO_is_present(avg->nmeaInfo.present, VDOP);
 
        /* lastTx field presence booleans */
-       lastTxHasPos = nmea_INFO_has_field(lastTx->nmeaInfo.smask, LAT)
-                       && nmea_INFO_has_field(lastTx->nmeaInfo.smask, LON);
-       lastTxHasHdop = nmea_INFO_has_field(lastTx->nmeaInfo.smask, HDOP);
-       lastTxHasElv = nmea_INFO_has_field(lastTx->nmeaInfo.smask, ELV);
-       lastTxHasVdop = nmea_INFO_has_field(lastTx->nmeaInfo.smask, VDOP);
+       lastTxHasPos = nmea_INFO_is_present(lastTx->nmeaInfo.present, LAT)
+                       && nmea_INFO_is_present(lastTx->nmeaInfo.present, LON);
+       lastTxHasHdop = nmea_INFO_is_present(lastTx->nmeaInfo.present, HDOP);
+       lastTxHasElv = nmea_INFO_is_present(lastTx->nmeaInfo.present, ELV);
+       lastTxHasVdop = nmea_INFO_is_present(lastTx->nmeaInfo.present, VDOP);
 
        /* fill in some values _or_ defaults */
        dopMultiplier = getDopMultiplier();
@@ -471,7 +468,7 @@ static void detemineMovingFromPosition(PositionUpdateEntry * avg, PositionUpdate
                lastTxPos.lat = nmea_degree2radian(lastTx->nmeaInfo.lat);
                lastTxPos.lon = nmea_degree2radian(lastTx->nmeaInfo.lon);
 
-               hDistance = nmea_distance_ellipsoid(&avgPos, &lastTxPos, NULL, NULL);
+               hDistance = fabs(nmea_distance_ellipsoid(&avgPos, &lastTxPos, NULL, NULL));
                hDistanceValid = true;
        } else {
                hDistanceValid = false;
@@ -671,7 +668,6 @@ bool receiverUpdateGpsInformation(unsigned char * rxBuffer, size_t rxCount) {
        bool subStateExternalStateChange;
        bool externalStateChange;
        bool updateTransmitGpsInformation = false;
-       PositionUpdateEntry txPosition;
        MovementState externalState;
 
        /* do not process when the message does not start with $GP */
@@ -714,10 +710,7 @@ bool receiverUpdateGpsInformation(unsigned char * rxBuffer, size_t rxCount) {
 
        clearMovementType(&movementResult);
 
-       (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
-       txPosition = transmitGpsInformation.txPosition;
-
-       detemineMovingFromPosition(posAvgEntry, &txPosition, &movementResult);
+       detemineMovingFromPosition(posAvgEntry, &transmitGpsInformation.txPosition, &movementResult);
 
        /*
         * State Determination
@@ -731,15 +724,24 @@ bool receiverUpdateGpsInformation(unsigned char * rxBuffer, size_t rxCount) {
         */
 
        updateTransmitGpsInformation = subStateExternalStateChange
-                       || (positionValid(posAvgEntry) && !positionValid(&txPosition))
+                       || (positionValid(posAvgEntry) && !positionValid(&transmitGpsInformation.txPosition))
                        || (movementResult.inside == TRISTATE_BOOLEAN_SET);
 
        if ((externalState == MOVEMENT_STATE_MOVING) || updateTransmitGpsInformation) {
-               transmitGpsInformation.txPosition.nmeaInfo = posAvgEntry->nmeaInfo;
+               transmitGpsInformation.txPosition = *posAvgEntry;
                transmitGpsInformation.positionUpdated = true;
-       }
 
-       (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
+               /*
+                * When we're stationary:
+                * - the track is not reliable or even invalid, so we must clear it.
+                * - to avoid confusion in consumers of the data, we must clear the speed
+                *   because it is possible to have a very low speed while moving.
+                */
+               if (externalState == MOVEMENT_STATE_STATIONARY) {
+                       transmitGpsInformation.txPosition.nmeaInfo.speed = (double)0.0;
+                       transmitGpsInformation.txPosition.nmeaInfo.track = (double)0.0;
+               }
+       }
 
        if (externalStateChange) {
                doImmediateTransmit(externalState);
@@ -751,6 +753,15 @@ bool receiverUpdateGpsInformation(unsigned char * rxBuffer, size_t rxCount) {
        return retval;
 }
 
+/**
+ * Log nmea library errors as plugin errors
+ * @param str
+ * @param str_size
+ */
+static void nmea_errors(const char *str, int str_size __attribute__((unused))) {
+       pudError(false, "NMEA library error: %s", str);
+}
+
 /**
  Start the receiver
 
@@ -760,27 +771,28 @@ bool receiverUpdateGpsInformation(unsigned char * rxBuffer, size_t rxCount) {
  */
 bool startReceiver(void) {
        MovementState externalState;
-
-       pthread_mutexattr_t attr;
-       if (pthread_mutexattr_init(&attr)) {
-               return false;
-       }
-       if (pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE_NP)) {
-               return false;
-       }
-       if (pthread_mutex_init(&transmitGpsInformation.mutex, &attr)) {
-               return false;
-       }
+       char * positionFile = getPositionFile();
 
        if (!nmea_parser_init(&nmeaParser)) {
                pudError(false, "Could not initialise NMEA parser");
                return false;
        }
 
-       nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
+       /* hook up the NMEA library error callback */
+       nmea_context_set_error_func(&nmea_errors);
+
+       if (positionFile) {
+               readPositionFile(positionFile, &transmitGpsInformation.txPosition.nmeaInfo);
+       } else {
+               nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
+       }
        transmitGpsInformation.txGateway = olsr_cnf->main_addr;
        transmitGpsInformation.positionUpdated = false;
+       transmitGpsInformation.nodeId = getNodeId();
 
+#ifdef HTTPINFO_PUD
+       olsr_cnf->pud_position = &transmitGpsInformation;
+#endif /* HTTPINFO_PUD */
        initPositionAverageList(&positionAverageList, getAverageDepth());
 
        if (!initOlsrTxTimer()) {
@@ -820,13 +832,9 @@ void stopReceiver(void) {
 
        destroyPositionAverageList(&positionAverageList);
 
-       (void) pthread_mutex_lock(&transmitGpsInformation.mutex);
        nmea_zero_INFO(&transmitGpsInformation.txPosition.nmeaInfo);
        transmitGpsInformation.txGateway = olsr_cnf->main_addr;
        transmitGpsInformation.positionUpdated = false;
-       (void) pthread_mutex_unlock(&transmitGpsInformation.mutex);
 
        nmea_parser_destroy(&nmeaParser);
-
-       (void) pthread_mutex_destroy(&transmitGpsInformation.mutex);
 }