diff options
author | Eric S. Raymond <esr@thyrsus.com> | 2010-12-16 18:27:18 -0500 |
---|---|---|
committer | Eric S. Raymond <esr@thyrsus.com> | 2010-12-16 18:27:18 -0500 |
commit | a7b4401c57dd2b35460685e8762b2d4da0baf4d0 (patch) | |
tree | ce38ee893b3281e790bacef91b06c2b087c752d5 | |
parent | 4a8ba33a11077c241c60cf888f1c5ddb6bbb5b4b (diff) | |
download | gpsd-a7b4401c57dd2b35460685e8762b2d4da0baf4d0.tar.gz |
Abolish gpsd_report.c. No more hardwired logging from the client libraries.
Only the daemon now uses this function.
-rw-r--r-- | Makefile.am | 1 | ||||
-rw-r--r-- | gpsd.h-tail | 2 | ||||
-rw-r--r-- | gpsd_report.c | 22 | ||||
-rw-r--r-- | gpsutils.c | 315 | ||||
-rw-r--r-- | libgps_core.c | 6 | ||||
-rw-r--r-- | libgpsd_core.c | 316 | ||||
-rw-r--r-- | netlib.c | 5 | ||||
-rw-r--r-- | www/client-howto.txt | 8 |
8 files changed, 326 insertions, 349 deletions
diff --git a/Makefile.am b/Makefile.am index af03c2a9..95f00a98 100644 --- a/Makefile.am +++ b/Makefile.am @@ -162,7 +162,6 @@ print_libgps_VERSION: libgps_c_sources = \ ais_json.c \ - gpsd_report.c \ gpsutils.c \ geoid.c \ gpsdclient.c \ diff --git a/gpsd.h-tail b/gpsd.h-tail index cb638258..11f6caf4 100644 --- a/gpsd.h-tail +++ b/gpsd.h-tail @@ -572,8 +572,6 @@ extern void ecef_to_wgs84fix(/*@out@*/struct gps_fix_t *, double, double, double, double, double, double); extern void clear_dop(/*@out@*/struct dop_t *); -extern gps_mask_t fill_dop(/*@in@*/const struct gps_data_t *gpsdata, - struct dop_t *dop); /* srecord.c */ extern void hexdump(size_t, unsigned char *, unsigned char *); diff --git a/gpsd_report.c b/gpsd_report.c deleted file mode 100644 index f1c89704..00000000 --- a/gpsd_report.c +++ /dev/null @@ -1,22 +0,0 @@ -/* - * This file is Copyright (c) 2010 by the GPSD project - * BSD terms apply: see the file COPYING in the distribution root for details. - */ -#include <stdarg.h> -#include "gpsd.h" - - -# if __GNUC__ >= 3 || (__GNUC__ == 2 && __GNUC_MINOR__ >= 7) -void gpsd_report(int errlevel UNUSED, const char *fmt, ...) - __attribute__ ((weak)); -#endif - -void gpsd_report(int errlevel UNUSED, const char *fmt, ...) -/* stub logger for clients that don't supply one */ -{ - va_list ap; - - va_start(ap, fmt); - (void)vfprintf(stderr, fmt, ap); - va_end(ap); -} @@ -282,318 +282,3 @@ double earth_distance(double lat1, double lon1, double lat2, double lon2) return earth_distance_and_bearings(lat1, lon1, lat2, lon2, NULL, NULL); } -/***************************************************************************** - -Carl Carter of SiRF supplied this algorithm for computing DOPs from -a list of visible satellites (some typos corrected)... - -For satellite n, let az(n) = azimuth angle from North and el(n) be elevation. -Let: - - a(k, 1) = sin az(k) * cos el(k) - a(k, 2) = cos az(k) * cos el(k) - a(k, 3) = sin el(k) - -Then form the line-of-sight matrix A for satellites used in the solution: - - | a(1,1) a(1,2) a(1,3) 1 | - | a(2,1) a(2,2) a(2,3) 1 | - | : : : : | - | a(n,1) a(n,2) a(n,3) 1 | - -And its transpose A~: - - |a(1, 1) a(2, 1) . . . a(n, 1) | - |a(1, 2) a(2, 2) . . . a(n, 2) | - |a(1, 3) a(2, 3) . . . a(n, 3) | - | 1 1 . . . 1 | - -Compute the covariance matrix (A~*A)^-1, which is guaranteed symmetric: - - | s(x)^2 s(x)*s(y) s(x)*s(z) s(x)*s(t) | - | s(y)*s(x) s(y)^2 s(y)*s(z) s(y)*s(t) | - | s(z)*s(x) s(z)*s(y) s(z)^2 s(z)*s(t) | - | s(t)*s(x) s(t)*s(y) s(t)*s(z) s(t)^2 | - -Then: - -GDOP = sqrt(s(x)^2 + s(y)^2 + s(z)^2 + s(t)^2) -TDOP = sqrt(s(t)^2) -PDOP = sqrt(s(x)^2 + s(y)^2 + s(z)^2) -HDOP = sqrt(s(x)^2 + s(y)^2) -VDOP = sqrt(s(z)^2) - -Here's how we implement it... - -First, each compute element P(i,j) of the 4x4 product A~*A. -If S(k=1,k=n): f(...) is the sum of f(...) as k varies from 1 to n, then -applying the definition of matrix product tells us: - -P(i,j) = S(k=1,k=n): B(i, k) * A(k, j) - -But because B is the transpose of A, this reduces to - -P(i,j) = S(k=1,k=n): A(k, i) * A(k, j) - -This is not, however, the entire algorithm that SiRF uses. Carl writes: - -> As you note, with rounding accounted for, most values agree exactly, and -> those that don't agree our number is higher. That is because we -> deweight some satellites and account for that in the DOP calculation. -> If a satellite is not used in a solution at the same weight as others, -> it should not contribute to DOP calculation at the same weight. So our -> internal algorithm does a compensation for that which you would have no -> way to duplicate on the outside since we don't output the weighting -> factors. In fact those are not even available to API users. - -Queried about the deweighting, Carl says: - -> In the SiRF tracking engine, each satellite track is assigned a quality -> value based on the tracker's estimate of that signal. It includes C/No -> estimate, ability to hold onto the phase, stability of the I vs. Q phase -> angle, etc. The navigation algorithm then ranks all the tracks into -> quality order and selects which ones to use in the solution and what -> weight to give those used in the solution. The process is actually a -> bit of a "trial and error" method -- we initially use all available -> tracks in the solution, then we sequentially remove the lowest quality -> ones until the solution stabilizes. The weighting is inherent in the -> Kalman filter algorithm. Once the solution is stable, the DOP is -> computed from those SVs used, and there is an algorithm that looks at -> the quality ratings and determines if we need to deweight any. -> Likewise, if we use altitude hold mode for a 3-SV solution, we deweight -> the phantom satellite at the center of the Earth. - -So we cannot exactly duplicate what SiRF does internally. We'll leave -HDOP alone and use our computed values for VDOP and PDOP. Note, this -may have to change in the future if this code is used by a non-SiRF -driver. - -******************************************************************************/ - -void clear_dop( /*@out@*/ struct dop_t *dop) -{ - dop->xdop = dop->ydop = dop->vdop = dop->tdop = dop->hdop = dop->pdop = - dop->gdop = NAN; -} - -/*@ -fixedformalarray -mustdefine @*/ -static bool invert(double mat[4][4], /*@out@*/ double inverse[4][4]) -{ - // Find all NECESSARY 2x2 subdeterminants - double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]; - double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]; - //double Det2_12_03 = mat[1][0]*mat[2][3] - mat[1][3]*mat[2][0]; - double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]; - //double Det2_12_13 = mat[1][1]*mat[2][3] - mat[1][3]*mat[2][1]; - //double Det2_12_23 = mat[1][2]*mat[2][3] - mat[1][3]*mat[2][2]; - double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0]; - //double Det2_13_02 = mat[1][0]*mat[3][2] - mat[1][2]*mat[3][0]; - double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0]; - //double Det2_13_12 = mat[1][1]*mat[3][2] - mat[1][2]*mat[3][1]; - double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1]; - //double Det2_13_23 = mat[1][2]*mat[3][3] - mat[1][3]*mat[3][2]; - double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0]; - double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0]; - double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0]; - double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1]; - double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1]; - double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2]; - - // Find all NECESSARY 3x3 subdeterminants - double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02 - + mat[0][2] * Det2_12_01; - //double Det3_012_013 = mat[0][0]*Det2_12_13 - mat[0][1]*Det2_12_03 - // + mat[0][3]*Det2_12_01; - //double Det3_012_023 = mat[0][0]*Det2_12_23 - mat[0][2]*Det2_12_03 - // + mat[0][3]*Det2_12_02; - //double Det3_012_123 = mat[0][1]*Det2_12_23 - mat[0][2]*Det2_12_13 - // + mat[0][3]*Det2_12_12; - //double Det3_013_012 = mat[0][0]*Det2_13_12 - mat[0][1]*Det2_13_02 - // + mat[0][2]*Det2_13_01; - double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03 - + mat[0][3] * Det2_13_01; - //double Det3_013_023 = mat[0][0]*Det2_13_23 - mat[0][2]*Det2_13_03 - // + mat[0][3]*Det2_13_02; - //double Det3_013_123 = mat[0][1]*Det2_13_23 - mat[0][2]*Det2_13_13 - // + mat[0][3]*Det2_13_12; - //double Det3_023_012 = mat[0][0]*Det2_23_12 - mat[0][1]*Det2_23_02 - // + mat[0][2]*Det2_23_01; - //double Det3_023_013 = mat[0][0]*Det2_23_13 - mat[0][1]*Det2_23_03 - // + mat[0][3]*Det2_23_01; - double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03 - + mat[0][3] * Det2_23_02; - //double Det3_023_123 = mat[0][1]*Det2_23_23 - mat[0][2]*Det2_23_13 - // + mat[0][3]*Det2_23_12; - double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02 - + mat[1][2] * Det2_23_01; - double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03 - + mat[1][3] * Det2_23_01; - double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03 - + mat[1][3] * Det2_23_02; - double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13 - + mat[1][3] * Det2_23_12; - - // Find the 4x4 determinant - static double det; - det = mat[0][0] * Det3_123_123 - - mat[0][1] * Det3_123_023 - + mat[0][2] * Det3_123_013 - mat[0][3] * Det3_123_012; - - // Very small determinants probably reflect floating-point fuzz near zero - if (fabs(det) < 0.0001) - return false; - - inverse[0][0] = Det3_123_123 / det; - //inverse[0][1] = -Det3_023_123 / det; - //inverse[0][2] = Det3_013_123 / det; - //inverse[0][3] = -Det3_012_123 / det; - - //inverse[1][0] = -Det3_123_023 / det; - inverse[1][1] = Det3_023_023 / det; - //inverse[1][2] = -Det3_013_023 / det; - //inverse[1][3] = Det3_012_023 / det; - - //inverse[2][0] = Det3_123_013 / det; - //inverse[2][1] = -Det3_023_013 / det; - inverse[2][2] = Det3_013_013 / det; - //inverse[2][3] = -Det3_012_013 / det; - - //inverse[3][0] = -Det3_123_012 / det; - //inverse[3][1] = Det3_023_012 / det; - //inverse[3][2] = -Det3_013_012 / det; - inverse[3][3] = Det3_012_012 / det; - - return true; -} - -/*@ +fixedformalarray +mustdefine @*/ - -gps_mask_t fill_dop(const struct gps_data_t * gpsdata, struct dop_t * dop) -{ - double prod[4][4]; - double inv[4][4]; - double satpos[MAXCHANNELS][4]; - double xdop, ydop, hdop, vdop, pdop, tdop, gdop; - int i, j, k, n; - - memset(satpos, 0, sizeof(satpos)); - -#ifdef __UNUSED__ - gpsd_report(LOG_INF, "Satellite picture:\n"); - for (k = 0; k < MAXCHANNELS; k++) { - if (gpsdata->used[k]) - gpsd_report(LOG_INF, "az: %d el: %d SV: %d\n", - gpsdata->azimuth[k], gpsdata->elevation[k], - gpsdata->used[k]); - } -#endif /* __UNUSED__ */ - - for (n = k = 0; k < gpsdata->satellites_used; k++) { - if (gpsdata->used[k] == 0) - continue; - satpos[n][0] = sin(gpsdata->azimuth[k] * DEG_2_RAD) - * cos(gpsdata->elevation[k] * DEG_2_RAD); - satpos[n][1] = cos(gpsdata->azimuth[k] * DEG_2_RAD) - * cos(gpsdata->elevation[k] * DEG_2_RAD); - satpos[n][2] = sin(gpsdata->elevation[k] * DEG_2_RAD); - satpos[n][3] = 1; - n++; - } - - /* If we don't have 4 satellites then we don't have enough information to calculate DOPS */ - if (n < 4) { - gpsd_report(LOG_DATA + 2, "Not enough Satellites available %d < 4:\n", - n); - return 0; /* Is this correct return code here? or should it be ERROR_IS */ - } - - memset(prod, 0, sizeof(prod)); - memset(inv, 0, sizeof(inv)); - -#ifdef __UNUSED__ - gpsd_report(LOG_INF, "Line-of-sight matrix:\n"); - for (k = 0; k < n; k++) { - gpsd_report(LOG_INF, "%f %f %f %f\n", - satpos[k][0], satpos[k][1], satpos[k][2], satpos[k][3]); - } -#endif /* __UNUSED__ */ - - for (i = 0; i < 4; ++i) { //< rows - for (j = 0; j < 4; ++j) { //< cols - prod[i][j] = 0.0; - for (k = 0; k < n; ++k) { - prod[i][j] += satpos[k][i] * satpos[k][j]; - } - } - } - -#ifdef __UNUSED__ - gpsd_report(LOG_INF, "product:\n"); - for (k = 0; k < 4; k++) { - gpsd_report(LOG_INF, "%f %f %f %f\n", - prod[k][0], prod[k][1], prod[k][2], prod[k][3]); - } -#endif /* __UNUSED__ */ - - if (invert(prod, inv)) { -#ifdef __UNUSED__ - /* - * Note: this will print garbage unless all the subdeterminants - * are computed in the invert() function. - */ - gpsd_report(LOG_RAW, "inverse:\n"); - for (k = 0; k < 4; k++) { - gpsd_report(LOG_RAW, "%f %f %f %f\n", - inv[k][0], inv[k][1], inv[k][2], inv[k][3]); - } -#endif /* __UNUSED__ */ - } else { -#ifndef USE_QT - gpsd_report(LOG_DATA, - "LOS matrix is singular, can't calculate DOPs - source '%s'\n", - gpsdata->dev.path); -#endif - return 0; - } - - xdop = sqrt(inv[0][0]); - ydop = sqrt(inv[1][1]); - hdop = sqrt(inv[0][0] + inv[1][1]); - vdop = sqrt(inv[2][2]); - pdop = sqrt(inv[0][0] + inv[1][1] + inv[2][2]); - tdop = sqrt(inv[3][3]); - gdop = sqrt(inv[0][0] + inv[1][1] + inv[2][2] + inv[3][3]); - -#ifndef USE_QT - gpsd_report(LOG_DATA, - "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", - xdop, dop->xdop, ydop, dop->ydop, hdop, dop->hdop, vdop, - dop->vdop, pdop, dop->pdop, tdop, dop->tdop, gdop, dop->gdop); -#endif - - /*@ -usedef @*/ - if (isnan(dop->xdop) != 0) { - dop->xdop = xdop; - } - if (isnan(dop->ydop) != 0) { - dop->ydop = ydop; - } - if (isnan(dop->hdop) != 0) { - dop->hdop = hdop; - } - if (isnan(dop->vdop) != 0) { - dop->vdop = vdop; - } - if (isnan(dop->pdop) != 0) { - dop->pdop = pdop; - } - if (isnan(dop->tdop) != 0) { - dop->tdop = tdop; - } - if (isnan(dop->gdop) != 0) { - dop->gdop = gdop; - } - /*@ +usedef @*/ - - return DOP_IS; -} diff --git a/libgps_core.c b/libgps_core.c index 2ca14364..bec129fc 100644 --- a/libgps_core.c +++ b/libgps_core.c @@ -96,12 +96,12 @@ int gps_open(/*@null@*/const char *host, /*@null@*/const char *port, if ((gpsdata->gps_fd = netlib_connectsock(AF_UNSPEC, host, port, "tcp")) < 0) { errno = gpsdata->gps_fd; - gpsd_report(LOG_SPIN, "netlib_connectsock() returns error %d\n", errno); + libgps_debug_trace((2, "netlib_connectsock() returns error %d\n", errno)); return -1; } else - gpsd_report(LOG_SPIN, "netlib_connectsock() returns socket on fd %d\n", - gpsdata->gps_fd); + libgps_debug_trace((2, "netlib_connectsock() returns socket on fd %d\n", + gpsdata->gps_fd)); #else QTcpSocket *sock = new QTcpSocket(); gpsdata->gps_fd = sock; diff --git a/libgpsd_core.c b/libgpsd_core.c index 1d7a9f06..85dca683 100644 --- a/libgpsd_core.c +++ b/libgpsd_core.c @@ -791,6 +791,322 @@ char /*@observer@*/ *gpsd_id( /*@in@ */ struct gps_device_t *session) return (buf); } +/***************************************************************************** + +Carl Carter of SiRF supplied this algorithm for computing DOPs from +a list of visible satellites (some typos corrected)... + +For satellite n, let az(n) = azimuth angle from North and el(n) be elevation. +Let: + + a(k, 1) = sin az(k) * cos el(k) + a(k, 2) = cos az(k) * cos el(k) + a(k, 3) = sin el(k) + +Then form the line-of-sight matrix A for satellites used in the solution: + + | a(1,1) a(1,2) a(1,3) 1 | + | a(2,1) a(2,2) a(2,3) 1 | + | : : : : | + | a(n,1) a(n,2) a(n,3) 1 | + +And its transpose A~: + + |a(1, 1) a(2, 1) . . . a(n, 1) | + |a(1, 2) a(2, 2) . . . a(n, 2) | + |a(1, 3) a(2, 3) . . . a(n, 3) | + | 1 1 . . . 1 | + +Compute the covariance matrix (A~*A)^-1, which is guaranteed symmetric: + + | s(x)^2 s(x)*s(y) s(x)*s(z) s(x)*s(t) | + | s(y)*s(x) s(y)^2 s(y)*s(z) s(y)*s(t) | + | s(z)*s(x) s(z)*s(y) s(z)^2 s(z)*s(t) | + | s(t)*s(x) s(t)*s(y) s(t)*s(z) s(t)^2 | + +Then: + +GDOP = sqrt(s(x)^2 + s(y)^2 + s(z)^2 + s(t)^2) +TDOP = sqrt(s(t)^2) +PDOP = sqrt(s(x)^2 + s(y)^2 + s(z)^2) +HDOP = sqrt(s(x)^2 + s(y)^2) +VDOP = sqrt(s(z)^2) + +Here's how we implement it... + +First, each compute element P(i,j) of the 4x4 product A~*A. +If S(k=1,k=n): f(...) is the sum of f(...) as k varies from 1 to n, then +applying the definition of matrix product tells us: + +P(i,j) = S(k=1,k=n): B(i, k) * A(k, j) + +But because B is the transpose of A, this reduces to + +P(i,j) = S(k=1,k=n): A(k, i) * A(k, j) + +This is not, however, the entire algorithm that SiRF uses. Carl writes: + +> As you note, with rounding accounted for, most values agree exactly, and +> those that don't agree our number is higher. That is because we +> deweight some satellites and account for that in the DOP calculation. +> If a satellite is not used in a solution at the same weight as others, +> it should not contribute to DOP calculation at the same weight. So our +> internal algorithm does a compensation for that which you would have no +> way to duplicate on the outside since we don't output the weighting +> factors. In fact those are not even available to API users. + +Queried about the deweighting, Carl says: + +> In the SiRF tracking engine, each satellite track is assigned a quality +> value based on the tracker's estimate of that signal. It includes C/No +> estimate, ability to hold onto the phase, stability of the I vs. Q phase +> angle, etc. The navigation algorithm then ranks all the tracks into +> quality order and selects which ones to use in the solution and what +> weight to give those used in the solution. The process is actually a +> bit of a "trial and error" method -- we initially use all available +> tracks in the solution, then we sequentially remove the lowest quality +> ones until the solution stabilizes. The weighting is inherent in the +> Kalman filter algorithm. Once the solution is stable, the DOP is +> computed from those SVs used, and there is an algorithm that looks at +> the quality ratings and determines if we need to deweight any. +> Likewise, if we use altitude hold mode for a 3-SV solution, we deweight +> the phantom satellite at the center of the Earth. + +So we cannot exactly duplicate what SiRF does internally. We'll leave +HDOP alone and use our computed values for VDOP and PDOP. Note, this +may have to change in the future if this code is used by a non-SiRF +driver. + +******************************************************************************/ + +void clear_dop( /*@out@*/ struct dop_t *dop) +{ + dop->xdop = dop->ydop = dop->vdop = dop->tdop = dop->hdop = dop->pdop = + dop->gdop = NAN; +} + +/*@ -fixedformalarray -mustdefine @*/ +static bool invert(double mat[4][4], /*@out@*/ double inverse[4][4]) +{ + // Find all NECESSARY 2x2 subdeterminants + double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0]; + double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0]; + //double Det2_12_03 = mat[1][0]*mat[2][3] - mat[1][3]*mat[2][0]; + double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1]; + //double Det2_12_13 = mat[1][1]*mat[2][3] - mat[1][3]*mat[2][1]; + //double Det2_12_23 = mat[1][2]*mat[2][3] - mat[1][3]*mat[2][2]; + double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0]; + //double Det2_13_02 = mat[1][0]*mat[3][2] - mat[1][2]*mat[3][0]; + double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0]; + //double Det2_13_12 = mat[1][1]*mat[3][2] - mat[1][2]*mat[3][1]; + double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1]; + //double Det2_13_23 = mat[1][2]*mat[3][3] - mat[1][3]*mat[3][2]; + double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0]; + double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0]; + double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0]; + double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1]; + double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1]; + double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2]; + + // Find all NECESSARY 3x3 subdeterminants + double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02 + + mat[0][2] * Det2_12_01; + //double Det3_012_013 = mat[0][0]*Det2_12_13 - mat[0][1]*Det2_12_03 + // + mat[0][3]*Det2_12_01; + //double Det3_012_023 = mat[0][0]*Det2_12_23 - mat[0][2]*Det2_12_03 + // + mat[0][3]*Det2_12_02; + //double Det3_012_123 = mat[0][1]*Det2_12_23 - mat[0][2]*Det2_12_13 + // + mat[0][3]*Det2_12_12; + //double Det3_013_012 = mat[0][0]*Det2_13_12 - mat[0][1]*Det2_13_02 + // + mat[0][2]*Det2_13_01; + double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03 + + mat[0][3] * Det2_13_01; + //double Det3_013_023 = mat[0][0]*Det2_13_23 - mat[0][2]*Det2_13_03 + // + mat[0][3]*Det2_13_02; + //double Det3_013_123 = mat[0][1]*Det2_13_23 - mat[0][2]*Det2_13_13 + // + mat[0][3]*Det2_13_12; + //double Det3_023_012 = mat[0][0]*Det2_23_12 - mat[0][1]*Det2_23_02 + // + mat[0][2]*Det2_23_01; + //double Det3_023_013 = mat[0][0]*Det2_23_13 - mat[0][1]*Det2_23_03 + // + mat[0][3]*Det2_23_01; + double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03 + + mat[0][3] * Det2_23_02; + //double Det3_023_123 = mat[0][1]*Det2_23_23 - mat[0][2]*Det2_23_13 + // + mat[0][3]*Det2_23_12; + double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02 + + mat[1][2] * Det2_23_01; + double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03 + + mat[1][3] * Det2_23_01; + double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03 + + mat[1][3] * Det2_23_02; + double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13 + + mat[1][3] * Det2_23_12; + + // Find the 4x4 determinant + static double det; + det = mat[0][0] * Det3_123_123 + - mat[0][1] * Det3_123_023 + + mat[0][2] * Det3_123_013 - mat[0][3] * Det3_123_012; + + // Very small determinants probably reflect floating-point fuzz near zero + if (fabs(det) < 0.0001) + return false; + + inverse[0][0] = Det3_123_123 / det; + //inverse[0][1] = -Det3_023_123 / det; + //inverse[0][2] = Det3_013_123 / det; + //inverse[0][3] = -Det3_012_123 / det; + + //inverse[1][0] = -Det3_123_023 / det; + inverse[1][1] = Det3_023_023 / det; + //inverse[1][2] = -Det3_013_023 / det; + //inverse[1][3] = Det3_012_023 / det; + + //inverse[2][0] = Det3_123_013 / det; + //inverse[2][1] = -Det3_023_013 / det; + inverse[2][2] = Det3_013_013 / det; + //inverse[2][3] = -Det3_012_013 / det; + + //inverse[3][0] = -Det3_123_012 / det; + //inverse[3][1] = Det3_023_012 / det; + //inverse[3][2] = -Det3_013_012 / det; + inverse[3][3] = Det3_012_012 / det; + + return true; +} + +/*@ +fixedformalarray +mustdefine @*/ + +static gps_mask_t fill_dop(const struct gps_data_t * gpsdata, struct dop_t * dop) +{ + double prod[4][4]; + double inv[4][4]; + double satpos[MAXCHANNELS][4]; + double xdop, ydop, hdop, vdop, pdop, tdop, gdop; + int i, j, k, n; + + memset(satpos, 0, sizeof(satpos)); + +#ifdef __UNUSED__ + gpsd_report(LOG_INF, "Satellite picture:\n"); + for (k = 0; k < MAXCHANNELS; k++) { + if (gpsdata->used[k]) + gpsd_report(LOG_INF, "az: %d el: %d SV: %d\n", + gpsdata->azimuth[k], gpsdata->elevation[k], + gpsdata->used[k]); + } +#endif /* __UNUSED__ */ + + for (n = k = 0; k < gpsdata->satellites_used; k++) { + if (gpsdata->used[k] == 0) + continue; + satpos[n][0] = sin(gpsdata->azimuth[k] * DEG_2_RAD) + * cos(gpsdata->elevation[k] * DEG_2_RAD); + satpos[n][1] = cos(gpsdata->azimuth[k] * DEG_2_RAD) + * cos(gpsdata->elevation[k] * DEG_2_RAD); + satpos[n][2] = sin(gpsdata->elevation[k] * DEG_2_RAD); + satpos[n][3] = 1; + n++; + } + + /* If we don't have 4 satellites then we don't have enough information to calculate DOPS */ + if (n < 4) { + gpsd_report(LOG_DATA + 2, "Not enough Satellites available %d < 4:\n", + n); + return 0; /* Is this correct return code here? or should it be ERROR_IS */ + } + + memset(prod, 0, sizeof(prod)); + memset(inv, 0, sizeof(inv)); + +#ifdef __UNUSED__ + gpsd_report(LOG_INF, "Line-of-sight matrix:\n"); + for (k = 0; k < n; k++) { + gpsd_report(LOG_INF, "%f %f %f %f\n", + satpos[k][0], satpos[k][1], satpos[k][2], satpos[k][3]); + } +#endif /* __UNUSED__ */ + + for (i = 0; i < 4; ++i) { //< rows + for (j = 0; j < 4; ++j) { //< cols + prod[i][j] = 0.0; + for (k = 0; k < n; ++k) { + prod[i][j] += satpos[k][i] * satpos[k][j]; + } + } + } + +#ifdef __UNUSED__ + gpsd_report(LOG_INF, "product:\n"); + for (k = 0; k < 4; k++) { + gpsd_report(LOG_INF, "%f %f %f %f\n", + prod[k][0], prod[k][1], prod[k][2], prod[k][3]); + } +#endif /* __UNUSED__ */ + + if (invert(prod, inv)) { +#ifdef __UNUSED__ + /* + * Note: this will print garbage unless all the subdeterminants + * are computed in the invert() function. + */ + gpsd_report(LOG_RAW, "inverse:\n"); + for (k = 0; k < 4; k++) { + gpsd_report(LOG_RAW, "%f %f %f %f\n", + inv[k][0], inv[k][1], inv[k][2], inv[k][3]); + } +#endif /* __UNUSED__ */ + } else { +#ifndef USE_QT + gpsd_report(LOG_DATA, + "LOS matrix is singular, can't calculate DOPs - source '%s'\n", + gpsdata->dev.path); +#endif + return 0; + } + + xdop = sqrt(inv[0][0]); + ydop = sqrt(inv[1][1]); + hdop = sqrt(inv[0][0] + inv[1][1]); + vdop = sqrt(inv[2][2]); + pdop = sqrt(inv[0][0] + inv[1][1] + inv[2][2]); + tdop = sqrt(inv[3][3]); + gdop = sqrt(inv[0][0] + inv[1][1] + inv[2][2] + inv[3][3]); + +#ifndef USE_QT + gpsd_report(LOG_DATA, + "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", + xdop, dop->xdop, ydop, dop->ydop, hdop, dop->hdop, vdop, + dop->vdop, pdop, dop->pdop, tdop, dop->tdop, gdop, dop->gdop); +#endif + + /*@ -usedef @*/ + if (isnan(dop->xdop) != 0) { + dop->xdop = xdop; + } + if (isnan(dop->ydop) != 0) { + dop->ydop = ydop; + } + if (isnan(dop->hdop) != 0) { + dop->hdop = hdop; + } + if (isnan(dop->vdop) != 0) { + dop->vdop = vdop; + } + if (isnan(dop->pdop) != 0) { + dop->pdop = pdop; + } + if (isnan(dop->tdop) != 0) { + dop->tdop = tdop; + } + if (isnan(dop->gdop) != 0) { + dop->gdop = gdop; + } + /*@ +usedef @*/ + + return DOP_IS; +} + static void gpsd_error_model(struct gps_device_t *session, struct gps_fix_t *fix, struct gps_fix_t *oldfix) /* compute errors and derived quantities */ @@ -147,6 +147,7 @@ char /*@observer@*/ *netlib_errstr(const int err) } char *netlib_sock2ip(int fd) +/* retrieve the IP address corresponding to a socket */ { sockaddr_t fsin; socklen_t alen = (socklen_t) sizeof(fsin); @@ -168,15 +169,11 @@ char *netlib_sock2ip(int fd) break; #endif default: - gpsd_report(LOG_ERROR, "Unhandled address family %d in %s\n", - fsin.sa.sa_family, __FUNCTION__); (void)strlcpy(ip, "<unknown AF>", sizeof(ip)); return ip; } } if (r != 0) { - gpsd_report(LOG_INF, "getpeername() = %d, error = %s (%d)\n", r, - strerror(errno), errno); (void)strlcpy(ip, "<unknown>", sizeof(ip)); } /*@ +branchstate +unrecog -boolint @*/ diff --git a/www/client-howto.txt b/www/client-howto.txt index f9669bf3..bfc078a4 100644 --- a/www/client-howto.txt +++ b/www/client-howto.txt @@ -1,6 +1,6 @@ = GPSD Client HOWTO = Eric S. Raymond <esr@thyrsus.com> -v1.2, April 2010 +v1.3, December 2010 This document is mastered in asciidoc format. If you are reading it in HTML, you can find the original at @@ -46,7 +46,7 @@ that in at least the following ways: Time to fix after a power-on matters because in many use cases for GPSes they're running off a battery, and you can't afford to keep them powered when you don't actually need location data. This is why GPS -sensors are usually designed to go to a low-power mode when you close +sensors are sometimes designed to go to a low-power mode when you close the serial port they're attached to. AIS receivers have a simpler set of constraints. They report @@ -399,5 +399,9 @@ In major versions before 5: a different return convention. The name 'poll()' will at some point be reintroduced as an interface to the wire-protocol POLL command. +* Clients needed to define a gpsd_report() function for c;ient-side logging + if they didn't want code in netlib.c and libgps_core.c to occasionally + senfd messages to stderr. This requirement is now gone. + See http://gpsd.berlios.de/future.html#api_cleanup[Client API Cleanup] for details. |