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