PUD: introduce wireFormat.c
[olsrd.git] / lib / pud / src / wireFormat.c
1 #include "wireFormat.h"
2
3 /* System includes */
4 #include <stdlib.h>
5 #include <stdbool.h>
6 #include <math.h>
7 #include <assert.h>
8
9 /* ************************************************************************
10  * DEFINES
11  * ************************************************************************ */
12
13 /** Compiler hint to expect x */
14 #ifndef likely
15 # if defined(__GNUC__)
16 #  define likely(x)                                     __builtin_expect((x),1)
17 # else
18 #  define likely(x)                                             (x)
19 # endif
20 #endif
21
22 /** Compiler hint to not expect x */
23 #ifndef unlikely
24 # if defined(__GNUC__)
25 #  define unlikely(x)                                   __builtin_expect((x),0)
26 # else
27 #  define unlikely(x)                                   (x)
28 # endif
29 #endif
30
31 /*
32  * GPS Information Conversion Functions For OLSR GPS Wire Format
33  */
34
35 /* ************************************************************************
36  * VALIDITY TIME
37  * ************************************************************************ */
38
39 /** Determine the validity time in seconds from the OLSR wire format value */
40 #define PUD_VALIDITY_TIME_FROM_OLSR(msn, lsn) ((((lsn) + 16) * (1 << (msn))) - 16)
41
42 static unsigned long long cachedValidityTimeMsn[16];
43
44 static bool cachedValidityTimeMsnValid = false;
45
46 /**
47  Setup of cache of calculated most significant nibble results of the validity
48  time calculation to speed up run-time calculations. This method has to be
49  called once upon first use of ValidityTime functions.
50  */
51 static void setupCachedValidityTimeMsn(void) {
52         unsigned int msn;
53         for (msn = 0; msn < 16; msn++) {
54                 cachedValidityTimeMsn[msn] = PUD_VALIDITY_TIME_FROM_OLSR(msn, 0);
55         }
56         cachedValidityTimeMsnValid = true;
57 }
58
59 /**
60  Convert the validity time to the validity time for an OLSR message
61
62  @param validityTime
63  The validity time (in seconds)
64
65  @return
66  The validity time converted to the format for the wire format
67  */
68 unsigned char getValidityTimeForOlsr(unsigned long long validityTime) {
69         unsigned int msn = 1;
70         unsigned long long lsn = 0;
71         unsigned long long upperBound;
72
73         if (!cachedValidityTimeMsnValid) {
74                 setupCachedValidityTimeMsn();
75         }
76         upperBound = cachedValidityTimeMsn[msn];
77         while ((msn < 16) && (validityTime >= upperBound)) {
78                 msn++;
79                 upperBound = cachedValidityTimeMsn[msn];
80         }
81         msn--;
82
83         if (unlikely(validityTime >= upperBound)) {
84                 lsn = 15;
85         } else {
86                 unsigned long lowerBound = PUD_VALIDITY_TIME_FROM_OLSR(msn, 0);
87                 unsigned long resolution = (1 << msn);
88                 lsn = ((validityTime - lowerBound + (resolution >> 1)) / resolution);
89         }
90
91         assert(msn <= 15);
92         assert(lsn <= 15);
93
94         return (msn << 4) | lsn;
95 }
96
97 /**
98  Convert the validity time of an OLSR message to the validity time for internal
99  use.
100
101  @param internal
102  The validity time as contained in the wire format
103
104  @return
105  The validity time converted to seconds
106  */
107 unsigned long getValidityTimeFromOlsr(unsigned char internal) {
108         return PUD_VALIDITY_TIME_FROM_OLSR(internal >> 4, internal % 16);
109 }
110
111 /* ************************************************************************
112  * TIME
113  * ************************************************************************ */
114
115 /**
116  Convert the time to the time for an OLSR message (the number of seconds after
117  midnight).
118
119  @param hour
120  The hours
121  @param min
122  The minutes
123  @param sec
124  The seconds
125
126  @return
127  The time converted to the format for the wire format
128  */
129 unsigned long getTimeForOlsr(int hour, int min, int sec) {
130         return ((hour * 60 * 60) + (min * 60) + sec);
131 }
132
133 /**
134  Convert the time of an OLSR message (the number of seconds after midnight) to
135  a time structure, based on midnight of the current day.
136
137  @param olsrTime
138  The time from the wire format
139  @param nowStruct
140  A pointer to the time structure into which to put the converted time
141  */
142 void getTimeFromOlsr(uint32_t olsrTime, struct tm *nowStruct) {
143         unsigned int secNow;
144
145         time_t now = time(NULL);
146         gmtime_r(&now, nowStruct);
147
148         secNow = ((nowStruct->tm_hour * 60 * 60) + (nowStruct->tm_min * 60)
149                         + nowStruct->tm_sec);
150
151         if (secNow <= (12 * 60 * 60)) {
152                 /* we are now in the first 12h of the day */
153                 if (unlikely(olsrTime > (secNow + (12 * 60 * 60)))) {
154                         /* the message was sent more than 12h later in time:
155                          the message was sent yesterday: adjust the date by -1 day */
156                         now -= (24 * 60 * 60);
157                         gmtime_r(&now, nowStruct);
158                 }
159         } else {
160                 /* we are now in the last 12h of the day */
161                 if (unlikely(olsrTime < (secNow - (12 * 60 * 60)))) {
162                         /* the message was sent more than 12h earlier in time:
163                          the message was sent tomorrow: adjust the date by +1 day */
164                         now += (24 * 60 * 60);
165                         gmtime_r(&now, nowStruct);
166                 }
167         }
168
169         nowStruct->tm_mon++;
170         nowStruct->tm_hour = ((olsrTime % (24 * 60 * 60)) / 3600);
171         nowStruct->tm_min = ((olsrTime % (60 * 60)) / 60);
172         nowStruct->tm_sec = (olsrTime % 60);
173 }
174
175 /* ************************************************************************
176  * LATITUDE
177  * ************************************************************************ */
178
179 /**
180  Convert the latitude of a nmeaINFO structure to the latitude for an OLSR
181  message
182
183  @param infoLat
184  The latitude as contained in an NMEA info structure (in degrees)
185
186  @return
187  The latitude converted to the format for the wire format
188  */
189 unsigned long getLatitudeForOlsr(double infoLat) {
190         double lat = infoLat;
191
192         /* lat is in [-90, 90] */
193         assert(lat >= -90.0);
194         assert(lat <= 90.0);
195
196         lat /= 180.0;
197         /* lat is now in [-0.5, 0.5] */
198
199         lat += 0.5;
200         /* lat is now in [0, 1] */
201
202         lat *= (double) (1 << PUD_LATITUDE_BITS);
203         /* lat is now in [0, LATITUDE_BITS] */
204
205         /* clip max */
206         if (unlikely(lat > (double)((1 << PUD_LATITUDE_BITS) - 1))) {
207                 lat = (double) ((1 << PUD_LATITUDE_BITS) - 1);
208         }
209         /* lat is now in [0, 2^LATITUDE_BITS> */
210
211         return lrint(lat);
212 }
213
214 /**
215  Convert the latitude of an OLSR message to the latitude for a nmeaINFO
216  structure
217
218  @param olsrLat
219  The latitude as contained in the wire format
220
221  @return
222  The latitude converted to the format for an NMEA info structure (in degrees)
223  */
224 double getLatitudeFromOlsr(uint32_t olsrLat) {
225         double lat = (double) olsrLat;
226
227         /* lat is in [0, 2^LATITUDE_BITS> */
228
229         /* take half of the rounding error */
230         lat += 0.5;
231
232         lat /= (double) (1 << PUD_LATITUDE_BITS);
233         /* lat is now in [0, 1> */
234
235         lat -= 0.5;
236         /* lat is now in [-0.5, 0.5> */
237
238         lat *= 180.0;
239         /* lat is now in [-90, 90> */
240
241         return lat;
242 }
243
244 /* ************************************************************************
245  * LONGITUDE
246  * ************************************************************************ */
247
248 /**
249  Convert the longitude of a nmeaINFO structure to the longitude for an OLSR
250  message
251
252  @param infoLon
253  The longitude as contained in an NMEA info structure (in degrees)
254
255  @return
256  The longitude converted to the format for the wire format
257  */
258 unsigned long getLongitudeForOlsr(double infoLon) {
259         double lon = infoLon;
260
261         /* lon is in [-180, 180] */
262         assert(lon >= -180.0);
263         assert(lon <= 180.0);
264
265         lon /= 360.0;
266         /* lon is now in [-0.5, 0.5] */
267
268         lon += 0.5;
269         /* lon is now in [0, 1] */
270
271         lon *= (double) (1 << PUD_LONGITUDE_BITS);
272         /* lon is now in [0, LONGITUDE_BITS] */
273
274         /* clip max */
275         if (unlikely(lon > (double)((1 << PUD_LATITUDE_BITS) - 1))) {
276                 lon = (double) ((1 << PUD_LATITUDE_BITS) - 1);
277         }
278
279         /* lon is now in [0, 2^LONGITUDE_BITS> */
280
281         return lrint(lon);
282 }
283
284 /**
285  Convert the longitude of an OLSR message to the longitude for a nmeaINFO
286  structure
287
288  @param olsrLon
289  The longitude as contained in the wire format
290
291  @return
292  The longitude converted to the format for an NMEA info structure (in degrees)
293  */
294 double getLongitudeFromOlsr(uint32_t olsrLon) {
295         double lon = (double) olsrLon;
296
297         /* lon is in [0, 2^LONGITUDE_BITS> */
298
299         /* take half of the rounding error */
300         lon += 0.5;
301
302         lon /= (1 << PUD_LONGITUDE_BITS);
303         /* lon is now in [0, 1> */
304
305         lon -= 0.5;
306         /* lon is now in [-0.5, 0.5> */
307
308         lon *= 360.0;
309         /* lon is now in [-180, 180> */
310
311         return lon;
312 }
313
314 /* ************************************************************************
315  * ALTITIDE
316  * ************************************************************************ */
317
318 /**
319  Convert the altitude of a nmeaINFO structure to the altitude for an OLSR
320  message
321
322  @param infoElv
323  The altitude as contained in an NMEA info structure
324
325  @return
326  The altitude converted to the format for the wire format
327  */
328 long getAltitudeForOlsr(double infoElv) {
329         double elv = infoElv;
330
331         if (unlikely(elv > PUD_ALTITUDE_MAX)) {
332                 elv = PUD_ALTITUDE_MAX;
333         } else if (unlikely(elv < PUD_ALTITUDE_MIN)) {
334                 elv = PUD_ALTITUDE_MIN;
335         }
336
337         elv -= PUD_ALTITUDE_MIN;
338
339         return lrint(elv);
340 }
341
342 /**
343  Convert the altitude of an OLSR message to the altitude for a nmeaINFO
344  structure
345
346  @param olsrAlt
347  The altitude as contained in the wire format
348
349  @return
350  The altitude converted to the format for an NMEA info structure
351  */
352 long getAltitudeFromOlsr(uint32_t olsrAlt) {
353         return (olsrAlt + PUD_ALTITUDE_MIN);
354 }
355
356 /* ************************************************************************
357  * SPEED
358  * ************************************************************************ */
359
360 /**
361  Convert the speed of a nmeaINFO structure to the speed for an OLSR message
362
363  @param infoSpeed
364  The speed as contained in an NMEA info structure
365
366  @return
367  The speed converted to the format for the wire format
368  */
369 long getSpeedForOlsr(double infoSpeed) {
370         if (unlikely(infoSpeed < 0)) {
371                 return 0;
372         }
373         if (unlikely(infoSpeed > PUD_SPEED_MAX)) {
374                 return PUD_SPEED_MAX;
375         }
376
377         return lrint(infoSpeed);
378 }
379
380 /**
381  Convert the speed of an OLSR message to the speed for a nmeaINFO structure
382
383  @param olsrSpeed
384  The speed as contained in the wire format
385
386  @return
387  The speed converted to the format for an NMEA info structure
388  */
389 unsigned long getSpeedFromOlsr(uint32_t olsrSpeed) {
390         return olsrSpeed;
391 }
392
393 /* ************************************************************************
394  * TRACK
395  * ************************************************************************ */
396
397 /**
398  Convert the track angle of a nmeaINFO structure to the track angle for an OLSR
399  message
400
401  @param infoTrack
402  The track angle as contained in an NMEA info structure
403
404  @return
405  The track angle converted to the format for the wire format
406  */
407 long getTrackForOlsr(double infoTrack) {
408         return lrint(infoTrack);
409 }
410
411 /**
412  Convert the track angle of an OLSR message to the track angle for a nmeaINFO
413  structure
414
415  @param olsrTrack
416  The track angle as contained in the wire format
417
418  @return
419  The track angle converted to the format for an NMEA info structure
420  */
421 unsigned long getTrackFromOlsr(uint32_t olsrTrack) {
422         return olsrTrack;
423 }
424
425 /* ************************************************************************
426  * HDOP
427  * ************************************************************************ */
428
429 /**
430  Convert the HDOP of a nmeaINFO structure to the HDOP for an OLSR message
431
432  @param infoHdop
433  The HDOP as contained in an NMEA info structure
434
435  @return
436  The HDOP converted to the format for the wire format
437  */
438 long getHdopForOlsr(double infoHdop) {
439         double infoHdopInternal = infoHdop;
440
441         if (unlikely(infoHdopInternal > PUD_HDOP_MAX)) {
442                 infoHdopInternal = PUD_HDOP_MAX;
443         }
444
445         return lrint(infoHdopInternal / PUD_HDOP_RESOLUTION);
446 }
447
448 /**
449  Convert the HDOP of an OLSR message to the HDOP for a nmeaINFO structure
450
451  @param olsrHdop
452  The HDOP as contained in the wire format
453
454  @return
455  The HDOP converted to the format for an NMEA info structure
456  */
457 double getHdopFromOlsr(uint32_t olsrHdop) {
458         return (olsrHdop * PUD_HDOP_RESOLUTION);
459 }