/** * 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; }