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