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