PUD: include compiler.h where needed
[olsrd.git] / lib / pud / src / gpsConversion.c
1 #include "gpsConversion.h"
2
3 /* Plugin includes */
4 #include "pud.h"
5 #include "configuration.h"
6 #include "compiler.h"
7
8 /* OLSR includes */
9 #include "olsr.h"
10
11 /* System includes */
12 #include <nmea/tok.h>
13 #include <nmea/gmath.h>
14 #include <arpa/inet.h>
15 #include <OlsrdPudWireFormat/nodeIdConversion.h>
16
17 /* ************************************************************************
18  * OLSR --> External
19  * ************************************************************************ */
20
21 /**
22  Convert an OLSR message into a string to multicast on the LAN
23
24  @param olsrMessage
25  A pointer to the OLSR message
26  @param txGpsBuffer
27  A pointer to the buffer in which the transmit string can be written
28  @param txGpsBufferSize
29  The size of the txGpsBuffer
30
31  @return
32  - the length of the transmit string placed in the txGpsBuffer
33  - 0 (zero) in case of an error
34  */
35 unsigned int gpsFromOlsr(union olsr_message *olsrMessage,
36                 unsigned char * txGpsBuffer, unsigned int txGpsBufferSize) {
37         unsigned long validityTime;
38
39         struct tm timeStruct;
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];
48         uint8_t smask;
49         uint8_t flags;
50         char gateway[2] = { '0', '\0' };
51
52         char nodeIdTypeString[PUD_TX_NODEIDTYPE_DIGITS + 1];
53         char nodeIdString[PUD_TX_NODEID_BUFFERSIZE + 1];
54         const char * nodeId;
55         const void * ipAddr;
56         char originatorBuffer[64];
57         const char * originator;
58
59         unsigned int transmitStringLength;
60
61         PudOlsrPositionUpdate * olsrGpsMessage =
62                         getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
63
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);
69                 return 0;
70         }
71
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));
77
78         validityTime = getValidityTime(&olsrGpsMessage->validityTime);
79
80         smask = getPositionUpdateSmask(olsrGpsMessage);
81
82         flags = getPositionUpdateSmask(olsrGpsMessage);
83
84         if (flags & PUD_FLAGS_GATEWAY) {
85                 gateway[0] = '1';
86         }
87
88         /* time is ALWAYS present so we can just use it */
89         getPositionUpdateTime(olsrGpsMessage, time(NULL), &timeStruct);
90
91         if (likely(nmea_INFO_has_field(smask, LAT))) {
92                 int chars;
93                 double latitude = getPositionUpdateLatitude(olsrGpsMessage);
94
95                 if (latitude >= 0) {
96                         latitudeHemisphere = "N";
97                 } else {
98                         latitudeHemisphere = "S";
99                         latitude = -latitude;
100                 }
101                 latitude = nmea_degree2ndeg(latitude);
102
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';
107                 } else {
108                         latitudeString[PUD_TX_LATITUDE_DIGITS] = '\0';
109                 }
110         } else {
111                 latitudeHemisphere = "";
112                 latitudeString[0] = '\0';
113         }
114
115         if (likely(nmea_INFO_has_field(smask, LON))) {
116                 int chars;
117                 double longitude = getPositionUpdateLongitude(olsrGpsMessage);
118
119                 if (longitude >= 0) {
120                         longitudeHemisphere = "E";
121                 } else {
122                         longitudeHemisphere = "W";
123                         longitude = -longitude;
124                 }
125                 longitude = nmea_degree2ndeg(longitude);
126
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';
131                 } else {
132                         longitudeString[PUD_TX_LONGITUDE_DIGITS] = '\0';
133                 }
134         } else {
135                 longitudeHemisphere = "";
136                 longitudeString[0] = '\0';
137         }
138
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';
144                 } else {
145                         altitudeString[PUD_TX_ALTITUDE_DIGITS] = '\0';
146                 }
147         } else {
148                 altitudeString[0] = '\0';
149         }
150
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';
156                 } else {
157                         speedString[PUD_TX_SPEED_DIGITS] = '\0';
158                 }
159         } else {
160                 speedString[0] = '\0';
161         }
162
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';
168                 } else {
169                         trackString[PUD_TX_TRACK_DIGITS] = '\0';
170                 }
171         } else {
172                 trackString[0] = '\0';
173         }
174
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(
178                                                 olsrGpsMessage)));
179                 if (likely(chars < PUD_TX_HDOP_DIGITS)) {
180                         hdopString[chars] = '\0';
181                 } else {
182                         hdopString[PUD_TX_HDOP_DIGITS] = '\0';
183                 }
184         } else {
185                 hdopString[0] = '\0';
186         }
187
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));
192
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],
215                         &hdopString[0]);
216
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);
220                 return 0;
221         }
222
223         if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
224                 txGpsBuffer[txGpsBufferSize - 1] = '\0';
225         } else {
226                 txGpsBuffer[transmitStringLength] = '\0';
227         }
228
229         return transmitStringLength;
230 }
231
232 /* ************************************************************************
233  * External --> OLSR
234  * ************************************************************************ */
235
236 /**
237  Convert a nmeaINFO structure into an OLSR message.
238
239  @param nmeaInfo
240  A pointer to a nmeaINFO structure
241  @param olsrMessage
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
245  @param validityTime
246  the validity time of the message in seconds
247
248  @return
249  - the aligned size of the converted information
250  - 0 (zero) in case of an error
251  */
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;
256         size_t nodeLength;
257         nodeIdBinaryType * nodeIdBinary = NULL;
258
259         PudOlsrPositionUpdate * olsrGpsMessage =
260                         getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
261
262         /*
263          * Compose message contents
264          */
265         memset(olsrGpsMessage, 0, sizeof (PudOlsrPositionUpdate));
266
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);
272
273         /* utc is always present, we make sure of that elsewhere, so just use it */
274         setPositionUpdateTime(olsrGpsMessage, nmeaInfo->utc.hour, nmeaInfo->utc.min,
275                         nmeaInfo->utc.sec);
276
277         if (likely(nmea_INFO_has_field(nmeaInfo->smask, LAT))) {
278                 setPositionUpdateLatitude(olsrGpsMessage, nmeaInfo->lat);
279         } else {
280                 setPositionUpdateLatitude(olsrGpsMessage, 0.0);
281         }
282
283         if (likely(nmea_INFO_has_field(nmeaInfo->smask, LON))) {
284                 setPositionUpdateLongitude(olsrGpsMessage, nmeaInfo->lon);
285         } else {
286                 setPositionUpdateLongitude(olsrGpsMessage, 0.0);
287         }
288
289         if (likely(nmea_INFO_has_field(nmeaInfo->smask, ELV))) {
290                 setPositionUpdateAltitude(olsrGpsMessage, nmeaInfo->elv);
291         } else {
292                 setPositionUpdateAltitude(olsrGpsMessage, 0.0);
293         }
294
295         if (likely(nmea_INFO_has_field(nmeaInfo->smask, SPEED))) {
296                 setPositionUpdateSpeed(olsrGpsMessage, nmeaInfo->speed);
297         } else {
298                 setPositionUpdateSpeed(olsrGpsMessage, 0.0);
299         }
300
301         if (likely(nmea_INFO_has_field(nmeaInfo->smask, DIRECTION))) {
302                 setPositionUpdateTrack(olsrGpsMessage, nmeaInfo->direction);
303         } else {
304                 setPositionUpdateTrack(olsrGpsMessage, 0);
305         }
306
307         if (likely(nmea_INFO_has_field(nmeaInfo->smask, HDOP))) {
308                 setPositionUpdateHdop(olsrGpsMessage, nmeaInfo->HDOP);
309         } else {
310                 setPositionUpdateHdop(olsrGpsMessage, PUD_HDOP_MAX);
311         }
312
313         nodeIdBinary = getNodeIdBinary();
314         nodeLength = setPositionUpdateNodeInfo(olsr_cnf->ip_version, olsrGpsMessage,
315                         olsrMessageSize, getNodeIdTypeNumber(),
316                         (unsigned char *) &nodeIdBinary->buffer, nodeIdBinary->length);
317
318         /*
319          * Messages in OLSR are 4-byte aligned: align
320          */
321
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);
327         }
328
329         /*
330          * Fill message headers (fill ALL fields, except message)
331          * Note: olsr_vtime is currently unused, we use it for our validity time.
332          */
333
334         if (olsr_cnf->ip_version == AF_INET) {
335                 /* IPv4 */
336
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());
344
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);
349         } else {
350                 /* IPv6 */
351
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());
359
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);
364         }
365
366         /* pad with zeroes */
367         if (aligned_size_remainder != 0) {
368                 memset(&(((char *) &olsrGpsMessage->nodeInfo.nodeIdType)[nodeLength]),
369                                 0, (4 - aligned_size_remainder));
370         }
371
372         return aligned_size;
373 }