pseudonmea.c

   1 /*
   2  * This file is Copyright (c) 2010 by the GPSD project
   3  * BSD terms apply: see the file COPYING in the distribution root for details.
   4  */
   5 #include <stdlib.h>
   6 #include "gpsd_config.h"
   7 #include <sys/time.h>
   8 #ifdef HAVE_SYS_IOCTL_H
   9 #include <sys/ioctl.h>
  10 #endif /* HAVE_SYS_IOCTL_H */
  11 #ifndef S_SPLINT_S
  12 #ifdef HAVE_SYS_SOCKET_H
  13 #include <sys/socket.h>
  14 #endif /* HAVE_SYS_SOCKET_H */
  15 #include <unistd.h>
  16 #endif /* S_SPLINT_S */
  17 #include <sys/time.h>
  18 #include <stdio.h>
  19 #include <math.h>
  20 #ifndef S_SPLINT_S
  21 #ifdef HAVE_NETDB_H
  22 #include <netdb.h>
  23 #endif /* HAVE_NETDB_H */
  24 #endif /* S_SPLINT_S */
  25 #include <string.h>
  26 #include <errno.h>
  27 #include <fcntl.h>
  28
  29 #include "gpsd.h"
  30
  31 /*
  32  * Support for generic binary drivers.  These functions dump NMEA for passing
  33  * to the client in raw mode.  They assume that (a) the public gps.h structure
  34  * members are in a valid state, (b) that the private members hours, minutes,
  35  * and seconds have also been filled in, (c) that if the private member
  36  * mag_var is not NAN it is a magnetic variation in degrees that should be
  37  * passed on, and (d) if the private member separation does not have the
  38  * value NAN, it is a valid WGS84 geoidal separation in meters for the fix.
  39  */
  40
  41 static double degtodm(double angle)
  42 /* decimal degrees to GPS-style, degrees first followed by minutes */
  43 {
  44     double fraction, integer;
  45     fraction = modf(angle, &integer);
  46     return floor(angle) * 100 + fraction * 60;
  47 }
  48
  49 /*@ -mustdefine @*/
  50 void gpsd_position_fix_dump(struct gps_device_t *session,
  51                             /*@out@*/ char bufp[], size_t len)
  52 {
  53     struct tm tm;
  54     time_t intfixtime;
  55
  56     intfixtime = (time_t) session->gpsdata.fix.time;
  57     (void)gmtime_r(&intfixtime, &tm);
  58     if (session->gpsdata.fix.mode > 1) {
  59         (void)snprintf(bufp, len,
  60                        "$GPGGA,%02d%02d%02d,%09.4f,%c,%010.4f,%c,%d,%02d,",
  61                        tm.tm_hour,
  62                        tm.tm_min,
  63                        tm.tm_sec,
  64                        degtodm(fabs(session->gpsdata.fix.latitude)),
  65                        ((session->gpsdata.fix.latitude > 0) ? 'N' : 'S'),
  66                        degtodm(fabs(session->gpsdata.fix.longitude)),
  67                        ((session->gpsdata.fix.longitude > 0) ? 'E' : 'W'),
  68                        session->gpsdata.status,
  69                        session->gpsdata.satellites_used);
  70         if (isnan(session->gpsdata.dop.hdop))
  71             (void)strlcat(bufp, ",", len);
  72         else
  73             (void)snprintf(bufp + strlen(bufp), len - strlen(bufp),
  74                            "%.2f,", session->gpsdata.dop.hdop);
  75         if (isnan(session->gpsdata.fix.altitude))
  76             (void)strlcat(bufp, ",", len);
  77         else
  78             (void)snprintf(bufp + strlen(bufp), len - strlen(bufp),
  79                            "%.2f,M,", session->gpsdata.fix.altitude);
  80         if (isnan(session->gpsdata.separation))
  81             (void)strlcat(bufp, ",", len);
  82         else
  83             (void)snprintf(bufp + strlen(bufp), len - strlen(bufp),
  84                            "%.3f,M,", session->gpsdata.separation);
  85         if (isnan(session->mag_var))
  86             (void)strlcat(bufp, ",", len);
  87         else {
  88             (void)snprintf(bufp + strlen(bufp),
  89                            len - strlen(bufp),
  90                            "%3.2f,", fabs(session->mag_var));
  91             (void)strlcat(bufp, (session->mag_var > 0) ? "E" : "W", len);
  92         }
  93         nmea_add_checksum(bufp);
  94     }
  95 }
  96
  97 /*@ +mustdefine @*/
  98
  99 static void gpsd_transit_fix_dump(struct gps_device_t *session,
 100                                   char bufp[], size_t len)
 101 {
 102     struct tm tm;
 103     time_t intfixtime;
 104
 105     tm.tm_mday = tm.tm_mon = tm.tm_year = tm.tm_hour = tm.tm_min = tm.tm_sec =
 106         0;
 107     if (isnan(session->gpsdata.fix.time) == 0) {
 108         intfixtime = (time_t) session->gpsdata.fix.time;
 109         (void)gmtime_r(&intfixtime, &tm);
 110         tm.tm_mon++;
 111         tm.tm_year %= 100;
 112     }
 113 #define ZEROIZE(x)      (isnan(x)!=0 ? 0.0 : x)
 114     /*@ -usedef @*/
 115     (void)snprintf(bufp, len,
 116                    "$GPRMC,%02d%02d%02d,%c,%09.4f,%c,%010.4f,%c,%.4f,%.3f,%02d%02d%02d,,",
 117                    tm.tm_hour,
 118                    tm.tm_min,
 119                    tm.tm_sec,
 120                    session->gpsdata.status ? 'A' : 'V',
 121                    ZEROIZE(degtodm(fabs(session->gpsdata.fix.latitude))),
 122                    ((session->gpsdata.fix.latitude > 0) ? 'N' : 'S'),
 123                    ZEROIZE(degtodm(fabs(session->gpsdata.fix.longitude))),
 124                    ((session->gpsdata.fix.longitude > 0) ? 'E' : 'W'),
 125                    ZEROIZE(session->gpsdata.fix.speed * MPS_TO_KNOTS),
 126                    ZEROIZE(session->gpsdata.fix.track),
 127                    tm.tm_mday, tm.tm_mon, tm.tm_year);
 128     /*@ +usedef @*/
 129 #undef ZEROIZE
 130     nmea_add_checksum(bufp);
 131 }
 132
 133 static void gpsd_binary_satellite_dump(struct gps_device_t *session,
 134                                        char bufp[], size_t len)
 135 {
 136     int i;
 137     char *bufp2 = bufp;
 138     bufp[0] = '\0';
 139
 140     for (i = 0; i < session->gpsdata.satellites_visible; i++) {
 141         if (i % 4 == 0) {
 142             bufp += strlen(bufp);
 143             bufp2 = bufp;
 144             len -= snprintf(bufp, len,
 145                             "$GPGSV,%d,%d,%02d",
 146                             ((session->gpsdata.satellites_visible - 1) / 4) +
 147                             1, (i / 4) + 1,
 148                             session->gpsdata.satellites_visible);
 149         }
 150         bufp += strlen(bufp);
 151         if (i < session->gpsdata.satellites_visible)
 152             len -= snprintf(bufp, len,
 153                             ",%02d,%02d,%03d,%02.0f",
 154                             session->gpsdata.PRN[i],
 155                             session->gpsdata.elevation[i],
 156                             session->gpsdata.azimuth[i],
 157                             session->gpsdata.ss[i]);
 158         if (i % 4 == 3 || i == session->gpsdata.satellites_visible - 1) {
 159             nmea_add_checksum(bufp2);
 160             len -= 5;
 161         }
 162     }
 163
 164 #ifdef ZODIAC_ENABLE
 165     if (session->packet.type == ZODIAC_PACKET
 166         && session->driver.zodiac.Zs[0] != 0) {
 167         bufp += strlen(bufp);
 168         bufp2 = bufp;
 169         (void)strlcpy(bufp, "$PRWIZCH", len);
 170         for (i = 0; i < ZODIAC_CHANNELS; i++) {
 171             len -= snprintf(bufp + strlen(bufp), len,
 172                             ",%02u,%X",
 173                             session->driver.zodiac.Zs[i],
 174                             session->driver.zodiac.Zv[i] & 0x0f);
 175         }
 176         nmea_add_checksum(bufp2);
 177     }
 178 #endif /* ZODIAC_ENABLE */
 179 }
 180
 181 static void gpsd_binary_quality_dump(struct gps_device_t *session,
 182                                      char bufp[], size_t len)
 183 {
 184     int i, j;
 185     char *bufp2 = bufp;
 186     bool used_valid = (session->gpsdata.set & USED_IS) != 0;
 187
 188     if (session->device_type != NULL && (session->gpsdata.set & MODE_IS) != 0) {
 189         (void)snprintf(bufp, len - strlen(bufp),
 190                        "$GPGSA,%c,%d,", 'A', session->gpsdata.fix.mode);
 191         j = 0;
 192         for (i = 0; i < session->device_type->channels; i++) {
 193             if (session->gpsdata.used[i]) {
 194                 bufp += strlen(bufp);
 195                 (void)snprintf(bufp, len - strlen(bufp),
 196                                "%02d,",
 197                                used_valid ? session->gpsdata.used[i] : 0);
 198                 j++;
 199             }
 200         }
 201         for (i = j; i < session->device_type->channels; i++) {
 202             bufp += strlen(bufp);
 203             (void)strlcpy(bufp, ",", len);
 204         }
 205         bufp += strlen(bufp);
 206 #define ZEROIZE(x)      (isnan(x)!=0 ? 0.0 : x)
 207         if (session->gpsdata.fix.mode == MODE_NO_FIX)
 208             (void)strlcat(bufp, ",,,", len);
 209         else
 210             (void)snprintf(bufp, len - strlen(bufp),
 211                            "%.1f,%.1f,%.1f*",
 212                            ZEROIZE(session->gpsdata.dop.pdop),
 213                            ZEROIZE(session->gpsdata.dop.hdop),
 214                            ZEROIZE(session->gpsdata.dop.vdop));
 215         nmea_add_checksum(bufp2);
 216         bufp += strlen(bufp);
 217     }
 218     if (finite(session->gpsdata.fix.epx)
 219         && finite(session->gpsdata.fix.epy)
 220         && finite(session->gpsdata.fix.epv)
 221         && finite(session->gpsdata.epe)) {
 222         struct tm tm;
 223         time_t intfixtime;
 224
 225         tm.tm_hour = tm.tm_min = tm.tm_sec = 0;
 226         if (isnan(session->gpsdata.fix.time) == 0) {
 227             intfixtime = (time_t) session->gpsdata.fix.time;
 228             (void)gmtime_r(&intfixtime, &tm);
 229         }
 230         (void)snprintf(bufp, len - strlen(bufp),
 231                        "$GPGBS,%02d%02d%02d,%.2f,M,%.2f,M,%.2f,M",
 232                        tm.tm_hour, tm.tm_min, tm.tm_sec,
 233                        ZEROIZE(session->gpsdata.fix.epx),
 234                        ZEROIZE(session->gpsdata.fix.epy),
 235                        ZEROIZE(session->gpsdata.fix.epv));
 236         nmea_add_checksum(bufp);
 237     }
 238 #undef ZEROIZE
 239 }
 240
 241 /*@-compdef -mustdefine@*/
 242 /* *INDENT-OFF* */
 243 void nmea_tpv_dump(struct gps_device_t *session,
 244                    /*@out@*/ char bufp[], size_t len)
 245 {
 246     bufp[0] = '\0';
 247     if ((session->gpsdata.set & LATLON_IS) != 0) {
 248         gpsd_position_fix_dump(session, bufp, len);
 249         gpsd_transit_fix_dump(session, bufp + strlen(bufp),
 250                               len - strlen(bufp));
 251     }
 252     if ((session->gpsdata.set
 253          & (MODE_IS | DOP_IS | USED_IS | HERR_IS | VERR_IS)) != 0)
 254         gpsd_binary_quality_dump(session, bufp + strlen(bufp),
 255                                  len - strlen(bufp));
 256 }
 257 /* *INDENT-ON* */
 258
 259 void nmea_sky_dump(struct gps_device_t *session,
 260                    /*@out@*/ char bufp[], size_t len)
 261 {
 262     bufp[0] = '\0';
 263     if ((session->gpsdata.set & SATELLITE_IS) != 0)
 264         gpsd_binary_satellite_dump(session, bufp + strlen(bufp),
 265                                    len - strlen(bufp));
 266 }
 267
 268 /*@+compdef +mustdefine@*/
 269
 270 /* pseudonmea.c ends here */