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