lib/pud/src/gpsConversion.c

   1 /*
   2  * The olsr.org Optimized Link-State Routing daemon (olsrd)
   3  *
   4  * (c) by the OLSR project
   5  *
   6  * See our Git repository to find out who worked on this file
   7  * and thus is a copyright holder on it.
   8  *
   9  * All rights reserved.
  10  *
  11  * Redistribution and use in source and binary forms, with or without
  12  * modification, are permitted provided that the following conditions
  13  * are met:
  14  *
  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
  20  *   distribution.
  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.
  24  *
  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.
  37  *
  38  * Visit http://www.olsr.org for more information.
  39  *
  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.
  43  *
  44  */
  45
  46 #include "gpsConversion.h"
  47
  48 /* Plugin includes */
  49 #include "pud.h"
  50 #include "configuration.h"
  51 #include "compiler.h"
  52
  53 /* OLSR includes */
  54 #include "olsr.h"
  55
  56 /* System includes */
  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>
  62
  63 /* ************************************************************************
  64  * OLSR --> External
  65  * ************************************************************************ */
  66
  67 /**
  68  Convert an OLSR message into a string to multicast on the LAN
  69
  70  @param olsrMessage
  71  A pointer to the OLSR message
  72  @param txGpsBuffer
  73  A pointer to the buffer in which the transmit string can be written
  74  @param txGpsBufferSize
  75  The size of the txGpsBuffer
  76
  77  @return
  78  - the length of the transmit string placed in the txGpsBuffer
  79  - 0 (zero) in case of an error
  80  */
  81 unsigned int gpsFromOlsr(union olsr_message *olsrMessage,
  82                 unsigned char * txGpsBuffer, unsigned int txGpsBufferSize) {
  83         unsigned long validityTime;
  84
  85         struct tm timeStruct;
  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];
  94         uint32_t present;
  95         char gateway[2] = { '0', '\0' };
  96
  97         char nodeIdTypeString[PUD_TX_NODEIDTYPE_DIGITS];
  98         char nodeIdString[PUD_TX_NODEID_BUFFERSIZE];
  99         const char * nodeId;
 100         const void * ipAddr;
 101         char originatorBuffer[64];
 102         const char * originator;
 103
 104         unsigned int transmitStringLength;
 105
 106         PudOlsrPositionUpdate * olsrGpsMessage =
 107                         getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
 108
 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);
 114                 return 0;
 115         }
 116
 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));
 122
 123         validityTime = getValidityTime(&olsrGpsMessage->validityTime);
 124
 125         present = getPositionUpdatePresent(olsrGpsMessage);
 126
 127         if (present & PUD_PRESENT_GATEWAY) {
 128                 gateway[0] = '1';
 129         }
 130
 131         /* time is ALWAYS present so we can just use it */
 132         getPositionUpdateTime(olsrGpsMessage, time(NULL), &timeStruct);
 133
 134         if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_LAT))) {
 135                 double latitude = getPositionUpdateLatitude(olsrGpsMessage);
 136
 137                 if (latitude >= 0) {
 138                         latitudeHemisphere = "N";
 139                 } else {
 140                         latitudeHemisphere = "S";
 141                         latitude = -latitude;
 142                 }
 143                 latitude = nmeaMathDegreeToNdeg(latitude);
 144
 145                 snprintf(&latitudeString[0], PUD_TX_LATITUDE_DIGITS, "%." PUD_TX_LATITUDE_DECIMALS "f", latitude);
 146         } else {
 147                 latitudeHemisphere = "";
 148                 latitudeString[0] = '\0';
 149         }
 150
 151         if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_LON))) {
 152                 double longitude = getPositionUpdateLongitude(olsrGpsMessage);
 153
 154                 if (longitude >= 0) {
 155                         longitudeHemisphere = "E";
 156                 } else {
 157                         longitudeHemisphere = "W";
 158                         longitude = -longitude;
 159                 }
 160                 longitude = nmeaMathDegreeToNdeg(longitude);
 161
 162                 snprintf(&longitudeString[0], PUD_TX_LONGITUDE_DIGITS, "%." PUD_TX_LONGITUDE_DECIMALS "f", longitude);
 163         } else {
 164                 longitudeHemisphere = "";
 165                 longitudeString[0] = '\0';
 166         }
 167
 168         if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_ELV))) {
 169                 snprintf(&altitudeString[0], PUD_TX_ALTITUDE_DIGITS, "%ld", getPositionUpdateAltitude(olsrGpsMessage));
 170         } else {
 171                 altitudeString[0] = '\0';
 172         }
 173
 174         if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_SPEED))) {
 175                 snprintf(&speedString[0], PUD_TX_SPEED_DIGITS, "%lu", getPositionUpdateSpeed(olsrGpsMessage));
 176         } else {
 177                 speedString[0] = '\0';
 178         }
 179
 180         if (likely(nmeaInfoIsPresentAll(present, NMEALIB_PRESENT_TRACK))) {
 181                 snprintf(&trackString[0], PUD_TX_TRACK_DIGITS, "%lu", getPositionUpdateTrack(olsrGpsMessage));
 182         } else {
 183                 trackString[0] = '\0';
 184         }
 185
 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)));
 189         } else {
 190                 hdopString[0] = '\0';
 191         }
 192
 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));
 197
 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],
 220                         &hdopString[0]);
 221
 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);
 225                 return 0;
 226         }
 227
 228         if (unlikely(transmitStringLength == (txGpsBufferSize - 1))) {
 229                 txGpsBuffer[txGpsBufferSize - 1] = '\0';
 230         } else {
 231                 txGpsBuffer[transmitStringLength] = '\0';
 232         }
 233
 234         return transmitStringLength;
 235 }
 236
 237 /* ************************************************************************
 238  * External --> OLSR
 239  * ************************************************************************ */
 240
 241 /**
 242  Convert a nmeaINFO structure into an OLSR message.
 243
 244  @param nmeaInfo
 245  A pointer to a nmeaINFO structure
 246  @param olsrMessage
 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
 250  @param validityTime
 251  the validity time of the message in seconds
 252
 253  @return
 254  - the aligned size of the converted information
 255  - 0 (zero) in case of an error
 256  */
 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;
 261         size_t nodeLength;
 262         nodeIdBinaryType * nodeIdBinary = NULL;
 263
 264         PudOlsrPositionUpdate * olsrGpsMessage =
 265                         getOlsrMessagePayload(olsr_cnf->ip_version, olsrMessage);
 266
 267         /*
 268          * Compose message contents
 269          */
 270         memset(olsrGpsMessage, 0, sizeof (PudOlsrPositionUpdate));
 271
 272         setPositionUpdateVersion(olsrGpsMessage, PUD_WIRE_FORMAT_VERSION);
 273         setValidityTime(&olsrGpsMessage->validityTime, validityTime);
 274         setPositionUpdatePresent(olsrGpsMessage, nmeaInfo->present & ~PUD_PRESENT_GATEWAY);
 275
 276         /* utc is always present, we make sure of that elsewhere, so just use it */
 277         setPositionUpdateTime(olsrGpsMessage, nmeaInfo->utc.hour, nmeaInfo->utc.min,
 278                         nmeaInfo->utc.sec);
 279
 280         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_LAT))) {
 281                 setPositionUpdateLatitude(olsrGpsMessage, nmeaInfo->latitude);
 282         } else {
 283                 setPositionUpdateLatitude(olsrGpsMessage, 0.0);
 284         }
 285
 286         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_LON))) {
 287                 setPositionUpdateLongitude(olsrGpsMessage, nmeaInfo->longitude);
 288         } else {
 289                 setPositionUpdateLongitude(olsrGpsMessage, 0.0);
 290         }
 291
 292         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_ELV))) {
 293                 setPositionUpdateAltitude(olsrGpsMessage, nmeaInfo->elevation);
 294         } else {
 295                 setPositionUpdateAltitude(olsrGpsMessage, 0.0);
 296         }
 297
 298         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_SPEED))) {
 299                 setPositionUpdateSpeed(olsrGpsMessage, nmeaInfo->speed);
 300         } else {
 301                 setPositionUpdateSpeed(olsrGpsMessage, 0.0);
 302         }
 303
 304         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_TRACK))) {
 305                 setPositionUpdateTrack(olsrGpsMessage, nmeaInfo->track);
 306         } else {
 307                 setPositionUpdateTrack(olsrGpsMessage, 0);
 308         }
 309
 310         if (likely(nmeaInfoIsPresentAll(nmeaInfo->present, NMEALIB_PRESENT_HDOP))) {
 311                 setPositionUpdateHdop(olsrGpsMessage, nmeaInfo->hdop);
 312         } else {
 313                 setPositionUpdateHdop(olsrGpsMessage, PUD_HDOP_MAX);
 314         }
 315
 316         nodeIdBinary = getNodeIdBinary();
 317         nodeLength = setPositionUpdateNodeInfo(olsr_cnf->ip_version, olsrGpsMessage,
 318                         olsrMessageSize, getNodeIdTypeNumber(),
 319                         (unsigned char *) &nodeIdBinary->buffer, nodeIdBinary->length);
 320
 321         /*
 322          * Messages in OLSR are 4-byte aligned: align
 323          */
 324
 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);
 330         }
 331
 332         /*
 333          * Fill message headers (fill ALL fields, except message)
 334          * Note: olsr_vtime is currently unused, we use it for our validity time.
 335          */
 336
 337         if (olsr_cnf->ip_version == AF_INET) {
 338                 /* IPv4 */
 339
 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());
 347
 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);
 352         } else {
 353                 /* IPv6 */
 354
 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());
 362
 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);
 367         }
 368
 369         /* pad with zeroes */
 370         if (aligned_size_remainder != 0) {
 371                 memset(&(((char *) &olsrGpsMessage->nodeInfo.nodeIdType)[nodeLength]),
 372                                 0, (4 - aligned_size_remainder));
 373         }
 374
 375         return aligned_size;
 376 }