double MSEC2NDeg(int msdeg)
{
double deg;
deg = (double)msdeg/_MSEC2DEG_FACTOR;
return nmea_degree2ndeg(deg);
}
/**
* \fn nmea_radian2ndeg
* \brief Convert radian to NDEG (NMEA degree)
*/
double nmea_radian2ndeg(double val)
{ return nmea_degree2ndeg(nmea_radian2degree(val)); }
/**
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;
}
