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