2 * The olsr.org Optimized Link-State Routing daemon (olsrd)
4 * (c) by the OLSR project
6 * See our Git repository to find out who worked on this file
7 * and thus is a copyright holder on it.
11 * Redistribution and use in source and binary forms, with or without
12 * modification, are permitted provided that the following conditions
15 * * Redistributions of source code must retain the above copyright
16 * notice, this list of conditions and the following disclaimer.
17 * * Redistributions in binary form must reproduce the above copyright
18 * notice, this list of conditions and the following disclaimer in
19 * the documentation and/or other materials provided with the
21 * * Neither the name of olsr.org, olsrd nor the names of its
22 * contributors may be used to endorse or promote products derived
23 * from this software without specific prior written permission.
25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36 * POSSIBILITY OF SUCH DAMAGE.
38 * Visit http://www.olsr.org for more information.
40 * If you find this software useful feel free to make a donation
41 * to the project. For more information see the website or contact
42 * the copyright holders.
46 #include "gpsConversion.h"
50 #include "configuration.h"
57 #include <nmealib/info.h>
58 #include <nmealib/nmath.h>
59 #include <nmealib/util.h>
60 #include <arpa/inet.h>
61 #include <OlsrdPudWireFormat/nodeIdConversion.h>
63 /* ************************************************************************
65 * ************************************************************************ */
68 Convert an OLSR message into a string to multicast on the LAN
71 A pointer to the OLSR message
73 A pointer to the buffer in which the transmit string can be written
74 @param txGpsBufferSize
75 The size of the txGpsBuffer
78 - the length of the transmit string placed in the txGpsBuffer
79 - 0 (zero) in case of an error
81 unsigned int gpsFromOlsr(union olsr_message *olsrMessage,
82 unsigned char * txGpsBuffer, unsigned int txGpsBufferSize) {
83 unsigned long validityTime;
86 char latitudeString[PUD_TX_LATITUDE_DIGITS];
87 const char * latitudeHemisphere;
88 char longitudeString[PUD_TX_LONGITUDE_DIGITS];
89 const char * longitudeHemisphere;
90 char altitudeString[PUD_TX_ALTITUDE_DIGITS];
91 char speedString[PUD_TX_SPEED_DIGITS];
92 char trackString[PUD_TX_TRACK_DIGITS];
93 char hdopString[PUD_TX_HDOP_DIGITS];
95 char gateway[2] = { '0', '\0' };
97 char nodeIdTypeString[PUD_TX_NODEIDTYPE_DIGITS];
98 char nodeIdString[PUD_TX_NODEID_BUFFERSIZE];
101 char originatorBuffer[64];
102 const char * originator;
104 unsigned int transmitStringLength;
106 PudOlsrPositionUpdate * olsrGpsMessage =
107 getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
109 if (unlikely(getPositionUpdateVersion(olsrGpsMessage) != PUD_WIRE_FORMAT_VERSION)) {
110 /* currently we can only handle our own version */
111 pudError(false, "Can not handle version %u OLSR PUD messages"
112 " (only version %u): message ignored",
113 getPositionUpdateVersion(olsrGpsMessage), PUD_WIRE_FORMAT_VERSION);
117 ipAddr = (olsr_cnf->ip_version == AF_INET) ?
118 (void *) &olsrMessage->v4.originator :
119 (void *) &olsrMessage->v6.originator;
120 originator = inet_ntop(olsr_cnf->ip_version, ipAddr, &originatorBuffer[0],
121 sizeof(originatorBuffer));
123 validityTime = getValidityTime(&olsrGpsMessage->validityTime);
125 present = getPositionUpdatePresent(olsrGpsMessage);
127 if (present & PUD_PRESENT_GATEWAY) {
131 /* time is ALWAYS present so we can just use it */
132 getPositionUpdateTime(olsrGpsMessage, time(NULL), &timeStruct);
134 if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_LAT))) {
135 double latitude = getPositionUpdateLatitude(olsrGpsMessage);
138 latitudeHemisphere = "N";
140 latitudeHemisphere = "S";
141 latitude = -latitude;
143 latitude = nmeaMathDegreeToNdeg(latitude);
145 snprintf(&latitudeString[0], PUD_TX_LATITUDE_DIGITS, "%." PUD_TX_LATITUDE_DECIMALS "f", latitude);
147 latitudeHemisphere = "";
148 latitudeString[0] = '\0';
151 if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_LON))) {
152 double longitude = getPositionUpdateLongitude(olsrGpsMessage);
154 if (longitude >= 0) {
155 longitudeHemisphere = "E";
157 longitudeHemisphere = "W";
158 longitude = -longitude;
160 longitude = nmeaMathDegreeToNdeg(longitude);
162 snprintf(&longitudeString[0], PUD_TX_LONGITUDE_DIGITS, "%." PUD_TX_LONGITUDE_DECIMALS "f", longitude);
164 longitudeHemisphere = "";
165 longitudeString[0] = '\0';
168 if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_ELV))) {
169 snprintf(&altitudeString[0], PUD_TX_ALTITUDE_DIGITS, "%ld", getPositionUpdateAltitude(olsrGpsMessage));
171 altitudeString[0] = '\0';
174 if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_SPEED))) {
175 snprintf(&speedString[0], PUD_TX_SPEED_DIGITS, "%lu", getPositionUpdateSpeed(olsrGpsMessage));
177 speedString[0] = '\0';
180 if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_TRACK))) {
181 snprintf(&trackString[0], PUD_TX_TRACK_DIGITS, "%lu", getPositionUpdateTrack(olsrGpsMessage));
183 trackString[0] = '\0';
186 if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_HDOP))) {
187 snprintf(&hdopString[0], PUD_TX_HDOP_DIGITS, "%." PUD_TX_HDOP_DECIMALS "f",
188 nmeaMathMetersToDop(getPositionUpdateHdop(olsrGpsMessage)));
190 hdopString[0] = '\0';
193 getNodeTypeStringFromOlsr(olsr_cnf->ip_version, olsrGpsMessage,
194 &nodeIdTypeString[0], sizeof(nodeIdTypeString));
195 getNodeIdStringFromOlsr(olsr_cnf->ip_version, olsrMessage, &nodeId,
196 &nodeIdString[0], sizeof(nodeIdString));
198 transmitStringLength = nmeaPrintf((char *) txGpsBuffer, txGpsBufferSize
199 - 1, "$P%s," /* prefix (always) */
200 "%u," /* sentence version (always) */
201 "%s," /* gateway flag (always) */
202 "%s," /* OLSR originator (always) */
203 "%s,%s," /* nodeIdType/nodeId (always) */
204 "%02u%02u%02u," /* date (always) */
205 "%02u%02u%02u," /* time (always) */
206 "%lu," /* validity time (always) */
207 "%s,%s," /* latitude (optional) */
208 "%s,%s," /* longitude (optional) */
209 "%s," /* altitude (optional) */
210 "%s," /* speed (optional) */
211 "%s," /* track (optional) */
212 "%s" /* hdop (optional) */
213 , getTxNmeaMessagePrefix(), PUD_TX_SENTENCE_VERSION, &gateway[0],
214 originator , &nodeIdTypeString[0],
215 nodeId, timeStruct.tm_mday, timeStruct.tm_mon + 1, (timeStruct.tm_year
216 % 100), timeStruct.tm_hour, timeStruct.tm_min,
217 timeStruct.tm_sec, validityTime, &latitudeString[0],
218 latitudeHemisphere, &longitudeString[0], longitudeHemisphere,
219 &altitudeString[0], &speedString[0], &trackString[0],
222 if (unlikely(transmitStringLength > (txGpsBufferSize - 1))) {
223 pudError(false, "String to transmit on non-OLSR is too large, need"
224 " at least %u bytes, skipped", transmitStringLength);
228 if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
229 txGpsBuffer[txGpsBufferSize - 1] = '\0';
231 txGpsBuffer[transmitStringLength] = '\0';
234 return transmitStringLength;
237 /* ************************************************************************
239 * ************************************************************************ */
242 Convert a nmeaINFO structure into an OLSR message.
245 A pointer to a nmeaINFO structure
247 A pointer to an OLSR message in which to place the converted information
248 @param olsrMessageSize
249 The maximum number of bytes available for the olsrMessage
251 the validity time of the message in seconds
254 - the aligned size of the converted information
255 - 0 (zero) in case of an error
257 unsigned int gpsToOlsr(NmeaInfo *nmeaInfo, union olsr_message *olsrMessage,
258 unsigned int olsrMessageSize, unsigned long long validityTime) {
259 unsigned int aligned_size;
260 unsigned int aligned_size_remainder;
262 nodeIdBinaryType * nodeIdBinary = NULL;
264 PudOlsrPositionUpdate * olsrGpsMessage =
265 getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
268 * Compose message contents
270 memset(olsrGpsMessage, 0, sizeof (PudOlsrPositionUpdate));
272 setPositionUpdateVersion(olsrGpsMessage, PUD_WIRE_FORMAT_VERSION);
273 setValidityTime(&olsrGpsMessage->validityTime, validityTime);
274 setPositionUpdatePresent(olsrGpsMessage, nmeaInfo->present & ~PUD_PRESENT_GATEWAY);
276 /* utc is always present, we make sure of that elsewhere, so just use it */
277 setPositionUpdateTime(olsrGpsMessage, nmeaInfo->utc.hour, nmeaInfo->utc.min,
280 if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_LAT))) {
281 setPositionUpdateLatitude(olsrGpsMessage, nmeaInfo->latitude);
283 setPositionUpdateLatitude(olsrGpsMessage, 0.0);
286 if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_LON))) {
287 setPositionUpdateLongitude(olsrGpsMessage, nmeaInfo->longitude);
289 setPositionUpdateLongitude(olsrGpsMessage, 0.0);
292 if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_ELV))) {
293 setPositionUpdateAltitude(olsrGpsMessage, nmeaInfo->elevation);
295 setPositionUpdateAltitude(olsrGpsMessage, 0.0);
298 if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_SPEED))) {
299 setPositionUpdateSpeed(olsrGpsMessage, nmeaInfo->speed);
301 setPositionUpdateSpeed(olsrGpsMessage, 0.0);
304 if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_TRACK))) {
305 setPositionUpdateTrack(olsrGpsMessage, nmeaInfo->track);
307 setPositionUpdateTrack(olsrGpsMessage, 0);
310 if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_HDOP))) {
311 setPositionUpdateHdop(olsrGpsMessage, nmeaInfo->hdop);
313 setPositionUpdateHdop(olsrGpsMessage, PUD_HDOP_MAX);
316 nodeIdBinary = getNodeIdBinary();
317 nodeLength = setPositionUpdateNodeInfo(olsr_cnf->ip_version, olsrGpsMessage,
318 olsrMessageSize, getNodeIdTypeNumber(),
319 (unsigned char *) &nodeIdBinary->buffer, nodeIdBinary->length);
322 * Messages in OLSR are 4-byte aligned: align
325 /* size = type, string, string terminator */
326 aligned_size = PUD_OLSRWIREFORMATSIZE + nodeLength;
327 aligned_size_remainder = (aligned_size % 4);
328 if (aligned_size_remainder != 0) {
329 aligned_size += (4 - aligned_size_remainder);
333 * Fill message headers (fill ALL fields, except message)
334 * Note: olsr_vtime is currently unused, we use it for our validity time.
337 if (olsr_cnf->ip_version == AF_INET) {
340 olsrMessage->v4.olsr_msgtype = PUD_OLSR_MSG_TYPE;
341 olsrMessage->v4.olsr_vtime = reltime_to_me(validityTime * 1000);
342 /* message->v4.olsr_msgsize at the end */
343 olsrMessage->v4.originator = olsr_cnf->main_addr.v4.s_addr;
344 olsrMessage->v4.ttl = getOlsrTtl();
345 olsrMessage->v4.hopcnt = 0;
346 olsrMessage->v4.seqno = htons(get_msg_seqno());
348 /* add length of message->v4 fields */
349 aligned_size += (sizeof(olsrMessage->v4)
350 - sizeof(olsrMessage->v4.message));
351 olsrMessage->v4.olsr_msgsize = htons(aligned_size);
355 olsrMessage->v6.olsr_msgtype = PUD_OLSR_MSG_TYPE;
356 olsrMessage->v6.olsr_vtime = reltime_to_me(validityTime * 1000);
357 /* message->v6.olsr_msgsize at the end */
358 olsrMessage->v6.originator = olsr_cnf->main_addr.v6;
359 olsrMessage->v6.ttl = getOlsrTtl();
360 olsrMessage->v6.hopcnt = 0;
361 olsrMessage->v6.seqno = htons(get_msg_seqno());
363 /* add length of message->v6 fields */
364 aligned_size += (sizeof(olsrMessage->v6)
365 - sizeof(olsrMessage->v6.message));
366 olsrMessage->v6.olsr_msgsize = htons(aligned_size);
369 /* pad with zeroes */
370 if (aligned_size_remainder != 0) {
371 memset(&(((char *) &olsrGpsMessage->nodeInfo.nodeIdType)[nodeLength]),
372 0, (4 - aligned_size_remainder));