gpsutils.c

   1 /* gpsutils.c -- code shared between low-level and high-level interfaces
   2  *
   3  * This file is Copyright (c) 2010 by the GPSD project
   4  * BSD terms apply: see the file COPYING in the distribution root for details.
   5  */
   6 #include <sys/types.h>
   7 #include <stdio.h>
   8 #ifndef S_SPLINT_S
   9 #include <unistd.h>
  10 #endif /* S_SPLINT_S */
  11 #include <stdlib.h>
  12 #include <math.h>
  13 #include <string.h>
  14 #include <stdarg.h>
  15 #include <time.h>
  16
  17 #include "gpsd.h"
  18
  19 #ifdef USE_QT
  20 #include <QDateTime>
  21 #include <QStringList>
  22 #endif
  23
  24 #define MONTHSPERYEAR   12      /* months per calendar year */
  25
  26 void gps_clear_fix( /*@out@*/ struct gps_fix_t *fixp)
  27 /* stuff a fix structure with recognizable out-of-band values */
  28 {
  29     fixp->time = NAN;
  30     fixp->mode = MODE_NOT_SEEN;
  31     fixp->latitude = fixp->longitude = NAN;
  32     fixp->track = NAN;
  33     fixp->speed = NAN;
  34     fixp->climb = NAN;
  35     fixp->altitude = NAN;
  36     fixp->ept = NAN;
  37     fixp->epx = NAN;
  38     fixp->epy = NAN;
  39     fixp->epv = NAN;
  40     fixp->epd = NAN;
  41     fixp->eps = NAN;
  42     fixp->epc = NAN;
  43 }
  44
  45 void gps_merge_fix( /*@ out @*/ struct gps_fix_t *to,
  46                    gps_mask_t transfer,
  47                    /*@ in @*/ struct gps_fix_t *from)
  48 /* merge new data into an old fix */
  49 {
  50     if ((NULL == to) || (NULL == from))
  51         return;
  52     if ((transfer & TIME_IS) != 0)
  53         to->time = from->time;
  54     if ((transfer & LATLON_IS) != 0) {
  55         to->latitude = from->latitude;
  56         to->longitude = from->longitude;
  57     }
  58     if ((transfer & MODE_IS) != 0)
  59         to->mode = from->mode;
  60     if ((transfer & ALTITUDE_IS) != 0)
  61         to->altitude = from->altitude;
  62     if ((transfer & TRACK_IS) != 0)
  63         to->track = from->track;
  64     if ((transfer & SPEED_IS) != 0)
  65         to->speed = from->speed;
  66     if ((transfer & CLIMB_IS) != 0)
  67         to->climb = from->climb;
  68     if ((transfer & TIMERR_IS) != 0)
  69         to->ept = from->ept;
  70     if ((transfer & HERR_IS) != 0) {
  71         to->epx = from->epx;
  72         to->epy = from->epy;
  73     }
  74     if ((transfer & VERR_IS) != 0)
  75         to->epv = from->epv;
  76     if ((transfer & SPEEDERR_IS) != 0)
  77         to->eps = from->eps;
  78 }
  79
  80 double timestamp(void)
  81 {
  82     struct timeval tv;
  83     (void)gettimeofday(&tv, NULL);
  84     /*@i1@*/ return (tv.tv_sec + tv.tv_usec * 1e-6);
  85 }
  86
  87 time_t mkgmtime(register struct tm * t)
  88 /* struct tm to seconds since Unix epoch */
  89 {
  90     register int year;
  91     register time_t result;
  92     static const int cumdays[MONTHSPERYEAR] =
  93         { 0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334 };
  94
  95     /*@ +matchanyintegral @*/
  96     year = 1900 + t->tm_year + t->tm_mon / MONTHSPERYEAR;
  97     result = (year - 1970) * 365 + cumdays[t->tm_mon % MONTHSPERYEAR];
  98     result += (year - 1968) / 4;
  99     result -= (year - 1900) / 100;
 100     result += (year - 1600) / 400;
 101     if ((year % 4) == 0 && ((year % 100) != 0 || (year % 400) == 0) &&
 102         (t->tm_mon % MONTHSPERYEAR) < 2)
 103         result--;
 104     result += t->tm_mday - 1;
 105     result *= 24;
 106     result += t->tm_hour;
 107     result *= 60;
 108     result += t->tm_min;
 109     result *= 60;
 110     result += t->tm_sec;
 111     /*@ -matchanyintegral @*/
 112     return (result);
 113 }
 114
 115 double iso8601_to_unix( /*@in@*/ char *isotime)
 116 /* ISO8601 UTC to Unix UTC */
 117 {
 118 #ifndef USE_QT
 119     char *dp = NULL;
 120     double usec;
 121     struct tm tm;
 122
 123     /*@i1@*/ dp = strptime(isotime, "%Y-%m-%dT%H:%M:%S", &tm);
 124     if (dp != NULL && *dp == '.')
 125         usec = strtod(dp, NULL);
 126     else
 127         usec = 0;
 128     return (double)mkgmtime(&tm) + usec;
 129 #else
 130     double usec = 0;
 131
 132     QString t(isotime);
 133     QDateTime d = QDateTime::fromString(isotime, Qt::ISODate);
 134     QStringList sl = t.split(".");
 135     if (sl.size() > 1)
 136         usec = sl[1].toInt() / pow(10., (double)sl[1].size());
 137     return d.toTime_t() + usec;
 138 #endif
 139 }
 140
 141 /* *INDENT-OFF* */
 142 /*@observer@*/char *unix_to_iso8601(double fixtime, /*@ out @*/
 143                                      char isotime[], size_t len)
 144 /* Unix UTC time to ISO8601, no timezone adjustment */
 145 /* example: 2007-12-11T23:38:51.0Z */
 146 {
 147     struct tm when;
 148     double integral, fractional;
 149     time_t intfixtime;
 150     char timestr[30];
 151     char fractstr[10];
 152
 153     fractional = modf(fixtime, &integral);
 154     intfixtime = (time_t) integral;
 155     (void)gmtime_r(&intfixtime, &when);
 156
 157     (void)strftime(timestr, sizeof(timestr), "%Y-%m-%dT%H:%M:%S", &when);
 158     (void)snprintf(fractstr, sizeof(fractstr), "%.1f", fractional);
 159     /* add fractional part, ignore leading 0; "0.2" -> ".2" */
 160     (void)snprintf(isotime, len, "%s%sZ", timestr, fractstr + 1);
 161     return isotime;
 162 }
 163 /* *INDENT-ON* */
 164
 165 /*
 166  * The 'week' part of GPS dates are specified in weeks since 0000 on 06
 167  * January 1980, with a rollover at 1024.  At time of writing the last
 168  * rollover happened at 0000 22 August 1999.  Time-of-week is in seconds.
 169  *
 170  * This code copes with both conventional GPS weeks and the "extended"
 171  * 15-or-16-bit version with no wraparound that appears in Zodiac
 172  * chips and is supposed to appear in the Geodetic Navigation
 173  * Information (0x29) packet of SiRF chips.  Some SiRF firmware versions
 174  * (notably 231) actually ship the wrapped 10-bit week, despite what
 175  * the protocol reference claims.
 176  *
 177  * Note: This time will need to be corrected for leap seconds.
 178  */
 179 #define GPS_EPOCH       315964800       /* GPS epoch in Unix time */
 180 #define SECS_PER_WEEK   (60*60*24*7)    /* seconds per week */
 181 #define GPS_ROLLOVER    (1024*SECS_PER_WEEK)    /* rollover period */
 182
 183 double gpstime_to_unix(int week, double tow)
 184 {
 185     double fixtime;
 186
 187     if (week >= 1024)
 188         fixtime = GPS_EPOCH + (week * SECS_PER_WEEK) + tow;
 189     else {
 190         time_t now, last_rollover;
 191         (void)time(&now);
 192         last_rollover =
 193             GPS_EPOCH + ((now - GPS_EPOCH) / GPS_ROLLOVER) * GPS_ROLLOVER;
 194         /*@i@*/ fixtime = last_rollover + (week * SECS_PER_WEEK) + tow;
 195     }
 196     return fixtime;
 197 }
 198
 199 void unix_to_gpstime(double unixtime,
 200                      /*@out@*/ int *week,
 201                      /*@out@*/ double *tow)
 202 {
 203     unixtime -= GPS_EPOCH;
 204     *week = (int)(unixtime / SECS_PER_WEEK);
 205     *tow = fmod(unixtime, SECS_PER_WEEK);
 206 }
 207
 208 #define Deg2Rad(n)      ((n) * DEG_2_RAD)
 209
 210 double earth_distance(double lat1, double lon1, double lat2, double lon2)
 211 /* distance in meters between two points specified in degrees. */
 212 {
 213     /*
 214      * this is a translation of the javascript implementation of the
 215      * Vincenty distance formula by Chris Veness. See
 216      * http://www.movable-type.co.uk/scripts/latlong-vincenty.html
 217      */
 218     double a, b, f;             // WGS-84 ellipsoid params
 219     double L, L_P, U1, U2, s_U1, c_U1, s_U2, c_U2;
 220     double uSq, A, B, d_S, lambda;
 221     double s_L, c_L, s_S, C;
 222     double c_S, S, s_A, c_SqA, c_2SM;
 223     int i = 100;
 224
 225     a = WGS84A;
 226     b = WGS84B;
 227     f = 1 / WGS84F;
 228     L = Deg2Rad(lon2 - lon1);
 229     U1 = atan((1 - f) * tan(Deg2Rad(lat1)));
 230     U2 = atan((1 - f) * tan(Deg2Rad(lat2)));
 231     s_U1 = sin(U1);
 232     c_U1 = cos(U1);
 233     s_U2 = sin(U2);
 234     c_U2 = cos(U2);
 235     lambda = L;
 236
 237     do {
 238         s_L = sin(lambda);
 239         c_L = cos(lambda);
 240         s_S = sqrt((c_U2 * s_L) * (c_U2 * s_L) +
 241                    (c_U1 * s_U2 - s_U1 * c_U2 * c_L) *
 242                    (c_U1 * s_U2 - s_U1 * c_U2 * c_L));
 243
 244         if (s_S == 0)
 245             return 0;
 246
 247         c_S = s_U1 * s_U2 + c_U1 * c_U2 * c_L;
 248         S = atan2(s_S, c_S);
 249         s_A = c_U1 * c_U2 * s_L / s_S;
 250         c_SqA = 1 - s_A * s_A;
 251         c_2SM = c_S - 2 * s_U1 * s_U2 / c_SqA;
 252
 253         if (isnan(c_2SM))
 254             c_2SM = 0;
 255
 256         C = f / 16 * c_SqA * (4 + f * (4 - 3 * c_SqA));
 257         L_P = lambda;
 258         lambda = L + (1 - C) * f * s_A *
 259             (S + C * s_S * (c_2SM + C * c_S * (2 * c_2SM * c_2SM - 1)));
 260     } while ((fabs(lambda - L_P) > 1.0e-12) && (--i > 0));
 261
 262     if (i == 0)
 263         return NAN;             // formula failed to converge
 264
 265     uSq = c_SqA * ((a * a) - (b * b)) / (b * b);
 266     A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
 267     B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
 268     d_S = B * s_S * (c_2SM + B / 4 *
 269                      (c_S * (-1 + 2 * c_2SM * c_2SM) - B / 6 * c_2SM *
 270                       (-3 + 4 * s_S * s_S) * (-3 + 4 * c_2SM * c_2SM)));
 271
 272     return (WGS84B * A * (S - d_S));
 273 }
 274
 275 /*****************************************************************************
 276
 277 Carl Carter of SiRF supplied this algorithm for computing DOPs from
 278 a list of visible satellites (some typos corrected)...
 279
 280 For satellite n, let az(n) = azimuth angle from North and el(n) be elevation.
 281 Let:
 282
 283     a(k, 1) = sin az(k) * cos el(k)
 284     a(k, 2) = cos az(k) * cos el(k)
 285     a(k, 3) = sin el(k)
 286
 287 Then form the line-of-sight matrix A for satellites used in the solution:
 288
 289     | a(1,1) a(1,2) a(1,3) 1 |
 290     | a(2,1) a(2,2) a(2,3) 1 |
 291     |   :       :      :   : |
 292     | a(n,1) a(n,2) a(n,3) 1 |
 293
 294 And its transpose A~:
 295
 296     |a(1, 1) a(2, 1) .  .  .  a(n, 1) |
 297     |a(1, 2) a(2, 2) .  .  .  a(n, 2) |
 298     |a(1, 3) a(2, 3) .  .  .  a(n, 3) |
 299     |    1       1   .  .  .     1    |
 300
 301 Compute the covariance matrix (A~*A)^-1, which is guaranteed symmetric:
 302
 303     | s(x)^2    s(x)*s(y)  s(x)*s(z)  s(x)*s(t) |
 304     | s(y)*s(x) s(y)^2     s(y)*s(z)  s(y)*s(t) |
 305     | s(z)*s(x) s(z)*s(y)  s(z)^2     s(z)*s(t) |
 306     | s(t)*s(x) s(t)*s(y)  s(t)*s(z)  s(t)^2    |
 307
 308 Then:
 309
 310 GDOP = sqrt(s(x)^2 + s(y)^2 + s(z)^2 + s(t)^2)
 311 TDOP = sqrt(s(t)^2)
 312 PDOP = sqrt(s(x)^2 + s(y)^2 + s(z)^2)
 313 HDOP = sqrt(s(x)^2 + s(y)^2)
 314 VDOP = sqrt(s(z)^2)
 315
 316 Here's how we implement it...
 317
 318 First, each compute element P(i,j) of the 4x4 product A~*A.
 319 If S(k=1,k=n): f(...) is the sum of f(...) as k varies from 1 to n, then
 320 applying the definition of matrix product tells us:
 321
 322 P(i,j) = S(k=1,k=n): B(i, k) * A(k, j)
 323
 324 But because B is the transpose of A, this reduces to
 325
 326 P(i,j) = S(k=1,k=n): A(k, i) * A(k, j)
 327
 328 This is not, however, the entire algorithm that SiRF uses.  Carl writes:
 329
 330 > As you note, with rounding accounted for, most values agree exactly, and
 331 > those that don't agree our number is higher.  That is because we
 332 > deweight some satellites and account for that in the DOP calculation.
 333 > If a satellite is not used in a solution at the same weight as others,
 334 > it should not contribute to DOP calculation at the same weight.  So our
 335 > internal algorithm does a compensation for that which you would have no
 336 > way to duplicate on the outside since we don't output the weighting
 337 > factors.  In fact those are not even available to API users.
 338
 339 Queried about the deweighting, Carl says:
 340
 341 > In the SiRF tracking engine, each satellite track is assigned a quality
 342 > value based on the tracker's estimate of that signal.  It includes C/No
 343 > estimate, ability to hold onto the phase, stability of the I vs. Q phase
 344 > angle, etc.  The navigation algorithm then ranks all the tracks into
 345 > quality order and selects which ones to use in the solution and what
 346 > weight to give those used in the solution.  The process is actually a
 347 > bit of a "trial and error" method -- we initially use all available
 348 > tracks in the solution, then we sequentially remove the lowest quality
 349 > ones until the solution stabilizes.  The weighting is inherent in the
 350 > Kalman filter algorithm.  Once the solution is stable, the DOP is
 351 > computed from those SVs used, and there is an algorithm that looks at
 352 > the quality ratings and determines if we need to deweight any.
 353 > Likewise, if we use altitude hold mode for a 3-SV solution, we deweight
 354 > the phantom satellite at the center of the Earth.
 355
 356 So we cannot exactly duplicate what SiRF does internally.  We'll leave
 357 HDOP alone and use our computed values for VDOP and PDOP.  Note, this
 358 may have to change in the future if this code is used by a non-SiRF
 359 driver.
 360
 361 ******************************************************************************/
 362
 363 void clear_dop( /*@out@*/ struct dop_t *dop)
 364 {
 365     dop->xdop = dop->ydop = dop->vdop = dop->tdop = dop->hdop = dop->pdop =
 366         dop->gdop = NAN;
 367 }
 368
 369 /*@ -fixedformalarray -mustdefine @*/
 370 static bool invert(double mat[4][4], /*@out@*/ double inverse[4][4])
 371 {
 372     // Find all NECESSARY 2x2 subdeterminants
 373     double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
 374     double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];
 375     //double Det2_12_03 = mat[1][0]*mat[2][3] - mat[1][3]*mat[2][0];
 376     double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
 377     //double Det2_12_13 = mat[1][1]*mat[2][3] - mat[1][3]*mat[2][1];
 378     //double Det2_12_23 = mat[1][2]*mat[2][3] - mat[1][3]*mat[2][2];
 379     double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
 380     //double Det2_13_02 = mat[1][0]*mat[3][2] - mat[1][2]*mat[3][0];
 381     double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
 382     //double Det2_13_12 = mat[1][1]*mat[3][2] - mat[1][2]*mat[3][1];
 383     double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
 384     //double Det2_13_23 = mat[1][2]*mat[3][3] - mat[1][3]*mat[3][2];
 385     double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
 386     double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
 387     double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
 388     double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
 389     double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
 390     double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
 391
 392     // Find all NECESSARY 3x3 subdeterminants
 393     double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02
 394         + mat[0][2] * Det2_12_01;
 395     //double Det3_012_013 = mat[0][0]*Det2_12_13 - mat[0][1]*Det2_12_03
 396     //                            + mat[0][3]*Det2_12_01;
 397     //double Det3_012_023 = mat[0][0]*Det2_12_23 - mat[0][2]*Det2_12_03
 398     //                            + mat[0][3]*Det2_12_02;
 399     //double Det3_012_123 = mat[0][1]*Det2_12_23 - mat[0][2]*Det2_12_13
 400     //                            + mat[0][3]*Det2_12_12;
 401     //double Det3_013_012 = mat[0][0]*Det2_13_12 - mat[0][1]*Det2_13_02
 402     //                            + mat[0][2]*Det2_13_01;
 403     double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03
 404         + mat[0][3] * Det2_13_01;
 405     //double Det3_013_023 = mat[0][0]*Det2_13_23 - mat[0][2]*Det2_13_03
 406     //                            + mat[0][3]*Det2_13_02;
 407     //double Det3_013_123 = mat[0][1]*Det2_13_23 - mat[0][2]*Det2_13_13
 408     //                            + mat[0][3]*Det2_13_12;
 409     //double Det3_023_012 = mat[0][0]*Det2_23_12 - mat[0][1]*Det2_23_02
 410     //                            + mat[0][2]*Det2_23_01;
 411     //double Det3_023_013 = mat[0][0]*Det2_23_13 - mat[0][1]*Det2_23_03
 412     //                            + mat[0][3]*Det2_23_01;
 413     double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03
 414         + mat[0][3] * Det2_23_02;
 415     //double Det3_023_123 = mat[0][1]*Det2_23_23 - mat[0][2]*Det2_23_13
 416     //                            + mat[0][3]*Det2_23_12;
 417     double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02
 418         + mat[1][2] * Det2_23_01;
 419     double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03
 420         + mat[1][3] * Det2_23_01;
 421     double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03
 422         + mat[1][3] * Det2_23_02;
 423     double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13
 424         + mat[1][3] * Det2_23_12;
 425
 426     // Find the 4x4 determinant
 427     static double det;
 428     det = mat[0][0] * Det3_123_123
 429         - mat[0][1] * Det3_123_023
 430         + mat[0][2] * Det3_123_013 - mat[0][3] * Det3_123_012;
 431
 432     // Very small determinants probably reflect floating-point fuzz near zero
 433     if (fabs(det) < 0.0001)
 434         return false;
 435
 436     inverse[0][0] = Det3_123_123 / det;
 437     //inverse[0][1] = -Det3_023_123 / det;
 438     //inverse[0][2] =  Det3_013_123 / det;
 439     //inverse[0][3] = -Det3_012_123 / det;
 440
 441     //inverse[1][0] = -Det3_123_023 / det;
 442     inverse[1][1] = Det3_023_023 / det;
 443     //inverse[1][2] = -Det3_013_023 / det;
 444     //inverse[1][3] =  Det3_012_023 / det;
 445
 446     //inverse[2][0] =  Det3_123_013 / det;
 447     //inverse[2][1] = -Det3_023_013 / det;
 448     inverse[2][2] = Det3_013_013 / det;
 449     //inverse[2][3] = -Det3_012_013 / det;
 450
 451     //inverse[3][0] = -Det3_123_012 / det;
 452     //inverse[3][1] =  Det3_023_012 / det;
 453     //inverse[3][2] = -Det3_013_012 / det;
 454     inverse[3][3] = Det3_012_012 / det;
 455
 456     return true;
 457 }
 458
 459 /*@ +fixedformalarray +mustdefine @*/
 460
 461 gps_mask_t fill_dop(const struct gps_data_t * gpsdata, struct dop_t * dop)
 462 {
 463     double prod[4][4];
 464     double inv[4][4];
 465     double satpos[MAXCHANNELS][4];
 466     double xdop, ydop, hdop, vdop, pdop, tdop, gdop;
 467     int i, j, k, n;
 468
 469 #ifdef __UNUSED__
 470     gpsd_report(LOG_INF, "Satellite picture:\n");
 471     for (k = 0; k < MAXCHANNELS; k++) {
 472         if (gpsdata->used[k])
 473             gpsd_report(LOG_INF, "az: %d el: %d  SV: %d\n",
 474                         gpsdata->azimuth[k], gpsdata->elevation[k],
 475                         gpsdata->used[k]);
 476     }
 477 #endif /* __UNUSED__ */
 478
 479     for (n = k = 0; k < gpsdata->satellites_used; k++) {
 480         if (gpsdata->used[k] == 0)
 481             continue;
 482         satpos[n][0] = sin(gpsdata->azimuth[k] * DEG_2_RAD)
 483             * cos(gpsdata->elevation[k] * DEG_2_RAD);
 484         satpos[n][1] = cos(gpsdata->azimuth[k] * DEG_2_RAD)
 485             * cos(gpsdata->elevation[k] * DEG_2_RAD);
 486         satpos[n][2] = sin(gpsdata->elevation[k] * DEG_2_RAD);
 487         satpos[n][3] = 1;
 488         n++;
 489     }
 490
 491 #ifdef __UNUSED__
 492     gpsd_report(LOG_INF, "Line-of-sight matrix:\n");
 493     for (k = 0; k < n; k++) {
 494         gpsd_report(LOG_INF, "%f %f %f %f\n",
 495                     satpos[k][0], satpos[k][1], satpos[k][2], satpos[k][3]);
 496     }
 497 #endif /* __UNUSED__ */
 498
 499     for (i = 0; i < 4; ++i) {   //< rows
 500         for (j = 0; j < 4; ++j) {       //< cols
 501             prod[i][j] = 0.0;
 502             for (k = 0; k < n; ++k) {
 503                 prod[i][j] += satpos[k][i] * satpos[k][j];
 504             }
 505         }
 506     }
 507
 508 #ifdef __UNUSED__
 509     gpsd_report(LOG_INF, "product:\n");
 510     for (k = 0; k < 4; k++) {
 511         gpsd_report(LOG_INF, "%f %f %f %f\n",
 512                     prod[k][0], prod[k][1], prod[k][2], prod[k][3]);
 513     }
 514 #endif /* __UNUSED__ */
 515
 516     if (invert(prod, inv)) {
 517 #ifdef __UNUSED__
 518         /*
 519          * Note: this will print garbage unless all the subdeterminants
 520          * are computed in the invert() function.
 521          */
 522         gpsd_report(LOG_RAW, "inverse:\n");
 523         for (k = 0; k < 4; k++) {
 524             gpsd_report(LOG_RAW, "%f %f %f %f\n",
 525                         inv[k][0], inv[k][1], inv[k][2], inv[k][3]);
 526         }
 527 #endif /* __UNUSED__ */
 528     } else {
 529 #ifndef USE_QT
 530         gpsd_report(LOG_DATA,
 531                     "LOS matrix is singular, can't calculate DOPs - source '%s'\n",
 532                     gpsdata->dev.path);
 533 #endif
 534         return 0;
 535     }
 536
 537     xdop = sqrt(inv[0][0]);
 538     ydop = sqrt(inv[1][1]);
 539     hdop = sqrt(inv[0][0] + inv[1][1]);
 540     vdop = sqrt(inv[2][2]);
 541     pdop = sqrt(inv[0][0] + inv[1][1] + inv[2][2]);
 542     tdop = sqrt(inv[3][3]);
 543     gdop = sqrt(inv[0][0] + inv[1][1] + inv[2][2] + inv[3][3]);
 544
 545 #ifndef USE_QT
 546     gpsd_report(LOG_DATA,
 547                 "DOPS computed/reported: X=%f/%f, Y=%f/%f, H=%f/%f, V=%f/%f, P=%f/%f, T=%f/%f, G=%f/%f\n",
 548                 xdop, dop->xdop, ydop, dop->ydop, hdop, dop->hdop, vdop,
 549                 dop->vdop, pdop, dop->pdop, tdop, dop->tdop, gdop, dop->gdop);
 550 #endif
 551
 552     /*@ -usedef @*/
 553     if (isnan(dop->xdop) != 0) {
 554         dop->xdop = xdop;
 555     }
 556     if (isnan(dop->ydop) != 0) {
 557         dop->ydop = ydop;
 558     }
 559     if (isnan(dop->hdop) != 0) {
 560         dop->hdop = hdop;
 561     }
 562     if (isnan(dop->vdop) != 0) {
 563         dop->vdop = vdop;
 564     }
 565     if (isnan(dop->pdop) != 0) {
 566         dop->pdop = pdop;
 567     }
 568     if (isnan(dop->tdop) != 0) {
 569         dop->tdop = tdop;
 570     }
 571     if (isnan(dop->gdop) != 0) {
 572         dop->gdop = gdop;
 573     }
 574     /*@ +usedef @*/
 575
 576     return DOP_IS;
 577 }