PUD: push olsrMessagePayload determination down into 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         getNodeInfoFromOlsr(olsrMessage, &nodeIdString[0], PUD_TX_NODEID_BUFFERSIZE,
180                         &nodeId, &nodeIdTypeString[0]);
181
182         transmitStringLength = nmea_printf((char *) txGpsBuffer, txGpsBufferSize
183                         - 1, "$P%s," /* prefix (always) */
184                 "%u," /* sentence version (always) */
185                 "%s,%s," /* nodeIdType/nodeId (always) */
186                 "%02u%02u%02u," /* date (always) */
187                 "%02u%02u%02u," /* time (always) */
188                 "%lu," /* validity time (always) */
189                 "%s,%s," /* latitude (optional) */
190                 "%s,%s," /* longitude (optional) */
191                 "%s," /* altitude (optional) */
192                 "%s," /* speed (optional) */
193                 "%s," /* track (optional) */
194                 "%s" /* hdop (optional) */
195         , getTxNmeaMessagePrefix(), PUD_TX_SENTENCE_VERSION, &nodeIdTypeString[0],
196                         nodeId, timeStruct.tm_mday, timeStruct.tm_mon, (timeStruct.tm_year
197                                         % 100), timeStruct.tm_hour, timeStruct.tm_min,
198                         timeStruct.tm_sec, validityTime, &latitudeString[0],
199                         latitudeHemisphere, &longitudeString[0], longitudeHemisphere,
200                         &altitudeString[0], &speedString[0], &trackString[0],
201                         &hdopString[0]);
202
203         if (unlikely(transmitStringLength > (txGpsBufferSize - 1))) {
204                 pudError(false, "String to transmit on non-OLSR is too large, need"
205                         " at least %u bytes, skipped", transmitStringLength);
206                 return 0;
207         }
208
209         if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
210                 txGpsBuffer[txGpsBufferSize - 1] = '\0';
211         } else {
212                 txGpsBuffer[transmitStringLength] = '\0';
213         }
214
215         return transmitStringLength;
216 }
217
218 /* ************************************************************************
219  * External --> OLSR
220  * ************************************************************************ */
221
222 /**
223  Convert a nmeaINFO structure into an OLSR message.
224
225  @param nmeaInfo
226  A pointer to a nmeaINFO structure
227  @param olsrMessage
228  A pointer to an OLSR message in which to place the converted information
229  @param olsrMessageSize
230  The maximum number of bytes available for the olsrMessage
231  @param validityTime
232  the validity time of the message
233
234  @return
235  - the aligned size of the converted information
236  - 0 (zero) in case of an error
237  */
238 unsigned int gpsToOlsr(nmeaINFO *nmeaInfo, union olsr_message *olsrMessage,
239                 unsigned int olsrMessageSize, unsigned long long validityTime) {
240         PudOlsrWireFormat * olsrGpsMessage;
241         unsigned int aligned_size;
242         unsigned int aligned_size_remainder;
243         size_t nodeLength;
244
245         if (olsr_cnf->ip_version == AF_INET) {
246                 olsrGpsMessage = (PudOlsrWireFormat *) &olsrMessage->v4.message;
247         } else {
248                 olsrGpsMessage = (PudOlsrWireFormat *) &olsrMessage->v6.message;
249         }
250
251         /*
252          * Compose message contents
253          */
254
255         olsrGpsMessage->version = PUD_WIRE_FORMAT_VERSION;
256         olsrGpsMessage->validityTime = getValidityTimeForOlsr(validityTime);
257         olsrGpsMessage->smask = nmeaInfo->smask;
258
259         /* utc is always present, we make sure of that, so just use it */
260         olsrGpsMessage->gpsInfo.time = getTimeForOlsr(nmeaInfo->utc.hour,
261                         nmeaInfo->utc.min, nmeaInfo->utc.sec);
262
263         if (likely(nmea_INFO_has_field(nmeaInfo->smask, LAT))) {
264                 olsrGpsMessage->gpsInfo.lat = getLatitudeForOlsr(nmeaInfo->lat);
265         } else {
266                 olsrGpsMessage->gpsInfo.lat = (1 << (PUD_LATITUDE_BITS - 1));
267         }
268
269         if (likely(nmea_INFO_has_field(nmeaInfo->smask, LON))) {
270                 olsrGpsMessage->gpsInfo.lon = getLongitudeForOlsr(nmeaInfo->lon);
271         } else {
272                 olsrGpsMessage->gpsInfo.lon = (1 << (PUD_LONGITUDE_BITS - 1));
273         }
274
275         if (likely(nmea_INFO_has_field(nmeaInfo->smask, ELV))) {
276                 olsrGpsMessage->gpsInfo.alt = getAltitudeForOlsr(nmeaInfo->elv);
277         } else {
278                 olsrGpsMessage->gpsInfo.alt = -PUD_ALTITUDE_MIN;
279         }
280
281         if (likely(nmea_INFO_has_field(nmeaInfo->smask, SPEED))) {
282                 olsrGpsMessage->gpsInfo.speed = getSpeedForOlsr(nmeaInfo->speed);
283         } else {
284                 olsrGpsMessage->gpsInfo.speed = 0;
285         }
286
287         if (likely(nmea_INFO_has_field(nmeaInfo->smask, DIRECTION))) {
288                 olsrGpsMessage->gpsInfo.track = getTrackForOlsr(nmeaInfo->direction);
289         } else {
290                 olsrGpsMessage->gpsInfo.track = 0;
291         }
292
293         if (likely(nmea_INFO_has_field(nmeaInfo->smask, HDOP))) {
294                 olsrGpsMessage->gpsInfo.hdop = getHdopForOlsr(nmeaInfo->HDOP);
295         } else {
296                 olsrGpsMessage->gpsInfo.hdop = PUD_HDOP_MAX;
297         }
298
299         nodeLength = setupNodeInfoForOlsr(olsrGpsMessage, olsrMessageSize);
300
301         /*
302          * Messages in OLSR are 4-byte aligned: align
303          */
304
305         /* size = type, string, string terminator */
306         aligned_size = PUD_OLSRWIREFORMATSIZE + nodeLength;
307         aligned_size_remainder = (aligned_size % 4);
308         if (aligned_size_remainder != 0) {
309                 aligned_size += (4 - aligned_size_remainder);
310         }
311
312         /*
313          * Fill message headers (fill ALL fields, except message)
314          * Note: olsr_vtime is currently unused, we use it for our validity time.
315          */
316
317         if (olsr_cnf->ip_version == AF_INET) {
318                 /* IPv4 */
319
320                 olsrMessage->v4.olsr_msgtype = PUD_OLSR_MSG_TYPE;
321                 olsrMessage->v4.olsr_vtime = reltime_to_me(validityTime * 1000);
322                 /* message->v4.olsr_msgsize at the end */
323                 memcpy(&olsrMessage->v4.originator, &olsr_cnf->main_addr,
324                                 olsr_cnf->ipsize);
325                 olsrMessage->v4.ttl = getOlsrTtl();
326                 olsrMessage->v4.hopcnt = 0;
327                 olsrMessage->v4.seqno = htons(get_msg_seqno());
328
329                 /* add length of message->v4 fields */
330                 aligned_size += (sizeof(olsrMessage->v4)
331                                 - sizeof(olsrMessage->v4.message));
332                 olsrMessage->v4.olsr_msgsize = htons(aligned_size);
333         } else {
334                 /* IPv6 */
335
336                 olsrMessage->v6.olsr_msgtype = PUD_OLSR_MSG_TYPE;
337                 olsrMessage->v6.olsr_vtime = reltime_to_me(validityTime * 1000);
338                 /* message->v6.olsr_msgsize at the end */
339                 memcpy(&olsrMessage->v6.originator, &olsr_cnf->main_addr,
340                                 olsr_cnf->ipsize);
341                 olsrMessage->v6.ttl = getOlsrTtl();
342                 olsrMessage->v6.hopcnt = 0;
343                 olsrMessage->v6.seqno = htons(get_msg_seqno());
344
345                 /* add length of message->v6 fields */
346                 aligned_size += (sizeof(olsrMessage->v6)
347                                 - sizeof(olsrMessage->v6.message));
348                 olsrMessage->v6.olsr_msgsize = htons(aligned_size);
349         }
350
351         /* pad with zeroes */
352         if (aligned_size_remainder != 0) {
353                 memset(&(((char *) &olsrGpsMessage->nodeInfo.nodeIdType)[nodeLength]),
354                                 0, (4 - aligned_size_remainder));
355         }
356
357         return aligned_size;
358 }