/**
* Generate a GPGSA sentence from an nmeaGPGSA structure
*
* @param s a pointer to the buffer to generate the string in
* @param len the size of the buffer
* @param pack the structure
* @return the length of the generated sentence
*/
int nmea_gen_GPGSA(char *s, const int len, const nmeaGPGSA *pack) {
int i;
char sFixMode[2];
char sFixType[2];
char sSatPrn[64];
char sPdop[16];
char sHdop[16];
char sVdop[16];
char * psSatPrn = &sSatPrn[0];
int ssSatPrn = sizeof(sSatPrn);
bool satinuse = nmea_INFO_is_present(pack->present, SATINUSE);
sFixMode[0] = sFixMode[1] = 0;
sFixType[0] = sFixType[1] = 0;
sSatPrn[0] = 0;
sPdop[0] = 0;
sHdop[0] = 0;
sVdop[0] = 0;
if (nmea_INFO_is_present(pack->present, FIX)) {
sFixMode[0] = pack->fix_mode;
snprintf(&sFixType[0], sizeof(sFixType), "%1d", pack->fix_type);
}
for (i = 0; i < NMEA_MAXSAT; i++) {
if (satinuse && pack->sat_prn[i]) {
int cnt = snprintf(psSatPrn, ssSatPrn, "%d", pack->sat_prn[i]);
if (cnt >= ssSatPrn) {
ssSatPrn = 0;
psSatPrn = &sSatPrn[sizeof(sSatPrn) - 1];
*psSatPrn = '\0';
break;
} else {
ssSatPrn -= cnt;
psSatPrn += cnt;
}
}
if (i < (NMEA_MAXSAT - 1)) {
*psSatPrn = ',';
psSatPrn++;
ssSatPrn--;
*psSatPrn = '\0';
}
}
if (nmea_INFO_is_present(pack->present, PDOP)) {
snprintf(&sPdop[0], sizeof(sPdop), "%03.1f", pack->PDOP);
}
if (nmea_INFO_is_present(pack->present, HDOP)) {
snprintf(&sHdop[0], sizeof(sHdop), "%03.1f", pack->HDOP);
}
if (nmea_INFO_is_present(pack->present, VDOP)) {
snprintf(&sVdop[0], sizeof(sVdop), "%03.1f", pack->VDOP);
}
return nmea_printf(s, len, "$GPGSA,%s,%s,%s,%s,%s,%s", &sFixMode[0], &sFixType[0], &sSatPrn[0], &sPdop[0],
&sHdop[0], &sVdop[0]);
}
/**
* Generate a GPGGA sentence from an nmeaGPGGA structure
*
* @param s a pointer to the buffer to generate the string in
* @param len the size of the buffer
* @param pack the structure
* @return the length of the generated sentence
*/
int nmea_gen_GPGGA(char *s, const int len, const nmeaGPGGA *pack) {
char sTime[16];
char sLat[16];
char sNs[2];
char sLon[16];
char sEw[2];
char sSig[4];
char sSatInUse[4];
char sHdop[16];
char sElv[16];
char sElvUnit[2];
sTime[0] = 0;
sLat[0] = 0;
sNs[0] = sNs[1] = 0;
sLon[0] = 0;
sEw[0] = sEw[1] = 0;
sSig[0] = 0;
sSatInUse[0] = 0;
sHdop[0] = 0;
sElv[0] = 0;
sElvUnit[0] = sElvUnit[1] = 0;
if (nmea_INFO_is_present(pack->present, UTCTIME)) {
snprintf(&sTime[0], sizeof(sTime), "%02d%02d%02d.%02d", pack->utc.hour, pack->utc.min, pack->utc.sec,
pack->utc.hsec);
}
if (nmea_INFO_is_present(pack->present, LAT)) {
snprintf(&sLat[0], sizeof(sLat), "%09.4f", pack->lat);
sNs[0] = pack->ns;
}
if (nmea_INFO_is_present(pack->present, LON)) {
snprintf(&sLon[0], sizeof(sLon), "%010.4f", pack->lon);
sEw[0] = pack->ew;
}
if (nmea_INFO_is_present(pack->present, SIG)) {
snprintf(&sSig[0], sizeof(sSig), "%1d", pack->sig);
}
if (nmea_INFO_is_present(pack->present, SATINUSECOUNT)) {
snprintf(&sSatInUse[0], sizeof(sSatInUse), "%02d", pack->satinuse);
}
if (nmea_INFO_is_present(pack->present, HDOP)) {
snprintf(&sHdop[0], sizeof(sHdop), "%03.1f", pack->HDOP);
}
if (nmea_INFO_is_present(pack->present, ELV)) {
snprintf(&sElv[0], sizeof(sElv), "%03.1f", pack->elv);
sElvUnit[0] = pack->elv_units;
}
return nmea_printf(s, len, "$GPGGA,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,,,,", &sTime[0], &sLat[0], &sNs[0],
&sLon[0], &sEw[0], &sSig[0], &sSatInUse[0], &sHdop[0], &sElv[0], &sElvUnit[0]);
}
/**
* Generate a GPRMC sentence from an nmeaGPRMC structure
*
* @param s a pointer to the buffer to generate the string in
* @param len the size of the buffer
* @param pack the structure
* @return the length of the generated sentence
*/
int nmea_gen_GPRMC(char *s, const int len, const nmeaGPRMC *pack) {
char sTime[16];
char sDate[16];
char sLat[16];
char sNs[2];
char sLon[16];
char sEw[2];
char sSpeed[16];
char sTrack[16];
char sMagvar[16];
char sMagvar_ew[2];
sTime[0] = 0;
sDate[0] = 0;
sLat[0] = 0;
sNs[0] = sNs[1] = 0;
sLon[0] = 0;
sEw[0] = sEw[1] = 0;
sSpeed[0] = 0;
sTrack[0] = 0;
sMagvar[0] = 0;
sMagvar_ew[0] = sMagvar_ew[1] = 0;
if (nmea_INFO_is_present(pack->present, UTCDATE)) {
snprintf(&sDate[0], sizeof(sDate), "%02d%02d%02d", pack->utc.day, pack->utc.mon + 1, pack->utc.year - 100);
}
if (nmea_INFO_is_present(pack->present, UTCTIME)) {
snprintf(&sTime[0], sizeof(sTime), "%02d%02d%02d.%02d", pack->utc.hour, pack->utc.min, pack->utc.sec,
pack->utc.hsec);
}
if (nmea_INFO_is_present(pack->present, LAT)) {
snprintf(&sLat[0], sizeof(sLat), "%09.4f", pack->lat);
sNs[0] = pack->ns;
}
if (nmea_INFO_is_present(pack->present, LON)) {
snprintf(&sLon[0], sizeof(sLon), "%010.4f", pack->lon);
sEw[0] = pack->ew;
}
if (nmea_INFO_is_present(pack->present, SPEED)) {
snprintf(&sSpeed[0], sizeof(sSpeed), "%03.1f", pack->speed);
}
if (nmea_INFO_is_present(pack->present, TRACK)) {
snprintf(&sTrack[0], sizeof(sTrack), "%03.1f", pack->track);
}
if (nmea_INFO_is_present(pack->present, MAGVAR)) {
snprintf(&sMagvar[0], sizeof(sMagvar), "%03.1f", pack->magvar);
sMagvar_ew[0] = pack->magvar_ew;
}
return nmea_printf(s, len, "$GPRMC,%s,%C,%s,%s,%s,%s,%s,%s,%s,%s,%s,%C", &sTime[0], pack->status, &sLat[0], &sNs[0],
&sLon[0], &sEw[0], &sSpeed[0], &sTrack[0], &sDate[0], &sMagvar[0], &sMagvar_ew[0], pack->mode);
}
/**
* Generate a GPGSV sentence from an nmeaGPGSV structure
*
* @param s a pointer to the buffer to generate the string in
* @param len the size of the buffer
* @param pack the structure
* @return the length of the generated sentence
*/
int nmea_gen_GPGSV(char *s, const int len, const nmeaGPGSV *pack) {
char sCount[2];
char sIndex[2];
char sSatCount[4];
char sSatInfo[64];
char * psSatInfo = &sSatInfo[0];
int ssSatInfo = sizeof(sSatInfo);
bool satinview = nmea_INFO_is_present(pack->present, SATINVIEW);
int i;
sCount[0] = 0;
sIndex[0] = 0;
sSatCount[0] = 0;
sSatInfo[0] = 0;
if (satinview) {
snprintf(&sCount[0], sizeof(sCount), "%1d", pack->pack_count);
snprintf(&sIndex[0], sizeof(sIndex), "%1d", pack->pack_index);
snprintf(&sSatCount[0], sizeof(sSatCount), "%02d", pack->sat_count);
}
for (i = 0; i < NMEA_SATINPACK; i++) {
int cnt = 0;
if (satinview && pack->sat_data[i].id) {
cnt = snprintf(psSatInfo, ssSatInfo, "%02d,%02d,%03d,%02d", pack->sat_data[i].id, pack->sat_data[i].elv,
pack->sat_data[i].azimuth, pack->sat_data[i].sig);
} else {
cnt = snprintf(psSatInfo, ssSatInfo, ",,,");
}
if (cnt >= ssSatInfo) {
ssSatInfo = 0;
psSatInfo = &sSatInfo[sizeof(sSatInfo) - 1];
*psSatInfo = '\0';
break;
} else {
ssSatInfo -= cnt;
psSatInfo += cnt;
}
if (i < (NMEA_SATINPACK - 1)) {
*psSatInfo = ',';
psSatInfo++;
ssSatInfo--;
*psSatInfo = '\0';
}
}
return nmea_printf(s, len, "$GPGSV,%s,%s,%s,%s", &sCount[0], &sIndex[0], &sSatCount[0], &sSatInfo[0]);
}
/**
* Generate a GPVTG sentence from an nmeaGPVTG structure
*
* @param s a pointer to the buffer to generate the string in
* @param len the size of the buffer
* @param pack the structure
* @return the length of the generated sentence
*/
int nmea_gen_GPVTG(char *s, const int len, const nmeaGPVTG *pack) {
char sTrackT[16];
char sTrackM[16];
char sSpeedN[16];
char sSpeedK[16];
char sUnitT[2];
char sUnitM[2];
char sUnitN[2];
char sUnitK[2];
sTrackT[0] = 0;
sTrackM[0] = 0;
sSpeedN[0] = 0;
sSpeedK[0] = 0;
sUnitT[0] = sUnitT[1] = 0;
sUnitM[0] = sUnitM[1] = 0;
sUnitN[0] = sUnitN[1] = 0;
sUnitK[0] = sUnitK[1] = 0;
if (nmea_INFO_is_present(pack->present, TRACK)) {
snprintf(&sTrackT[0], sizeof(sTrackT), "%03.1f", pack->track);
sUnitT[0] = 'T';
}
if (nmea_INFO_is_present(pack->present, MTRACK)) {
snprintf(&sTrackM[0], sizeof(sTrackM), "%03.1f", pack->mtrack);
sUnitM[0] = 'M';
}
if (nmea_INFO_is_present(pack->present, SPEED)) {
snprintf(&sSpeedN[0], sizeof(sSpeedN), "%03.1f", pack->spn);
sUnitN[0] = 'N';
snprintf(&sSpeedK[0], sizeof(sSpeedK), "%03.1f", pack->spk);
sUnitK[0] = 'K';
}
return nmea_printf(s, len, "$GPVTG,%s,%s,%s,%s,%s,%s,%s,%s", &sTrackT[0], &sUnitT[0], &sTrackM[0],
&sUnitM[0], &sSpeedN[0], &sUnitN[0], &sSpeedK[0], &sUnitK[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;
}
