3dc6b6b1d4078bcfd266fd0766c391b8ffdcbe3b
[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 #include "compiler.h"
9
10 /* OLSR includes */
11 #include "olsr.h"
12
13 /* System includes */
14 #include <nmea/gmath.h>
15 #include <nmea/tok.h>
16 #include <nmea/info.h>
17 #include <netinet/in.h>
18 #include <stdio.h>
19
20 /* ************************************************************************
21  * OLSR --> External
22  * ************************************************************************ */
23
24 /**
25  Convert an OLSR message into a string to multicast on the LAN
26
27  @param olsrMessage
28  A pointer to the OLSR message
29  @param txGpsBuffer
30  A pointer to the buffer in which the transmit string can be written
31  @param txGpsBufferSize
32  The size of the txGpsBuffer
33
34  @return
35  - the length of the transmit string placed in the txGpsBuffer
36  - 0 (zero) in case of an error
37  */
38 unsigned int gpsFromOlsr(union olsr_message *olsrMessage,
39                 unsigned char * txGpsBuffer, unsigned int txGpsBufferSize) {
40         unsigned long validityTime;
41
42         struct tm timeStruct;
43         char latitudeString[PUD_TX_LATITUDE_DIGITS + 1];
44         const char * latitudeHemisphere;
45         char longitudeString[PUD_TX_LONGITUDE_DIGITS + 1];
46         const char * longitudeHemisphere;
47         char altitudeString[PUD_TX_ALTITUDE_DIGITS + 1];
48         char speedString[PUD_TX_SPEED_DIGITS + 1];
49         char trackString[PUD_TX_TRACK_DIGITS + 1];
50         char hdopString[PUD_TX_HDOP_DIGITS + 1];
51
52         char nodeIdTypeString[PUD_TX_NODEIDTYPE_DIGITS + 1];
53         char nodeIdString[PUD_TX_NODEID_BUFFERSIZE + 1];
54         const char * nodeId;
55
56         unsigned int transmitStringLength;
57
58         GpsInfo* gpsMessage;
59         PudOlsrWireFormat * olsrGpsMessage =
60                         getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
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         gpsMessage = &olsrGpsMessage->gpsInfo;
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         getNodeTypeStringFromOlsr(olsr_cnf->ip_version, olsrMessage,
175                         &nodeIdTypeString[0], sizeof(nodeIdTypeString));
176         getNodeIdStringFromOlsr(olsr_cnf->ip_version, olsrMessage, &nodeId,
177                         &nodeIdString[0], sizeof(nodeIdString));
178
179         transmitStringLength = nmea_printf((char *) txGpsBuffer, txGpsBufferSize
180                         - 1, "$P%s," /* prefix (always) */
181                 "%u," /* sentence version (always) */
182                 "%s,%s," /* nodeIdType/nodeId (always) */
183                 "%02u%02u%02u," /* date (always) */
184                 "%02u%02u%02u," /* time (always) */
185                 "%lu," /* validity time (always) */
186                 "%s,%s," /* latitude (optional) */
187                 "%s,%s," /* longitude (optional) */
188                 "%s," /* altitude (optional) */
189                 "%s," /* speed (optional) */
190                 "%s," /* track (optional) */
191                 "%s" /* hdop (optional) */
192         , getTxNmeaMessagePrefix(), PUD_TX_SENTENCE_VERSION, &nodeIdTypeString[0],
193                         nodeId, timeStruct.tm_mday, timeStruct.tm_mon, (timeStruct.tm_year
194                                         % 100), timeStruct.tm_hour, timeStruct.tm_min,
195                         timeStruct.tm_sec, validityTime, &latitudeString[0],
196                         latitudeHemisphere, &longitudeString[0], longitudeHemisphere,
197                         &altitudeString[0], &speedString[0], &trackString[0],
198                         &hdopString[0]);
199
200         if (unlikely(transmitStringLength > (txGpsBufferSize - 1))) {
201                 pudError(false, "String to transmit on non-OLSR is too large, need"
202                         " at least %u bytes, skipped", transmitStringLength);
203                 return 0;
204         }
205
206         if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
207                 txGpsBuffer[txGpsBufferSize - 1] = '\0';
208         } else {
209                 txGpsBuffer[transmitStringLength] = '\0';
210         }
211
212         return transmitStringLength;
213 }
214
215 /* ************************************************************************
216  * External --> OLSR
217  * ************************************************************************ */
218
219 /**
220  Convert a nmeaINFO structure into an OLSR message.
221
222  @param nmeaInfo
223  A pointer to a nmeaINFO structure
224  @param olsrMessage
225  A pointer to an OLSR message in which to place the converted information
226  @param olsrMessageSize
227  The maximum number of bytes available for the olsrMessage
228  @param validityTime
229  the validity time of the message
230
231  @return
232  - the aligned size of the converted information
233  - 0 (zero) in case of an error
234  */
235 unsigned int gpsToOlsr(nmeaINFO *nmeaInfo, union olsr_message *olsrMessage,
236                 unsigned int olsrMessageSize, unsigned long long validityTime) {
237         unsigned int aligned_size;
238         unsigned int aligned_size_remainder;
239         size_t nodeLength;
240         PudOlsrWireFormat * olsrGpsMessage =
241                         getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
242
243         /*
244          * Compose message contents
245          */
246
247         olsrGpsMessage->version = PUD_WIRE_FORMAT_VERSION;
248         olsrGpsMessage->validityTime = getValidityTimeForOlsr(validityTime);
249         olsrGpsMessage->smask = nmeaInfo->smask;
250
251         /* utc is always present, we make sure of that, so just use it */
252         olsrGpsMessage->gpsInfo.time = getTimeForOlsr(nmeaInfo->utc.hour,
253                         nmeaInfo->utc.min, nmeaInfo->utc.sec);
254
255         if (likely(nmea_INFO_has_field(nmeaInfo->smask, LAT))) {
256                 olsrGpsMessage->gpsInfo.lat = getLatitudeForOlsr(nmeaInfo->lat);
257         } else {
258                 olsrGpsMessage->gpsInfo.lat = (1 << (PUD_LATITUDE_BITS - 1));
259         }
260
261         if (likely(nmea_INFO_has_field(nmeaInfo->smask, LON))) {
262                 olsrGpsMessage->gpsInfo.lon = getLongitudeForOlsr(nmeaInfo->lon);
263         } else {
264                 olsrGpsMessage->gpsInfo.lon = (1 << (PUD_LONGITUDE_BITS - 1));
265         }
266
267         if (likely(nmea_INFO_has_field(nmeaInfo->smask, ELV))) {
268                 olsrGpsMessage->gpsInfo.alt = getAltitudeForOlsr(nmeaInfo->elv);
269         } else {
270                 olsrGpsMessage->gpsInfo.alt = -PUD_ALTITUDE_MIN;
271         }
272
273         if (likely(nmea_INFO_has_field(nmeaInfo->smask, SPEED))) {
274                 olsrGpsMessage->gpsInfo.speed = getSpeedForOlsr(nmeaInfo->speed);
275         } else {
276                 olsrGpsMessage->gpsInfo.speed = 0;
277         }
278
279         if (likely(nmea_INFO_has_field(nmeaInfo->smask, DIRECTION))) {
280                 olsrGpsMessage->gpsInfo.track = getTrackForOlsr(nmeaInfo->direction);
281         } else {
282                 olsrGpsMessage->gpsInfo.track = 0;
283         }
284
285         if (likely(nmea_INFO_has_field(nmeaInfo->smask, HDOP))) {
286                 olsrGpsMessage->gpsInfo.hdop = getHdopForOlsr(nmeaInfo->HDOP);
287         } else {
288                 olsrGpsMessage->gpsInfo.hdop = PUD_HDOP_MAX;
289         }
290
291         nodeLength = setupNodeInfoForOlsr(olsrGpsMessage, olsrMessageSize);
292
293         /*
294          * Messages in OLSR are 4-byte aligned: align
295          */
296
297         /* size = type, string, string terminator */
298         aligned_size = PUD_OLSRWIREFORMATSIZE + nodeLength;
299         aligned_size_remainder = (aligned_size % 4);
300         if (aligned_size_remainder != 0) {
301                 aligned_size += (4 - aligned_size_remainder);
302         }
303
304         /*
305          * Fill message headers (fill ALL fields, except message)
306          * Note: olsr_vtime is currently unused, we use it for our validity time.
307          */
308
309         if (olsr_cnf->ip_version == AF_INET) {
310                 /* IPv4 */
311
312                 olsrMessage->v4.olsr_msgtype = PUD_OLSR_MSG_TYPE;
313                 olsrMessage->v4.olsr_vtime = reltime_to_me(validityTime * 1000);
314                 /* message->v4.olsr_msgsize at the end */
315                 memcpy(&olsrMessage->v4.originator, &olsr_cnf->main_addr,
316                                 olsr_cnf->ipsize);
317                 olsrMessage->v4.ttl = getOlsrTtl();
318                 olsrMessage->v4.hopcnt = 0;
319                 olsrMessage->v4.seqno = htons(get_msg_seqno());
320
321                 /* add length of message->v4 fields */
322                 aligned_size += (sizeof(olsrMessage->v4)
323                                 - sizeof(olsrMessage->v4.message));
324                 olsrMessage->v4.olsr_msgsize = htons(aligned_size);
325         } else {
326                 /* IPv6 */
327
328                 olsrMessage->v6.olsr_msgtype = PUD_OLSR_MSG_TYPE;
329                 olsrMessage->v6.olsr_vtime = reltime_to_me(validityTime * 1000);
330                 /* message->v6.olsr_msgsize at the end */
331                 memcpy(&olsrMessage->v6.originator, &olsr_cnf->main_addr,
332                                 olsr_cnf->ipsize);
333                 olsrMessage->v6.ttl = getOlsrTtl();
334                 olsrMessage->v6.hopcnt = 0;
335                 olsrMessage->v6.seqno = htons(get_msg_seqno());
336
337                 /* add length of message->v6 fields */
338                 aligned_size += (sizeof(olsrMessage->v6)
339                                 - sizeof(olsrMessage->v6.message));
340                 olsrMessage->v6.olsr_msgsize = htons(aligned_size);
341         }
342
343         /* pad with zeroes */
344         if (aligned_size_remainder != 0) {
345                 memset(&(((char *) &olsrGpsMessage->nodeInfo.nodeIdType)[nodeLength]),
346                                 0, (4 - aligned_size_remainder));
347         }
348
349         return aligned_size;
350 }