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