Example #1
0
double MSEC2NDeg(int msdeg)
{
	double deg;
	deg = (double)msdeg/_MSEC2DEG_FACTOR;
	
	return nmea_degree2ndeg(deg);
}
Example #2
0
/**
 * \fn nmea_radian2ndeg
 * \brief Convert radian to NDEG (NMEA degree)
 */
double nmea_radian2ndeg(double val)
{ return nmea_degree2ndeg(nmea_radian2degree(val)); }
Example #3
0
/**
 Convert an OLSR message into a string to multicast on the LAN

 @param olsrMessage
 A pointer to the OLSR message
 @param txGpsBuffer
 A pointer to the buffer in which the transmit string can be written
 @param txGpsBufferSize
 The size of the txGpsBuffer

 @return
 - the length of the transmit string placed in the txGpsBuffer
 - 0 (zero) in case of an error
 */
unsigned int gpsFromOlsr(union olsr_message *olsrMessage,
		unsigned char * txGpsBuffer, unsigned int txGpsBufferSize) {
	unsigned long validityTime;

	struct tm timeStruct;
	char latitudeString[PUD_TX_LATITUDE_DIGITS];
	const char * latitudeHemisphere;
	char longitudeString[PUD_TX_LONGITUDE_DIGITS];
	const char * longitudeHemisphere;
	char altitudeString[PUD_TX_ALTITUDE_DIGITS];
	char speedString[PUD_TX_SPEED_DIGITS];
	char trackString[PUD_TX_TRACK_DIGITS];
	char hdopString[PUD_TX_HDOP_DIGITS];
	uint32_t present;
	char gateway[2] = { '0', '\0' };

	char nodeIdTypeString[PUD_TX_NODEIDTYPE_DIGITS];
	char nodeIdString[PUD_TX_NODEID_BUFFERSIZE];
	const char * nodeId;
	const void * ipAddr;
	char originatorBuffer[64];
	const char * originator;

	unsigned int transmitStringLength;

	PudOlsrPositionUpdate * olsrGpsMessage =
			getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);

	if (unlikely(getPositionUpdateVersion(olsrGpsMessage) != PUD_WIRE_FORMAT_VERSION)) {
		/* currently we can only handle our own version */
		pudError(false, "Can not handle version %u OLSR PUD messages"
			" (only version %u): message ignored",
			getPositionUpdateVersion(olsrGpsMessage), PUD_WIRE_FORMAT_VERSION);
		return 0;
	}

	ipAddr = (olsr_cnf->ip_version == AF_INET) ?
				(void *) &olsrMessage->v4.originator :
				(void *) &olsrMessage->v6.originator;
	originator = inet_ntop(olsr_cnf->ip_version, ipAddr, &originatorBuffer[0],
			sizeof(originatorBuffer));

	validityTime = getValidityTime(&olsrGpsMessage->validityTime);

	present = getPositionUpdatePresent(olsrGpsMessage);

	if (present & PUD_PRESENT_GATEWAY) {
		gateway[0] = '1';
	}

	/* time is ALWAYS present so we can just use it */
	getPositionUpdateTime(olsrGpsMessage, time(NULL), &timeStruct);

	if (likely(nmea_INFO_is_present(present, LAT))) {
		double latitude = getPositionUpdateLatitude(olsrGpsMessage);

		if (latitude >= 0) {
			latitudeHemisphere = "N";
		} else {
			latitudeHemisphere = "S";
			latitude = -latitude;
		}
		latitude = nmea_degree2ndeg(latitude);

		snprintf(&latitudeString[0], PUD_TX_LATITUDE_DIGITS, "%." PUD_TX_LATITUDE_DECIMALS "f", latitude);
	} else {
		latitudeHemisphere = "";
		latitudeString[0] = '\0';
	}

	if (likely(nmea_INFO_is_present(present, LON))) {
		double longitude = getPositionUpdateLongitude(olsrGpsMessage);

		if (longitude >= 0) {
			longitudeHemisphere = "E";
		} else {
			longitudeHemisphere = "W";
			longitude = -longitude;
		}
		longitude = nmea_degree2ndeg(longitude);

		snprintf(&longitudeString[0], PUD_TX_LONGITUDE_DIGITS, "%." PUD_TX_LONGITUDE_DECIMALS "f", longitude);
	} else {
		longitudeHemisphere = "";
		longitudeString[0] = '\0';
	}

	if (likely(nmea_INFO_is_present(present, ELV))) {
		snprintf(&altitudeString[0], PUD_TX_ALTITUDE_DIGITS, "%ld", getPositionUpdateAltitude(olsrGpsMessage));
	} else {
		altitudeString[0] = '\0';
	}

	if (likely(nmea_INFO_is_present(present, SPEED))) {
		snprintf(&speedString[0], PUD_TX_SPEED_DIGITS, "%lu", getPositionUpdateSpeed(olsrGpsMessage));
	} else {
		speedString[0] = '\0';
	}

	if (likely(nmea_INFO_is_present(present, TRACK))) {
		snprintf(&trackString[0], PUD_TX_TRACK_DIGITS, "%lu", getPositionUpdateTrack(olsrGpsMessage));
	} else {
		trackString[0] = '\0';
	}

	if (likely(nmea_INFO_is_present(present, HDOP))) {
		snprintf(&hdopString[0], PUD_TX_HDOP_DIGITS, "%." PUD_TX_HDOP_DECIMALS "f",
				nmea_meters2dop(getPositionUpdateHdop(olsrGpsMessage)));
	} else {
		hdopString[0] = '\0';
	}

	getNodeTypeStringFromOlsr(olsr_cnf->ip_version, olsrGpsMessage,
			&nodeIdTypeString[0], sizeof(nodeIdTypeString));
	getNodeIdStringFromOlsr(olsr_cnf->ip_version, olsrMessage, &nodeId,
			&nodeIdString[0], sizeof(nodeIdString));

	transmitStringLength = nmea_printf((char *) txGpsBuffer, txGpsBufferSize
			- 1, "$P%s," /* prefix (always) */
		"%u," /* sentence version (always) */
		"%s," /* gateway flag (always) */
		"%s," /* OLSR originator (always) */
		"%s,%s," /* nodeIdType/nodeId (always) */
		"%02u%02u%02u," /* date (always) */
		"%02u%02u%02u," /* time (always) */
		"%lu," /* validity time (always) */
		"%s,%s," /* latitude (optional) */
		"%s,%s," /* longitude (optional) */
		"%s," /* altitude (optional) */
		"%s," /* speed (optional) */
		"%s," /* track (optional) */
		"%s" /* hdop (optional) */
	, getTxNmeaMessagePrefix(), PUD_TX_SENTENCE_VERSION, &gateway[0],
			originator , &nodeIdTypeString[0],
			nodeId, timeStruct.tm_mday, timeStruct.tm_mon + 1, (timeStruct.tm_year
					% 100), timeStruct.tm_hour, timeStruct.tm_min,
			timeStruct.tm_sec, validityTime, &latitudeString[0],
			latitudeHemisphere, &longitudeString[0], longitudeHemisphere,
			&altitudeString[0], &speedString[0], &trackString[0],
			&hdopString[0]);

	if (unlikely(transmitStringLength > (txGpsBufferSize - 1))) {
		pudError(false, "String to transmit on non-OLSR is too large, need"
			" at least %u bytes, skipped", transmitStringLength);
		return 0;
	}

	if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
		txGpsBuffer[txGpsBufferSize - 1] = '\0';
	} else {
		txGpsBuffer[transmitStringLength] = '\0';
	}

	return transmitStringLength;
}