PUD: introduce wireFormat.c
[olsrd.git] / lib / pud / src / gpsConversion.c
1 #include "gpsConversion.h"
2
3 /* Plugin includes */
4 #include "wireFormat.h"
5 #include "pud.h"
6 #include "nodeIdConversion.h"
7 #include "nmeaTools.h"
8
9 /* OLSR includes */
10 #include "olsr.h"
11
12 /* System includes */
13 #include <nmea/gmath.h>
14 #include <nmea/tok.h>
15 #include <netinet/in.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 olsrMessagePayload
27  A pointer to the GPS message in the OLSR message
28  @param txGpsBuffer
29  A pointer to the buffer in which the transmit string can be written
30  @param txGpsBufferSize
31  The size of the txGpsBuffer
32
33  @return
34  - the length of the transmit string placed in the txGpsBuffer
35  - 0 (zero) in case of an error
36  */
37 unsigned int gpsFromOlsr(const union olsr_message *olsrMessage,
38                 unsigned char *olsrMessagePayload, unsigned char * txGpsBuffer,
39                 unsigned int txGpsBufferSize) {
40         PudOlsrWireFormat *olsrGpsMessage =
41                         (PudOlsrWireFormat *) olsrMessagePayload;
42         const GpsInfo* gpsMessage = &olsrGpsMessage->gpsInfo;
43
44         unsigned long validityTime;
45
46         struct tm timeStruct;
47         char latitudeString[PUD_TX_LATITUDE_DIGITS + 1];
48         const char * latitudeHemisphere;
49         char longitudeString[PUD_TX_LONGITUDE_DIGITS + 1];
50         const char * longitudeHemisphere;
51         char altitudeString[PUD_TX_ALTITUDE_DIGITS + 1];
52         char speedString[PUD_TX_SPEED_DIGITS + 1];
53         char trackString[PUD_TX_TRACK_DIGITS + 1];
54         char hdopString[PUD_TX_HDOP_DIGITS + 1];
55
56         char nodeIdTypeString[PUD_TX_NODEIDTYPE_DIGITS + 1];
57         char nodeIdString[PUD_TX_NODEID_BUFFERSIZE + 1];
58         const char * nodeId;
59
60         unsigned int transmitStringLength;
61
62         if (unlikely(olsrGpsMessage->version != PUD_WIRE_FORMAT_VERSION)) {
63                 /* currently we can only handle our own version */
64                 pudError(false, "Can not handle version %u OLSR PUD messages"
65                         " (only version %u): message ignored", olsrGpsMessage->version,
66                                 PUD_WIRE_FORMAT_VERSION);
67                 return 0;
68         }
69
70         validityTime = getValidityTimeFromOlsr(olsrGpsMessage->validityTime);
71
72         /* time is ALWAYS present so we can just use it */
73         getTimeFromOlsr(gpsMessage->time, &timeStruct);
74
75         if (likely(nmeaInfoHasField(olsrGpsMessage->smask, LAT))) {
76                 int chars;
77                 double latitude = getLatitudeFromOlsr(gpsMessage->lat);
78
79                 if (latitude >= 0) {
80                         latitudeHemisphere = "N";
81                 } else {
82                         latitudeHemisphere = "S";
83                         latitude = -latitude;
84                 }
85                 latitude = nmea_degree2ndeg(latitude);
86
87                 chars = snprintf(&latitudeString[0], PUD_TX_LATITUDE_DIGITS,
88                                 "%." PUD_TX_LATITUDE_DECIMALS "f", latitude);
89                 if (likely(chars < PUD_TX_LATITUDE_DIGITS)) {
90                         latitudeString[chars] = '\0';
91                 } else {
92                         latitudeString[PUD_TX_LATITUDE_DIGITS] = '\0';
93                 }
94         } else {
95                 latitudeHemisphere = "";
96                 latitudeString[0] = '\0';
97         }
98
99         if (likely(nmeaInfoHasField(olsrGpsMessage->smask, LON))) {
100                 int chars;
101                 double longitude = getLongitudeFromOlsr(gpsMessage->lon);
102
103                 if (longitude >= 0) {
104                         longitudeHemisphere = "E";
105                 } else {
106                         longitudeHemisphere = "W";
107                         longitude = -longitude;
108                 }
109                 longitude = nmea_degree2ndeg(longitude);
110
111                 chars = snprintf(&longitudeString[0], PUD_TX_LONGITUDE_DIGITS,
112                                 "%." PUD_TX_LONGITUDE_DECIMALS "f", longitude);
113                 if (likely(chars < PUD_TX_LONGITUDE_DIGITS)) {
114                         longitudeString[chars] = '\0';
115                 } else {
116                         longitudeString[PUD_TX_LONGITUDE_DIGITS] = '\0';
117                 }
118         } else {
119                 longitudeHemisphere = "";
120                 longitudeString[0] = '\0';
121         }
122
123         if (likely(nmeaInfoHasField(olsrGpsMessage->smask, ELV))) {
124                 int chars = snprintf(&altitudeString[0], PUD_TX_ALTITUDE_DIGITS, "%ld",
125                                 getAltitudeFromOlsr(gpsMessage->alt));
126                 if (likely(chars < PUD_TX_ALTITUDE_DIGITS)) {
127                         altitudeString[chars] = '\0';
128                 } else {
129                         altitudeString[PUD_TX_ALTITUDE_DIGITS] = '\0';
130                 }
131         } else {
132                 altitudeString[0] = '\0';
133         }
134
135         if (likely(nmeaInfoHasField(olsrGpsMessage->smask, SPEED))) {
136                 int chars = snprintf(&speedString[0], PUD_TX_SPEED_DIGITS, "%lu",
137                                 getSpeedFromOlsr(gpsMessage->speed));
138                 if (likely(chars < PUD_TX_SPEED_DIGITS)) {
139                         speedString[chars] = '\0';
140                 } else {
141                         speedString[PUD_TX_SPEED_DIGITS] = '\0';
142                 }
143         } else {
144                 speedString[0] = '\0';
145         }
146
147         if (likely(nmeaInfoHasField(olsrGpsMessage->smask, DIRECTION))) {
148                 int chars = snprintf(&trackString[0], PUD_TX_TRACK_DIGITS, "%lu",
149                                 getTrackFromOlsr(gpsMessage->track));
150                 if (likely(chars < PUD_TX_TRACK_DIGITS)) {
151                         trackString[chars] = '\0';
152                 } else {
153                         trackString[PUD_TX_TRACK_DIGITS] = '\0';
154                 }
155         } else {
156                 trackString[0] = '\0';
157         }
158
159         if (likely(nmeaInfoHasField(olsrGpsMessage->smask, HDOP))) {
160                 int chars = snprintf(&hdopString[0], PUD_TX_HDOP_DIGITS,
161                                 "%." PUD_TX_HDOP_DECIMALS "f", nmea_meters2dop(getHdopFromOlsr(
162                                                 gpsMessage->hdop)));
163                 if (likely(chars < PUD_TX_HDOP_DIGITS)) {
164                         hdopString[chars] = '\0';
165                 } else {
166                         hdopString[PUD_TX_HDOP_DIGITS] = '\0';
167                 }
168         } else {
169                 hdopString[0] = '\0';
170         }
171
172         getNodeInfoFromOlsr(olsrMessage, olsrGpsMessage, &nodeIdString[0],
173                         PUD_TX_NODEID_BUFFERSIZE, &nodeId, &nodeIdTypeString[0]);
174
175         transmitStringLength = nmea_printf((char *) txGpsBuffer, txGpsBufferSize
176                         - 1, "$P%s," /* prefix (always) */
177                 "%u," /* sentence version (always) */
178                 "%s,%s," /* nodeIdType/nodeId (always) */
179                 "%02u%02u%02u," /* date (always) */
180                 "%02u%02u%02u," /* time (always) */
181                 "%lu," /* validity time (always) */
182                 "%s,%s," /* latitude (optional) */
183                 "%s,%s," /* longitude (optional) */
184                 "%s," /* altitude (optional) */
185                 "%s," /* speed (optional) */
186                 "%s," /* track (optional) */
187                 "%s" /* hdop (optional) */
188         , getTxNmeaMessagePrefix(), PUD_TX_SENTENCE_VERSION, &nodeIdTypeString[0],
189                         nodeId, timeStruct.tm_mday, timeStruct.tm_mon, (timeStruct.tm_year
190                                         % 100), timeStruct.tm_hour, timeStruct.tm_min,
191                         timeStruct.tm_sec, validityTime, &latitudeString[0],
192                         latitudeHemisphere, &longitudeString[0], longitudeHemisphere,
193                         &altitudeString[0], &speedString[0], &trackString[0],
194                         &hdopString[0]);
195
196         if (unlikely(transmitStringLength > (txGpsBufferSize - 1))) {
197                 pudError(false, "String to transmit on non-OLSR is too large, need"
198                         " at least %u bytes, skipped", transmitStringLength);
199                 return 0;
200         }
201
202         if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
203                 txGpsBuffer[txGpsBufferSize - 1] = '\0';
204         } else {
205                 txGpsBuffer[transmitStringLength] = '\0';
206         }
207
208         return transmitStringLength;
209 }
210
211 /* ************************************************************************
212  * External --> OLSR
213  * ************************************************************************ */
214
215 /**
216  Convert a nmeaINFO structure into an OLSR message.
217
218  @param nmeaInfo
219  A pointer to a nmeaINFO structure
220  @param olsrMessage
221  A pointer to an OLSR message in which to place the converted information
222  @param olsrMessageSize
223  The maximum number of bytes available for the olsrMessage
224  @param validityTime
225  the validity time of the message
226
227  @return
228  - the aligned size of the converted information
229  - 0 (zero) in case of an error
230  */
231 unsigned int gpsToOlsr(nmeaINFO *nmeaInfo, union olsr_message *olsrMessage,
232                 unsigned int olsrMessageSize, unsigned long long validityTime) {
233         PudOlsrWireFormat * olsrGpsMessage;
234         unsigned int aligned_size;
235         unsigned int aligned_size_remainder;
236         size_t nodeLength;
237
238         if (olsr_cnf->ip_version == AF_INET) {
239                 olsrGpsMessage = (PudOlsrWireFormat *) &olsrMessage->v4.message;
240         } else {
241                 olsrGpsMessage = (PudOlsrWireFormat *) &olsrMessage->v6.message;
242         }
243
244         /*
245          * Compose message contents
246          */
247
248         olsrGpsMessage->version = PUD_WIRE_FORMAT_VERSION;
249         olsrGpsMessage->validityTime = getValidityTimeForOlsr(validityTime);
250         olsrGpsMessage->smask = nmeaInfo->smask;
251
252         /* utc is always present, we make sure of that, so just use it */
253         olsrGpsMessage->gpsInfo.time = getTimeForOlsr(nmeaInfo->utc.hour,
254                         nmeaInfo->utc.min, nmeaInfo->utc.sec);
255
256         if (likely(nmeaInfoHasField(nmeaInfo->smask, LAT))) {
257                 olsrGpsMessage->gpsInfo.lat = getLatitudeForOlsr(nmeaInfo->lat);
258         } else {
259                 olsrGpsMessage->gpsInfo.lat = (1 << (PUD_LATITUDE_BITS - 1));
260         }
261
262         if (likely(nmeaInfoHasField(nmeaInfo->smask, LON))) {
263                 olsrGpsMessage->gpsInfo.lon = getLongitudeForOlsr(nmeaInfo->lon);
264         } else {
265                 olsrGpsMessage->gpsInfo.lon = (1 << (PUD_LONGITUDE_BITS - 1));
266         }
267
268         if (likely(nmeaInfoHasField(nmeaInfo->smask, ELV))) {
269                 olsrGpsMessage->gpsInfo.alt = getAltitudeForOlsr(nmeaInfo->elv);
270         } else {
271                 olsrGpsMessage->gpsInfo.alt = -PUD_ALTITUDE_MIN;
272         }
273
274         if (likely(nmeaInfoHasField(nmeaInfo->smask, SPEED))) {
275                 olsrGpsMessage->gpsInfo.speed = getSpeedForOlsr(nmeaInfo->speed);
276         } else {
277                 olsrGpsMessage->gpsInfo.speed = 0;
278         }
279
280         if (likely(nmeaInfoHasField(nmeaInfo->smask, DIRECTION))) {
281                 olsrGpsMessage->gpsInfo.track = getTrackForOlsr(nmeaInfo->direction);
282         } else {
283                 olsrGpsMessage->gpsInfo.track = 0;
284         }
285
286         if (likely(nmeaInfoHasField(nmeaInfo->smask, HDOP))) {
287                 olsrGpsMessage->gpsInfo.hdop = getHdopForOlsr(nmeaInfo->HDOP);
288         } else {
289                 olsrGpsMessage->gpsInfo.hdop = PUD_HDOP_MAX;
290         }
291
292         nodeLength = setupNodeInfoForOlsr(olsrGpsMessage, olsrMessageSize);
293
294         /*
295          * Messages in OLSR are 4-byte aligned: align
296          */
297
298         /* size = type, string, string terminator */
299         aligned_size = PUD_OLSRWIREFORMATSIZE + nodeLength;
300         aligned_size_remainder = (aligned_size % 4);
301         if (aligned_size_remainder != 0) {
302                 aligned_size += (4 - aligned_size_remainder);
303         }
304
305         /*
306          * Fill message headers (fill ALL fields, except message)
307          * Note: olsr_vtime is currently unused, we use it for our validity time.
308          */
309
310         if (olsr_cnf->ip_version == AF_INET) {
311                 /* IPv4 */
312
313                 olsrMessage->v4.olsr_msgtype = PUD_OLSR_MSG_TYPE;
314                 olsrMessage->v4.olsr_vtime = reltime_to_me(validityTime * 1000);
315                 /* message->v4.olsr_msgsize at the end */
316                 memcpy(&olsrMessage->v4.originator, &olsr_cnf->main_addr,
317                                 olsr_cnf->ipsize);
318                 olsrMessage->v4.ttl = getOlsrTtl();
319                 olsrMessage->v4.hopcnt = 0;
320                 olsrMessage->v4.seqno = htons(get_msg_seqno());
321
322                 /* add length of message->v4 fields */
323                 aligned_size += (sizeof(olsrMessage->v4)
324                                 - sizeof(olsrMessage->v4.message));
325                 olsrMessage->v4.olsr_msgsize = htons(aligned_size);
326         } else {
327                 /* IPv6 */
328
329                 olsrMessage->v6.olsr_msgtype = PUD_OLSR_MSG_TYPE;
330                 olsrMessage->v6.olsr_vtime = reltime_to_me(validityTime * 1000);
331                 /* message->v6.olsr_msgsize at the end */
332                 memcpy(&olsrMessage->v6.originator, &olsr_cnf->main_addr,
333                                 olsr_cnf->ipsize);
334                 olsrMessage->v6.ttl = getOlsrTtl();
335                 olsrMessage->v6.hopcnt = 0;
336                 olsrMessage->v6.seqno = htons(get_msg_seqno());
337
338                 /* add length of message->v6 fields */
339                 aligned_size += (sizeof(olsrMessage->v6)
340                                 - sizeof(olsrMessage->v6.message));
341                 olsrMessage->v6.olsr_msgsize = htons(aligned_size);
342         }
343
344         /* pad with zeroes */
345         if (aligned_size_remainder != 0) {
346                 memset(&(((char *) &olsrGpsMessage->nodeInfo.nodeIdType)[nodeLength]),
347                                 0, (4 - aligned_size_remainder));
348         }
349
350         return aligned_size;
351 }