1 #include "gpsConversion.h"
5 #include "configuration.h"
13 #include <nmea/gmath.h>
14 #include <arpa/inet.h>
15 #include <OlsrdPudWireFormat/nodeIdConversion.h>
17 /* ************************************************************************
19 * ************************************************************************ */
22 Convert an OLSR message into a string to multicast on the LAN
25 A pointer to the OLSR message
27 A pointer to the buffer in which the transmit string can be written
28 @param txGpsBufferSize
29 The size of the txGpsBuffer
32 - the length of the transmit string placed in the txGpsBuffer
33 - 0 (zero) in case of an error
35 unsigned int gpsFromOlsr(union olsr_message *olsrMessage,
36 unsigned char * txGpsBuffer, unsigned int txGpsBufferSize) {
37 unsigned long validityTime;
40 char latitudeString[PUD_TX_LATITUDE_DIGITS + 1];
41 const char * latitudeHemisphere;
42 char longitudeString[PUD_TX_LONGITUDE_DIGITS + 1];
43 const char * longitudeHemisphere;
44 char altitudeString[PUD_TX_ALTITUDE_DIGITS + 1];
45 char speedString[PUD_TX_SPEED_DIGITS + 1];
46 char trackString[PUD_TX_TRACK_DIGITS + 1];
47 char hdopString[PUD_TX_HDOP_DIGITS + 1];
50 char gateway[2] = { '0', '\0' };
52 char nodeIdTypeString[PUD_TX_NODEIDTYPE_DIGITS + 1];
53 char nodeIdString[PUD_TX_NODEID_BUFFERSIZE + 1];
56 char originatorBuffer[64];
57 const char * originator;
59 unsigned int transmitStringLength;
61 PudOlsrPositionUpdate * olsrGpsMessage =
62 getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
64 if (unlikely(getPositionUpdateVersion(olsrGpsMessage) != PUD_WIRE_FORMAT_VERSION)) {
65 /* currently we can only handle our own version */
66 pudError(false, "Can not handle version %u OLSR PUD messages"
67 " (only version %u): message ignored",
68 getPositionUpdateVersion(olsrGpsMessage), PUD_WIRE_FORMAT_VERSION);
72 ipAddr = (olsr_cnf->ip_version == AF_INET) ?
73 (void *) &olsrMessage->v4.originator :
74 (void *) &olsrMessage->v6.originator;
75 originator = inet_ntop(olsr_cnf->ip_version, ipAddr, &originatorBuffer[0],
76 sizeof(originatorBuffer));
78 validityTime = getValidityTime(&olsrGpsMessage->validityTime);
80 smask = getPositionUpdateSmask(olsrGpsMessage);
82 flags = getPositionUpdateSmask(olsrGpsMessage);
84 if (flags & PUD_FLAGS_GATEWAY) {
88 /* time is ALWAYS present so we can just use it */
89 getPositionUpdateTime(olsrGpsMessage, time(NULL), &timeStruct);
91 if (likely(nmea_INFO_has_field(smask, LAT))) {
93 double latitude = getPositionUpdateLatitude(olsrGpsMessage);
96 latitudeHemisphere = "N";
98 latitudeHemisphere = "S";
101 latitude = nmea_degree2ndeg(latitude);
103 chars = snprintf(&latitudeString[0], PUD_TX_LATITUDE_DIGITS,
104 "%." PUD_TX_LATITUDE_DECIMALS "f", latitude);
105 if (likely(chars < PUD_TX_LATITUDE_DIGITS)) {
106 latitudeString[chars] = '\0';
108 latitudeString[PUD_TX_LATITUDE_DIGITS] = '\0';
111 latitudeHemisphere = "";
112 latitudeString[0] = '\0';
115 if (likely(nmea_INFO_has_field(smask, LON))) {
117 double longitude = getPositionUpdateLongitude(olsrGpsMessage);
119 if (longitude >= 0) {
120 longitudeHemisphere = "E";
122 longitudeHemisphere = "W";
123 longitude = -longitude;
125 longitude = nmea_degree2ndeg(longitude);
127 chars = snprintf(&longitudeString[0], PUD_TX_LONGITUDE_DIGITS,
128 "%." PUD_TX_LONGITUDE_DECIMALS "f", longitude);
129 if (likely(chars < PUD_TX_LONGITUDE_DIGITS)) {
130 longitudeString[chars] = '\0';
132 longitudeString[PUD_TX_LONGITUDE_DIGITS] = '\0';
135 longitudeHemisphere = "";
136 longitudeString[0] = '\0';
139 if (likely(nmea_INFO_has_field(smask, ELV))) {
140 int chars = snprintf(&altitudeString[0], PUD_TX_ALTITUDE_DIGITS, "%ld",
141 getPositionUpdateAltitude(olsrGpsMessage));
142 if (likely(chars < PUD_TX_ALTITUDE_DIGITS)) {
143 altitudeString[chars] = '\0';
145 altitudeString[PUD_TX_ALTITUDE_DIGITS] = '\0';
148 altitudeString[0] = '\0';
151 if (likely(nmea_INFO_has_field(smask, SPEED))) {
152 int chars = snprintf(&speedString[0], PUD_TX_SPEED_DIGITS, "%lu",
153 getPositionUpdateSpeed(olsrGpsMessage));
154 if (likely(chars < PUD_TX_SPEED_DIGITS)) {
155 speedString[chars] = '\0';
157 speedString[PUD_TX_SPEED_DIGITS] = '\0';
160 speedString[0] = '\0';
163 if (likely(nmea_INFO_has_field(smask, DIRECTION))) {
164 int chars = snprintf(&trackString[0], PUD_TX_TRACK_DIGITS, "%lu",
165 getPositionUpdateTrack(olsrGpsMessage));
166 if (likely(chars < PUD_TX_TRACK_DIGITS)) {
167 trackString[chars] = '\0';
169 trackString[PUD_TX_TRACK_DIGITS] = '\0';
172 trackString[0] = '\0';
175 if (likely(nmea_INFO_has_field(smask, HDOP))) {
176 int chars = snprintf(&hdopString[0], PUD_TX_HDOP_DIGITS,
177 "%." PUD_TX_HDOP_DECIMALS "f", nmea_meters2dop(getPositionUpdateHdop(
179 if (likely(chars < PUD_TX_HDOP_DIGITS)) {
180 hdopString[chars] = '\0';
182 hdopString[PUD_TX_HDOP_DIGITS] = '\0';
185 hdopString[0] = '\0';
188 getNodeTypeStringFromOlsr(olsr_cnf->ip_version, olsrGpsMessage,
189 &nodeIdTypeString[0], sizeof(nodeIdTypeString));
190 getNodeIdStringFromOlsr(olsr_cnf->ip_version, olsrMessage, &nodeId,
191 &nodeIdString[0], sizeof(nodeIdString));
193 transmitStringLength = nmea_printf((char *) txGpsBuffer, txGpsBufferSize
194 - 1, "$P%s," /* prefix (always) */
195 "%u," /* sentence version (always) */
196 "%s," /* gateway flag (always) */
197 "%s," /* OLSR originator (always) */
198 "%s,%s," /* nodeIdType/nodeId (always) */
199 "%02u%02u%02u," /* date (always) */
200 "%02u%02u%02u," /* time (always) */
201 "%lu," /* validity time (always) */
202 "%s,%s," /* latitude (optional) */
203 "%s,%s," /* longitude (optional) */
204 "%s," /* altitude (optional) */
205 "%s," /* speed (optional) */
206 "%s," /* track (optional) */
207 "%s" /* hdop (optional) */
208 , getTxNmeaMessagePrefix(), PUD_TX_SENTENCE_VERSION, &gateway[0],
209 originator , &nodeIdTypeString[0],
210 nodeId, timeStruct.tm_mday, timeStruct.tm_mon + 1, (timeStruct.tm_year
211 % 100), timeStruct.tm_hour, timeStruct.tm_min,
212 timeStruct.tm_sec, validityTime, &latitudeString[0],
213 latitudeHemisphere, &longitudeString[0], longitudeHemisphere,
214 &altitudeString[0], &speedString[0], &trackString[0],
217 if (unlikely(transmitStringLength > (txGpsBufferSize - 1))) {
218 pudError(false, "String to transmit on non-OLSR is too large, need"
219 " at least %u bytes, skipped", transmitStringLength);
223 if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
224 txGpsBuffer[txGpsBufferSize - 1] = '\0';
226 txGpsBuffer[transmitStringLength] = '\0';
229 return transmitStringLength;
232 /* ************************************************************************
234 * ************************************************************************ */
237 Convert a nmeaINFO structure into an OLSR message.
240 A pointer to a nmeaINFO structure
242 A pointer to an OLSR message in which to place the converted information
243 @param olsrMessageSize
244 The maximum number of bytes available for the olsrMessage
246 the validity time of the message in seconds
249 - the aligned size of the converted information
250 - 0 (zero) in case of an error
252 unsigned int gpsToOlsr(nmeaINFO *nmeaInfo, union olsr_message *olsrMessage,
253 unsigned int olsrMessageSize, unsigned long long validityTime) {
254 unsigned int aligned_size;
255 unsigned int aligned_size_remainder;
257 nodeIdBinaryType * nodeIdBinary = NULL;
259 PudOlsrPositionUpdate * olsrGpsMessage =
260 getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
263 * Compose message contents
265 memset(olsrGpsMessage, 0, sizeof (PudOlsrPositionUpdate));
267 setPositionUpdateVersion(olsrGpsMessage, PUD_WIRE_FORMAT_VERSION);
268 setValidityTime(&olsrGpsMessage->validityTime, validityTime);
269 setPositionUpdateSmask(olsrGpsMessage, nmeaInfo->smask);
270 setPositionUpdateFlags(olsrGpsMessage,
271 getPositionUpdateFlags(olsrGpsMessage) & ~PUD_FLAGS_GATEWAY);
273 /* utc is always present, we make sure of that elsewhere, so just use it */
274 setPositionUpdateTime(olsrGpsMessage, nmeaInfo->utc.hour, nmeaInfo->utc.min,
277 if (likely(nmea_INFO_has_field(nmeaInfo->smask, LAT))) {
278 setPositionUpdateLatitude(olsrGpsMessage, nmeaInfo->lat);
280 setPositionUpdateLatitude(olsrGpsMessage, 0.0);
283 if (likely(nmea_INFO_has_field(nmeaInfo->smask, LON))) {
284 setPositionUpdateLongitude(olsrGpsMessage, nmeaInfo->lon);
286 setPositionUpdateLongitude(olsrGpsMessage, 0.0);
289 if (likely(nmea_INFO_has_field(nmeaInfo->smask, ELV))) {
290 setPositionUpdateAltitude(olsrGpsMessage, nmeaInfo->elv);
292 setPositionUpdateAltitude(olsrGpsMessage, 0.0);
295 if (likely(nmea_INFO_has_field(nmeaInfo->smask, SPEED))) {
296 setPositionUpdateSpeed(olsrGpsMessage, nmeaInfo->speed);
298 setPositionUpdateSpeed(olsrGpsMessage, 0.0);
301 if (likely(nmea_INFO_has_field(nmeaInfo->smask, DIRECTION))) {
302 setPositionUpdateTrack(olsrGpsMessage, nmeaInfo->direction);
304 setPositionUpdateTrack(olsrGpsMessage, 0);
307 if (likely(nmea_INFO_has_field(nmeaInfo->smask, HDOP))) {
308 setPositionUpdateHdop(olsrGpsMessage, nmeaInfo->HDOP);
310 setPositionUpdateHdop(olsrGpsMessage, PUD_HDOP_MAX);
313 nodeIdBinary = getNodeIdBinary();
314 nodeLength = setPositionUpdateNodeInfo(olsr_cnf->ip_version, olsrGpsMessage,
315 olsrMessageSize, getNodeIdTypeNumber(),
316 (unsigned char *) &nodeIdBinary->buffer, nodeIdBinary->length);
319 * Messages in OLSR are 4-byte aligned: align
322 /* size = type, string, string terminator */
323 aligned_size = PUD_OLSRWIREFORMATSIZE + nodeLength;
324 aligned_size_remainder = (aligned_size % 4);
325 if (aligned_size_remainder != 0) {
326 aligned_size += (4 - aligned_size_remainder);
330 * Fill message headers (fill ALL fields, except message)
331 * Note: olsr_vtime is currently unused, we use it for our validity time.
334 if (olsr_cnf->ip_version == AF_INET) {
337 olsrMessage->v4.olsr_msgtype = PUD_OLSR_MSG_TYPE;
338 olsrMessage->v4.olsr_vtime = reltime_to_me(validityTime * 1000);
339 /* message->v4.olsr_msgsize at the end */
340 olsrMessage->v4.originator = olsr_cnf->main_addr.v4.s_addr;
341 olsrMessage->v4.ttl = getOlsrTtl();
342 olsrMessage->v4.hopcnt = 0;
343 olsrMessage->v4.seqno = htons(get_msg_seqno());
345 /* add length of message->v4 fields */
346 aligned_size += (sizeof(olsrMessage->v4)
347 - sizeof(olsrMessage->v4.message));
348 olsrMessage->v4.olsr_msgsize = htons(aligned_size);
352 olsrMessage->v6.olsr_msgtype = PUD_OLSR_MSG_TYPE;
353 olsrMessage->v6.olsr_vtime = reltime_to_me(validityTime * 1000);
354 /* message->v6.olsr_msgsize at the end */
355 olsrMessage->v6.originator = olsr_cnf->main_addr.v6;
356 olsrMessage->v6.ttl = getOlsrTtl();
357 olsrMessage->v6.hopcnt = 0;
358 olsrMessage->v6.seqno = htons(get_msg_seqno());
360 /* add length of message->v6 fields */
361 aligned_size += (sizeof(olsrMessage->v6)
362 - sizeof(olsrMessage->v6.message));
363 olsrMessage->v6.olsr_msgsize = htons(aligned_size);
366 /* pad with zeroes */
367 if (aligned_size_remainder != 0) {
368 memset(&(((char *) &olsrGpsMessage->nodeInfo.nodeIdType)[nodeLength]),
369 0, (4 - aligned_size_remainder));