void nmea_zero_INFO(nmeaINFO *info)
{
    memset(info, 0, sizeof(nmeaINFO));
    nmea_time_now(&info->utc);
    info->sig = NMEA_SIG_BAD;
    info->fix = NMEA_FIX_BAD;
	info->smask = 0; 
}
Exemple #2
0
void nmea_zero_GPRMC(nmeaGPRMC *pack)
{
    memset(pack, 0, sizeof(nmeaGPRMC));
    nmea_time_now(&pack->utc);
    pack->status = 'V';
    pack->ns = 'N';
    pack->ew = 'E';
    pack->declin_ew = 'E';
}
Exemple #3
0
void nmea_zero_GPGGA(nmeaGPGGA *pack)
{
    memset(pack, 0, sizeof(nmeaGPGGA));
    nmea_time_now(&pack->utc);
    pack->ns = 'N';
    pack->ew = 'E';
    pack->elv_units = 'M';
    pack->diff_units = 'M';
}
Exemple #4
0
/**
 * Clear an info structure.
 * Resets the time to now, sets up the signal as BAD, the FIX as BAD, and
 * signals presence of these fields.
 * Resets all other fields to 0.
 *
 * @param info a pointer to the structure
 */
void nmea_zero_INFO(nmeaINFO *info) {
    if (!info) {
        return;
    }

    memset(info, 0, sizeof(nmeaINFO));
    nmea_time_now(&info->utc, &info->present);

    info->sig = NMEA_SIG_BAD;
    nmea_INFO_set_present(&info->present, SIG);

    info->fix = NMEA_FIX_BAD;
    nmea_INFO_set_present(&info->present, FIX);
}
Exemple #5
0
int nmea_igen_rotate_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
    int it;
    int count = info->satinfo.inview;
    double deg = 360 / (count?count:1);
    double srt = (count?(info->satinfo.sat[0].azimuth):0) + 5;

    nmea_time_now(&info->utc);

    for(it = 0; it < count; ++it)
    {
        info->satinfo.sat[it].azimuth =
            (int)((srt >= 360)?srt - 360:srt);
        srt += deg;
    }

    return 1;
};
Exemple #6
0
/**
 * Sanitise the NMEA info, make sure that:
 * - sig is in the range [0, 8],
 * - fix is in the range [1, 3],
 * - DOPs are positive,
 * - latitude is in the range [-9000, 9000],
 * - longitude is in the range [-18000, 18000],
 * - speed is positive,
 * - track is in the range [0, 360>.
 * - mtrack is in the range [0, 360>.
 * - magvar is in the range [0, 360>.
 * - satinfo:
 *   - inuse and in_use are consistent (w.r.t. count)
 *   - inview and sat are consistent (w.r.t. count/id)
 *   - in_use and sat are consistent (w.r.t. count/id)
 *   - elv is in the range [0, 90]
 *   - azimuth is in the range [0, 359]
 *   - sig is in the range [0, 99]
 *
 * Time is set to the current time when not present.
 * Fields are reset to their defaults (0) when not signaled as being present.
 *
 * @param nmeaInfo
 * the NMEA info structure to sanitise
 */
void nmea_INFO_sanitise(nmeaINFO *nmeaInfo) {
    double lat = 0;
    double lon = 0;
    double speed = 0;
    double track = 0;
    double mtrack = 0;
    double magvar = 0;
    bool latAdjusted = false;
    bool lonAdjusted = false;
    bool speedAdjusted = false;
    bool trackAdjusted = false;
    bool mtrackAdjusted = false;
    bool magvarAdjusted = false;
    nmeaTIME utc;
    int inuseIndex;
    int inviewIndex;

    if (!nmeaInfo) {
        return;
    }

    nmeaInfo->present = nmeaInfo->present & NMEA_INFO_PRESENT_MASK;

    if (!nmea_INFO_is_present(nmeaInfo->present, SMASK)) {
        nmeaInfo->smask = 0;
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, UTCDATE) || !nmea_INFO_is_present(nmeaInfo->present, UTCTIME)) {
        nmea_time_now(&utc, NULL);
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, UTCDATE)) {
        nmeaInfo->utc.year = utc.year;
        nmeaInfo->utc.mon = utc.mon;
        nmeaInfo->utc.day = utc.day;
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, UTCTIME)) {
        nmeaInfo->utc.hour = utc.hour;
        nmeaInfo->utc.min = utc.min;
        nmeaInfo->utc.sec = utc.sec;
        nmeaInfo->utc.hsec = utc.hsec;
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, SIG)) {
        nmeaInfo->sig = NMEA_SIG_BAD;
    } else {
        if ((nmeaInfo->sig < NMEA_SIG_BAD) || (nmeaInfo->sig > NMEA_SIG_SIM)) {
            nmeaInfo->sig = NMEA_SIG_BAD;
        }
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, FIX)) {
        nmeaInfo->fix = NMEA_FIX_BAD;
    } else {
        if ((nmeaInfo->fix < NMEA_FIX_BAD) || (nmeaInfo->fix > NMEA_FIX_3D)) {
            nmeaInfo->fix = NMEA_FIX_BAD;
        }
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, PDOP)) {
        nmeaInfo->PDOP = 0;
    } else {
        nmeaInfo->PDOP = fabs(nmeaInfo->PDOP);
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, HDOP)) {
        nmeaInfo->HDOP = 0;
    } else {
        nmeaInfo->HDOP = fabs(nmeaInfo->HDOP);
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, VDOP)) {
        nmeaInfo->VDOP = 0;
    } else {
        nmeaInfo->VDOP = fabs(nmeaInfo->VDOP);
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, LAT)) {
        nmeaInfo->lat = 0;
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, LON)) {
        nmeaInfo->lon = 0;
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, ELV)) {
        nmeaInfo->elv = 0;
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, SPEED)) {
        nmeaInfo->speed = 0;
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, TRACK)) {
        nmeaInfo->track = 0;
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, MTRACK)) {
        nmeaInfo->mtrack = 0;
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, MAGVAR)) {
        nmeaInfo->magvar = 0;
    }

    if (!nmea_INFO_is_present(nmeaInfo->present, SATINUSECOUNT)) {
        nmeaInfo->satinfo.inuse = 0;
    }
    if (!nmea_INFO_is_present(nmeaInfo->present, SATINUSE)) {
        memset(&nmeaInfo->satinfo.in_use, 0, sizeof(nmeaInfo->satinfo.in_use));
    }
    if (!nmea_INFO_is_present(nmeaInfo->present, SATINVIEW)) {
        nmeaInfo->satinfo.inview = 0;
        memset(&nmeaInfo->satinfo.sat, 0, sizeof(nmeaInfo->satinfo.sat));
    }

    /*
     * lat
     */

    lat = nmeaInfo->lat;
    lon = nmeaInfo->lon;

    /* force lat in [-18000, 18000] */
    while (lat < -18000.0) {
        lat += 36000.0;
        latAdjusted = true;
    }
    while (lat > 18000.0) {
        lat -= 36000.0;
        latAdjusted = true;
    }

    /* lat is now in [-18000, 18000] */

    /* force lat from <9000, 18000] in [9000, 0] */
    if (lat > 9000.0) {
        lat = 18000.0 - lat;
        lon += 18000.0;
        latAdjusted = true;
        lonAdjusted = true;
    }

    /* force lat from [-18000, -9000> in [0, -9000] */
    if (lat < -9000.0) {
        lat = -18000.0 - lat;
        lon += 18000.0;
        latAdjusted = true;
        lonAdjusted = true;
    }

    /* lat is now in [-9000, 9000] */

    if (latAdjusted) {
        nmeaInfo->lat = lat;
    }

    /*
     * lon
     */

    /* force lon in [-18000, 18000] */
    while (lon < -18000.0) {
        lon += 36000.0;
        lonAdjusted = true;
    }
    while (lon > 18000.0) {
        lon -= 36000.0;
        lonAdjusted = true;
    }

    /* lon is now in [-18000, 18000] */

    if (lonAdjusted) {
        nmeaInfo->lon = lon;
    }

    /*
     * speed
     */

    speed = nmeaInfo->speed;
    track = nmeaInfo->track;
    mtrack = nmeaInfo->mtrack;

    if (speed < 0.0) {
        speed = -speed;
        track += 180.0;
        mtrack += 180.0;
        speedAdjusted = true;
        trackAdjusted = true;
        mtrackAdjusted = true;
    }

    /* speed is now in [0, max> */

    if (speedAdjusted) {
        nmeaInfo->speed = speed;
    }

    /*
     * track
     */

    /* force track in [0, 360> */
    while (track < 0.0) {
        track += 360.0;
        trackAdjusted = true;
    }
    while (track >= 360.0) {
        track -= 360.0;
        trackAdjusted = true;
    }

    /* track is now in [0, 360> */

    if (trackAdjusted) {
        nmeaInfo->track = track;
    }

    /*
     * mtrack
     */

    /* force mtrack in [0, 360> */
    while (mtrack < 0.0) {
        mtrack += 360.0;
        mtrackAdjusted = true;
    }
    while (mtrack >= 360.0) {
        mtrack -= 360.0;
        mtrackAdjusted = true;
    }

    /* mtrack is now in [0, 360> */

    if (mtrackAdjusted) {
        nmeaInfo->mtrack = mtrack;
    }

    /*
     * magvar
     */

    magvar = nmeaInfo->magvar;

    /* force magvar in [0, 360> */
    while (magvar < 0.0) {
        magvar += 360.0;
        magvarAdjusted = true;
    }
    while (magvar >= 360.0) {
        magvar -= 360.0;
        magvarAdjusted = true;
    }

    /* magvar is now in [0, 360> */

    if (magvarAdjusted) {
        nmeaInfo->magvar = magvar;
    }

    /*
     * satinfo
     */

    nmeaInfo->satinfo.inuse = 0;
    for (inuseIndex = 0; inuseIndex < NMEA_MAXSAT; inuseIndex++) {
        if (nmeaInfo->satinfo.in_use[inuseIndex])
            nmeaInfo->satinfo.inuse++;
    }

    nmeaInfo->satinfo.inview = 0;
    for (inviewIndex = 0; inviewIndex < NMEA_MAXSAT; inviewIndex++) {
        if (nmeaInfo->satinfo.sat[inviewIndex].id) {
            nmeaInfo->satinfo.inview++;

            /* force elv in [-180, 180] */
            while (nmeaInfo->satinfo.sat[inviewIndex].elv < -180) {
                nmeaInfo->satinfo.sat[inviewIndex].elv += 360;
            }
            while (nmeaInfo->satinfo.sat[inviewIndex].elv > 180) {
                nmeaInfo->satinfo.sat[inviewIndex].elv -= 360;
            }

            /* elv is now in [-180, 180] */

            /* force elv from <90, 180] in [90, 0] */
            if (nmeaInfo->satinfo.sat[inviewIndex].elv > 90) {
                nmeaInfo->satinfo.sat[inviewIndex].elv = 180 - nmeaInfo->satinfo.sat[inviewIndex].elv;
            }

            /* force elv from [-180, -90> in [0, -90] */
            if (nmeaInfo->satinfo.sat[inviewIndex].elv < -90) {
                nmeaInfo->satinfo.sat[inviewIndex].elv = -180 - nmeaInfo->satinfo.sat[inviewIndex].elv;
            }

            /* elv is now in [-90, 90] */

            if (nmeaInfo->satinfo.sat[inviewIndex].elv < 0) {
                nmeaInfo->satinfo.sat[inviewIndex].elv = -nmeaInfo->satinfo.sat[inviewIndex].elv;
            }

            /* elv is now in [0, 90] */

            /* force azimuth in [0, 360> */
            while (nmeaInfo->satinfo.sat[inviewIndex].azimuth < 0) {
                nmeaInfo->satinfo.sat[inviewIndex].azimuth += 360;
            }
            while (nmeaInfo->satinfo.sat[inviewIndex].azimuth >= 360) {
                nmeaInfo->satinfo.sat[inviewIndex].azimuth -= 360;
            }
            /* azimuth is now in [0, 360> */

            /* force sig in [0, 99] */
            if (nmeaInfo->satinfo.sat[inviewIndex].sig < 0)
                nmeaInfo->satinfo.sat[inviewIndex].sig = 0;
            if (nmeaInfo->satinfo.sat[inviewIndex].sig > 99)
                nmeaInfo->satinfo.sat[inviewIndex].sig = 99;
        }
    }

    /* make sure the in_use IDs map to sat IDs */
    for (inuseIndex = 0; inuseIndex < NMEA_MAXSAT; inuseIndex++) {
        int inuseID = nmeaInfo->satinfo.in_use[inuseIndex];
        if (inuseID) {
            bool found = false;
            for (inviewIndex = 0; inviewIndex < NMEA_MAXSAT; inviewIndex++) {
                int inviewID = nmeaInfo->satinfo.sat[inviewIndex].id;
                if (inuseID == inviewID) {
                    found = true;
                    break;
                }
            }
            if (!found) {
                /* clear the id, did not find it */
                nmeaInfo->satinfo.in_use[inuseIndex] = 0;
                if (nmeaInfo->satinfo.inuse)
                    nmeaInfo->satinfo.inuse--;
            }
        }
    }
}
Exemple #7
0
int nmea_igen_static_loop(nmeaGENERATOR *gen, nmeaINFO *info)
{
    nmea_time_now(&info->utc);
    return 1;
};
Exemple #8
0
/**
 Send the transmit buffer out over all designated interfaces, called as a
 timer callback and also immediately on an external state change.

 @param interfaces
 a bitmap defining which interfaces to send over
 */
static void txToAllOlsrInterfaces(TimedTxInterface interfaces) {
	/** txBuffer is used to concatenate the position update and cluster leader messages in */
	unsigned char txBuffer[TX_BUFFER_SIZE_FOR_OLSR];
	unsigned int txBufferBytesUsed = 0;
	#define txBufferBytesFree	(sizeof(txBuffer) - txBufferBytesUsed)

	/*
	 * The first message in txBuffer is an OLSR position update.
	 *
	 * The position update is always present.
	 *
	 * The second message is the cluster leader message, but only when uplink
	 * was requested and correctly configured.
	 */

	UplinkMessage * pu_uplink = (UplinkMessage *) &txBuffer[0];
	union olsr_message * pu = &pu_uplink->msg.olsrMessage;
	unsigned int pu_size = 0;
	union olsr_ip_addr gateway;
	MovementState externalState;
	nmeaINFO nmeaInfo;

	externalState = getExternalState();

	/* 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, &transmitGpsInformation.txPosition.nmeaInfo.present);
	}

	nmeaInfo = transmitGpsInformation.txPosition.nmeaInfo;
	transmitGpsInformation.positionUpdated = false;
	gateway = transmitGpsInformation.txGateway;

	/* convert nmeaINFO to wireformat olsr message */
	txBufferBytesUsed += sizeof(UplinkHeader); /* keep before txBufferSpaceFree usage */
	pu_size = gpsToOlsr(&nmeaInfo, pu, txBufferBytesFree,
			((externalState == MOVEMENT_STATE_STATIONARY) ? getUpdateIntervalStationary() : getUpdateIntervalMoving()));
	txBufferBytesUsed += pu_size;

	/*
	 * push out to all OLSR interfaces
	 */
	if (((interfaces & TX_INTERFACE_OLSR) != 0) && getOlsrTtl() && (pu_size > 0)) {
		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(
						false,
						"Could not send to OLSR interface %s: %s (size=%u, r=%d)",
						ifn->int_name,
						((r == -1) ? "no buffer was found" :
							(r == 0) ? "there was not enough room in the buffer" : "unknown reason"), pu_size, r);
			}
		}

		/* loopback to tx interface when so configured */
		if (getUseLoopback()) {
			(void) packetReceivedFromOlsr(pu, NULL, NULL);
		}
	}

	/* push out over uplink when an uplink is configured */
	if (((interfaces & TX_INTERFACE_UPLINK) != 0) && isUplinkAddrSet()) {
		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) :
									sizeof(cl->leader.v6));

			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)
			 */

			/* set header fields in position update uplink message and adjust
			 * the validity time to the uplink validity time */
			if (pu_size > 0) {
				PudOlsrPositionUpdate * pu_gpsMessage = getOlsrMessagePayload(olsr_cnf->ip_version, pu);

				setUplinkMessageType(&pu_uplink->header, POSITION);
				setUplinkMessageLength(&pu_uplink->header, pu_size);
				setUplinkMessageIPv6(&pu_uplink->header, (olsr_cnf->ip_version != AF_INET));
				setUplinkMessagePadding(&pu_uplink->header, 0);

				/* fixup validity time */
				setValidityTime(&pu_gpsMessage->validityTime, uplinkUpdateInterval);
			}

			/*
			 * cluster leader message (cl)
			 */

			/* set cl_uplink header fields */
			setUplinkMessageType(&cl_uplink->header, CLUSTERLEADER);
			setUplinkMessageLength(&cl_uplink->header, cl_size);
			setUplinkMessageIPv6(&cl_uplink->header, (olsr_cnf->ip_version != AF_INET));
			setUplinkMessagePadding(&cl_uplink->header, 0);

			/* setup cl */
			setClusterLeaderVersion(cl, PUD_WIRE_FORMAT_VERSION);
			setValidityTime(&cl->validityTime, uplinkUpdateInterval);

			/* really need 2 memcpy's here because of olsr_cnf->ipsize */
			memcpy(cl_originator, &olsr_cnf->main_addr, olsr_cnf->ipsize);
			memcpy(cl_clusterLeader, &gateway, olsr_cnf->ipsize);

			txBufferBytesUsed += sizeof(UplinkHeader);
			txBufferBytesUsed += cl_size;

			errno = 0;
			if (sendto(fd, &txBuffer, txBufferBytesUsed, 0, addr, addrSize) < 0) {
				pudError(true, "Could not send to uplink (size=%u)", txBufferBytesUsed);
			}
		}
	}
}