diff options
Diffstat (limited to 'libgpsd_core.c')
| -rw-r--r-- | libgpsd_core.c | 94 |
1 files changed, 2 insertions, 92 deletions
diff --git a/libgpsd_core.c b/libgpsd_core.c index 7b679174..c35d98f7 100644 --- a/libgpsd_core.c +++ b/libgpsd_core.c @@ -32,6 +32,7 @@ #endif /* S_SPLINT_S */ #include "gpsd.h" +#include "matrix.h" #if defined(NMEA2000_ENABLE) #include "driver_nmea2000.h" #endif /* defined(NMEA2000_ENABLE) */ @@ -653,97 +654,6 @@ driver. ******************************************************************************/ -/*@ -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 gpsd_errout_t *errout, const struct gps_data_t * gpsdata, @@ -814,7 +724,7 @@ static gps_mask_t fill_dop(const struct gpsd_errout_t *errout, } #endif /* __UNUSED__ */ - if (invert(prod, inv)) { + if (matrix_invert(prod, inv)) { #ifdef __UNUSED__ /* * Note: this will print garbage unless all the subdeterminants |