Slimmed-down version of inversion code.

1 files changed, 53 insertions, 48 deletions

diff --git a/gpsutils.c b/gpsutils.c
index 01ec5d94..88168b4b 100644
--- a/gpsutils.c
+++ b/gpsutils.c
@@ -158,7 +158,6 @@ double earth_distance(double lat1, double lon1, double lat2, double lon2)
     return CalcRad((lat1+lat2) / 2) * acos(a);
 }
 
-#ifdef __UNUSED__
 /*****************************************************************************
 
 Carl Carter of SiRF supplied this algorithm for computing DOPs from 
@@ -217,18 +216,18 @@ P(i,j) = S(k=1,k=n): A(k, i) * A(k, j) + 1
 static int invert(double mat[4][4], 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_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_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_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_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];
@@ -237,30 +236,30 @@ static int invert(double mat[4][4], double inverse[4][4])
   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_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_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_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 
@@ -282,24 +281,24 @@ static int invert(double mat[4][4], double inverse[4][4])
       return 0;
 
   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[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][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[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][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[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;
+  //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 1;
 }  
@@ -314,7 +313,7 @@ void dop(int channels, struct gps_data_t *gpsdata)
     gpsd_report(0, "Satellite picture:\n");
     for (k = 0; k < MAXCHANNELS; k++) {
 	if (gpsdata->used[k])
-	    gpsd_report(0, "%d %d %d\n",
+	    gpsd_report(0, "az: %d el: %d  SV: %d\n",
 			gpsdata->azimuth[k], gpsdata->elevation[k], gpsdata->used[k]);
     }
 
@@ -349,14 +348,20 @@ void dop(int channels, struct gps_data_t *gpsdata)
 		    prod[k][0], prod[k][1], prod[k][2], prod[k][3]);
     }
 
-    if (invert(prod, inv))
-	gpsd_report(0, "HDOP: actual = %f, computed = %f\n",
+    if (invert(prod, inv)) {
+	gpsd_report(0, "inverse:\n");
+	for (k = 0; k < 4; k++) {
+	    gpsd_report(0, "%f %f %f %f\n",
+			inv[k][0], inv[k][1], inv[k][2], inv[k][3]);
+	}
+	gpsd_report(0, "HDOP: reported = %f, computed = %f\n",
 		    gpsdata->hdop, sqrt(inv[0][0] + inv[1][1]));
-    else
+    } else
 	gpsd_report(0, "Matrix is singular.\n");
 
     //gpsdata->hdop = sqrt(diag[0] + diag[1]);
     //gpsdata->vdop = sqrt(diag[1]);
     //gpsdata->pdop = sqrt(diag[0] + diag[1] + diag[2]);
 }
-#endif /* __UNUSED__ */
+#ifdef __UNUSED__
+#endif /* __UNUSED */