Backup
authorBenoit Papillault <benoit.papillault@free.fr>
Fri, 20 Jan 2012 20:38:45 +0000 (21:38 +0100)
committerBenoit Papillault <benoit.papillault@free.fr>
Fri, 20 Jan 2012 20:38:45 +0000 (21:38 +0100)
Makefile
gpstk-prsolution.cpp
gpstk-solution6.cpp
mygps.c
sirf3-post.cpp
sirf3.cpp

index ff70db1..e6b875d 100644 (file)
--- a/Makefile
+++ b/Makefile
@@ -1,3 +1,5 @@
+#CC    = clang
+#CXX   = clang
 # You need to install libgps first :
 # aptitude install libgps-dev
 
index 8d3dfda..aaeeaf9 100644 (file)
@@ -11,7 +11,7 @@ int main() {
        DayTime dayTime(1667, 66448.850000);
        PRSolution raimSolver;
 
-       // raimSolver.RMSLimit = 10;
+       raimSolver.RMSLimit = 10.0;
        //raimSolver.Debug = true;
 
        vector<SatID> satelliteVector;
@@ -55,10 +55,21 @@ int main() {
                cout << "y = " << raimSolver.Solution[1] << endl;
                cout << "z = " << raimSolver.Solution[2] << endl;
 
-               /* we got (the results depends on RMSLimit) : 
-                  x = 4205773.319
-                  y =  166187.065
-                  z = 4776312.107
+               /* we got (the results depends on RMSLimit)
+                  Default RMSLimit :
+                       x = 4205773.319
+                       y =  166187.065
+                       z = 4776312.107
+
+                  RMSLimit = 1.0
+                       x = 4205780.801
+                       y = 166176.178
+                       z = 4776323.217
+
+                  RMSLimit = 10.0
+                       x = 4205771.276
+                       y = 166200.837
+                       z = 4776326.902
 
                   SiRF Star III was computing (sat 2, 10 & 4) :
                     X-position : 4205766 m
index 7c02d1e..bbbcfc3 100644 (file)
@@ -5,7 +5,7 @@
 */
 #include <iostream>
 #include <iomanip>
-
+#include <algorithm>
 #include <math.h>
 
 // Class for handling satellite observation parameters RINEX files
@@ -77,7 +77,9 @@ using namespace gpstk;
 // mobile station nominal position
 //Position nominalPos_true(4201577.209,        189859.856,     4779064.567);
 //Position nominalPos_true(48.803125, 2.263235, 20, Position::Geodetic);
-Position nominalPos_true(48.803403, 2.263000, 133.0, Position::Geodetic);
+//Position nominalPos_true(48.803403, 2.263000, 133.0, Position::Geodetic);
+Position nominalPos_true(4205736.0, 166192.0, 4776266.0, Position::Cartesian); // Clamart
+//Position nominalPos_true(4166409.705, 182770.997, 4809802.953, Position::Cartesian); // CREI
 
 // class for weight 
 
@@ -165,6 +167,13 @@ public:
        };
 };
 
+bool compare_snr(pair<const SatID,typeValueMap> it1, pair<const SatID,typeValueMap> it2) {
+       double val1 = it1.second[TypeID::S1];
+       double val2 = it2.second[TypeID::S1];
+
+       return (val1 > val2);
+}
+
 void display_rinex(const gnssRinex& gRin) {
        //      cout << gRin.header.epoch
        //           << " " << gRin.header.epoch.second() << endl;
@@ -196,9 +205,13 @@ void display_position(const Position& p,
             << endl;
 }
 
-void display_solution(const gnssRinex& gData,
+Position lastPosition = nominalPos_true;;
+bool lastPositionFound = false;
+
+Position display_solution(const gnssRinex& gData,
                      const ModelObsFixedStation& model,
-                     const SolverLMS& solver) {
+                     const SolverLMS& solver,
+                     bool updateNominalPos) {
 
        static double sumX = 0;
        static double sumY = 0;
@@ -221,7 +234,7 @@ void display_solution(const gnssRinex& gData,
             << " X= " << pos.X()
             << " Y= " << pos.Y()
             << " Z= " << pos.Z()
-            << setprecision(9)
+            << setprecision(12)
             << " T= " << T.GPSfullweek() << "/" << T.GPSsecond()
             << setprecision(6)
             << " lon= " << pos.asGeodetic().getLongitude()
@@ -229,28 +242,34 @@ void display_solution(const gnssRinex& gData,
             << setprecision(0)
             << " alt= " << altitude
             << " cdt= " << solver.getSolution(TypeID::cdt)
-            << setprecision(9)
+            << setprecision(12)
             << " (" << solver.getSolution(TypeID::cdt) / C_GPS_M << ")"
             << setprecision(6)
             << " d= " << distance              
             << endl;
 
-
-       if ((0<=altitude) && (altitude<=300)) {
-               sumX += pos.X();
-               sumY += pos.Y();
-               sumZ += pos.Z();
-               count ++;
-
-               if (count > 10) {
-                       cout << "adjusting nominalPos_true" << endl;
-
-                       nominalPos_true = Position(sumX/count,
-                                                  sumY/count,
-                                                  sumZ/count,
-                                                  Position::Cartesian);
+       if (updateNominalPos) {
+               if ((0<=altitude) && (altitude<=300)) {
+                       sumX += pos.X();
+                       sumY += pos.Y();
+                       sumZ += pos.Z();
+                       count ++;
+                       
+                       if (count > 10) {
+                               cout << "adjusting nominalPos_true" << endl;
+                               
+                               nominalPos_true = Position(sumX/count,
+                                                          sumY/count,
+                                                          sumZ/count,
+                                                          Position::Cartesian);
+                       }
+
+                       lastPosition = pos;
+                       lastPositionFound = true;
                }
        }
+
+       return pos;
 }
 
 /* keep only GPS satellites (removing Glonass for instance, ... )*/
@@ -277,16 +296,13 @@ int main(int argc, const char *argv[])
        //const char navReference[] = "crei354a.11n";
 
        // second set : from rgpdata.ign.fr:/pub/data/2011/361/data_1
-       const char obsReference[] = "crei361o.11o";
-       const char obsMobile[] = "out7.ooo";
-       const char navReference[] = "brdc363z.11n";
+       const char obsReference[] = "crei001l.12o";
+       const char obsMobile[] = /*"crei001l.12o" */ "out7.ooo";
+       const char navReference[] = "brdc002z.12n";
        const char sp3File[] = "igu16682_12.sp3";
        
        // reference station nominal position
        Position nominalPosRef  (4166409.705,   182770.997,     4809802.953);
-       // initial set to nominalPos
-       Position lastPosition = nominalPosRef;
-       bool lastPositionFound = false;
 
        ////////// Initialization phase //////////
 
@@ -336,7 +352,7 @@ int main(int argc, const char *argv[])
        // Declare a simple filter object. By default, it filters C1 with
        // default limits
        SimpleFilter filter;
-       filter.setMaxLimit(100000000.0);
+       //filter.setMaxLimit(10000000.0);
 
        // This is the GNSS data structure that will hold all the
        // GNSS-related information
@@ -392,6 +408,15 @@ int main(int argc, const char *argv[])
 
       ////////////////////////////////////////
 
+   /// Case 6 objects
+   // Declare a new Kalman solver
+   CodeKalmanSolver solverK4, solverK6;
+
+   WhiteNoiseModel wnmX(100.0);
+   WhiteNoiseModel wnmT(100.0);
+   solverK6.setCoordinatesModel(&wnmX);
+   solverK6.setReceiverClockModel(&wnmT);
+
       //////////// CASE #10 OBJECTS ////////////
 
       // This is the GNSS data structure that will hold the
@@ -421,6 +446,7 @@ int main(int argc, const char *argv[])
    model.setDefaultIonoModel(ionoStore);
    model.setDefaultTropoModel(mopsTM);
    model.setDefaultEphemeris(/*sp3Store*/bceStore);
+   model.setModelPrepared(false);
    //model.setMinElev(1.0);
 
       // Declare the appropriate modeler object for a reference station
@@ -444,8 +470,8 @@ int main(int argc, const char *argv[])
       // Create an object to synchronize rover and reference station
       // data streams. This object will take data out from "rinRef" until
       // it is synchronized with data in "gOriginal". Default synchronization
-      // tolerance is 1 s.
-   Synchronize synchro(rinRef, gOriginal);
+      // tolerance is 0.5 s (with a 1s tolerance, we get samples 1s apart...
+   Synchronize synchro(rinRef, gOriginal, 0.5);
 
       //////////////////////////////////////////////
 
@@ -544,13 +570,11 @@ int main(int argc, const char *argv[])
    
           myModelRef.Prepare(nominalPosRef);
           if (lastPositionFound) {
-                  model.Prepare(lastPosition);
+                  //model.setModelPrepared(false); //Prepare(lastPosition);
                   myModel.Prepare(lastPosition);
           } else {
-                  model.setModelPrepared(false);
+                  //model.setModelPrepared(false);
                   myModel.setModelPrepared(false);
-                  //model.Prepare(nominalPosRef);
-                  //myModel.Prepare(nominalPosRef);
           }
 
           // CASE #0
@@ -575,12 +599,13 @@ int main(int argc, const char *argv[])
              
           //cout << "  Case 0 :";
                   //display_solution(gRin0, myModel, solver);
-
+          /*
                   double altitude = pos.asGeodetic().getAltitude();
                   if ((0.0 < altitude) && (altitude < 300.0))  {
                           lastPosition = pos;
                           lastPositionFound = true;
                   }
+          */
           }
           catch (Exception e) {
                   cout << "Case 0. Exception : " << e.what() << endl;
@@ -596,6 +621,8 @@ int main(int argc, const char *argv[])
 
       try {
             // This is the line that will process all the GPS data
+             //model.Prepare(gRin1);
+             //model.Prepare(nominalPos_true);
              gRin1 >> filter >> model >> solver;
 
             // - First, a basic filter to screen out very bad observables
@@ -604,14 +631,8 @@ int main(int argc, const char *argv[])
             // - Third, solve the equations using a simple Least-Mean-Squares
             //   solver
 
-             // Get your results out of the solver object. In ECEF system
-             // by default
-             Position pos(model.rxPos.X() + solver.getSolution(TypeID::dx),
-                          model.rxPos.Y() + solver.getSolution(TypeID::dy),
-                          model.rxPos.Z() + solver.getSolution(TypeID::dz));
-             
-             //cout << "  Case 1 :";
-             //display_solution(gRin1, model, solver);
+             cout << "  Case 1 :";
+             display_solution(gRin1, model, solver, false);
       }
       catch(...)
       {
@@ -627,27 +648,13 @@ int main(int argc, const char *argv[])
          // In this case we process data using C1 + Weighted Least Mean Squares
          // solver (WMS)
 
-         // Let's make a working copy
-      gnssRinex gRin3(gOriginal);
-
       try  {
+             // Let's make a working copy
+             gnssRinex gRin3(gOriginal);
 
              gRin3 >> filter >> model >> mopsW >> solverWMS;
-            // The "mopsW" object computes weights based on MOPS algorithm
-            // The "solverWMS" object solves the system using Weighted Least
-            // Mean Squares. It is already configured to work with NEU system.
-
-             Position pos((model.rxPos.X()+solverWMS.getSolution(TypeID::dx)),
-                          (model.rxPos.Y()+solverWMS.getSolution(TypeID::dy)),
-                          (model.rxPos.Z()+solverWMS.getSolution(TypeID::dz)));
-             
              cout << "  Case 3 :";
-             display_solution(gRin3, model, solverWMS);
-             double altitude = pos.asGeodetic().getAltitude();
-             if ((0.0 < altitude) && (altitude < 300.0))  {
-                     lastPosition = pos;
-                     lastPositionFound = true;
-             }
+             display_solution(gRin3, model, solverWMS, false);
       }
       catch (Exception e)
       {
@@ -656,19 +663,15 @@ int main(int argc, const char *argv[])
 
    ////////////////////////// END OF CASE #3  //////////////////////////
 
-      // Case #4 : use elevation as weight
+      // Case #4 : C1 + MOPS Weight + Kalman filter
          // Let's make a working copy
       gnssRinex gRin4(gOriginal);
 
       try  {
-             gRin4 >> filter >> model >> elevationW >> solverWMS;
-
-             Position pos((model.rxPos.X()+solverWMS.getSolution(TypeID::dx)),
-                          (model.rxPos.Y()+solverWMS.getSolution(TypeID::dy)),
-                          (model.rxPos.Z()+solverWMS.getSolution(TypeID::dz)));
-             
-             //cout << "  Case 4 :";
-             //display_solution(gRin4, model, solverWMS);
+             gRin4 >> filter >> model >> mopsW >> solverK4;
+             cout << "  Case 4 :";
+             Position pos = display_solution(gRin4, model, solverK4, false);
+             model.Prepare(pos);
       }
       catch (Exception e)
       {
@@ -681,19 +684,49 @@ int main(int argc, const char *argv[])
 
       try  {
              gRin5 >> filter >> model >> snrW >> solverWMS;
-             //cout << "  Case 5 :";
-             //display_solution(gRin5, model, solverWMS);
+             cout << "  Case 5 :";
+             display_solution(gRin5, model, solverWMS, false);
+
+             // take only the 4 best satellites, based on SNR
+             /*
+             gnssRinex gRin5_bis(gOriginal);
+             sort (gRin5_bis.body.begin(), gRin5_bis.body.end(), compare_snr);
+             for (satTypeValueMap::iterator it = gRin5_bis.body.begin();
+                  it != gRin5_bis.body.end(); ++it) {
+                     double old_val = (*it).second[TypeID::S1];
+             }
+             
+             cout << "gRin5_bis:" << endl;
+             display_rinex(gRin5_bis);
+             gRin5_bis >> filter >> model >> snrW >> solverWMS;
+             cout << "  Case 5_bis :";
+             display_solution(gRin5_bis, model, solverWMS);
+             */
       }
       catch (Exception e)
       {
              cout << "Exception : " << e.what() << endl;
       }
 
-      continue;
+      // Case #6 : use SNR as weight + Kalman solver
+      gnssRinex gRin6(gOriginal);
+
+      try  {
+             gRin6 >> filter >> model >> snrW >> solverK6;
+             cout << "  Case 6 :";
+             Position pos = display_solution(gRin6, model, solverK6, false);
+             //model.Prepare(pos);
+      }
+      catch (Exception e)
+      {
+             cout << "Exception : " << e.what() << endl;
+      }
 
 
    //////////////////////////// CASE #10  ////////////////////////////
 
+      continue;
+
 
          // This is like cases #1 and #2, but using DGPS techniques instead.
 
@@ -703,7 +736,11 @@ int main(int argc, const char *argv[])
          // First, let's synchronize and process reference station data
       try
       {
-             gRef >> synchro >> filter >> myModelRef;
+             gRef >> synchro;
+             gRef = keepOnlySatGPS(gRef);
+             gRef >> filter >> modelRef;
+             cout << "gRef:" << endl;
+             display_rinex(gRef);
             // Please note that the FIRST STEP is to synchronize "gRef", the
             // reference station data stream, with "gOriginal" (or with gRin10,
             // which is the same), the rover receiver data stream.
@@ -742,10 +779,12 @@ at epoch: " << gRef.header.epoch << endl;
                           (myModel.rxPos.Y()+solver.getSolution(TypeID::dy)),
                           (myModel.rxPos.Z()+solver.getSolution(TypeID::dz)));
              
-             cout << "  Case 10:";
-             display_solution(gRin10, myModel, solver);
+             //cout << "  Case 10:";
+             //display_solution(gRin10, myModel, solver);
+             /*
              lastPosition = pos;
              lastPositionFound = true;
+             */
       }
       catch(...)
       {
@@ -774,9 +813,8 @@ at epoch: " << gRef.header.epoch << endl;
     try {
            // Like case #10, but now with "mopsW" and "solverWMS"
            gRin11 >> filter >> myModel >> delta >> mopsW  >> solverWMS;
-           cout << "  Case 11:";
-           //display_position(pos, nominalPos_true);
-           display_solution(gRin11, myModel, solverWMS);
+           //cout << "  Case 11:";
+           //display_solution(gRin11, myModel, solverWMS);
       }
       catch(...)
       {
@@ -807,8 +845,8 @@ at epoch: " << gRef.header.epoch << endl;
              if (i > 1) {
                      gRin12 >> filter >> myModel >> delta
                             >> mopsW  >> solverK12;
-                     cout << "  Case 12:";
-                     display_solution(gRin12, myModel, solverK12);
+                     //cout << "  Case 12:";
+                     //display_solution(gRin12, myModel, solverK12);
              }
       }
       catch(...)
@@ -820,28 +858,28 @@ at epoch: " << gRef.header.epoch << endl;
    ////////////////////////// END OF CASE #12  //////////////////////////
 
 
-      /// Case #13
+      /// Case #13 : DGPS + snrW
       // Let's make a working copy
       gnssRinex gRin13(gOriginal);
 
       try {
              gRin13 >> filter >> myModel >> delta >> elevationW >> solverWMS;
-             cout << "  Case 13:";
-             display_solution(gRin13, myModel, solverWMS);
+             //cout << "  Case 13:";
+             //display_solution(gRin13, myModel, solverWMS);
       }
       catch(...)
       {
              cout << "Exception #13 " << endl;
       }
 
-      /// Case #14 : DGPS + iono/trop + WMS solver
+      /// Case #14 : DGPS + iono/trop + WMS solver based on SNR
       // Let's make a working copy
       gnssRinex gRin14(gOriginal);
 
       try {
-             gRin14 >> filter >> model >> delta >> mopsW >> solverWMS;
+             gRin14 >> filter >> model >> delta >> snrW >> solverWMS;
              cout << "  Case 14:";
-             display_solution(gRin14, model, solverWMS);
+             display_solution(gRin14, model, solverWMS, false);
       }
       catch(...)
       {
diff --git a/mygps.c b/mygps.c
index 9202823..23ca05c 100644 (file)
--- a/mygps.c
+++ b/mygps.c
@@ -241,7 +241,7 @@ int main(int argc, const char *argv[]) {
     
     /* NULL, NULL matches localhost and the default GPSD port */
 #if GPS_API_MAJOR_VERSION < 5
-    if ((gps = gps_open("192.168.38.2", NULL)) == NULL) {
+    if ((gps = gps_open(NULL, NULL)) == NULL) {
 #endif
 #if GPSD_API_MAJOR_VERSION == 5
       if (gps_open(NULL, NULL, &gpsdata) != 0) {
index a5d2275..27196b2 100644 (file)
@@ -7,6 +7,7 @@
 #include "RinexObsHeader.hpp"
 #include "RinexObsData.hpp"
 
+#include "Position.hpp"
 #include "DayTime.hpp"
 #include "icd_200_constants.hpp"
 
@@ -49,9 +50,127 @@ double prev_diff_tt_sec = 0.0;
 
 double diff_diff_tt_sec = 0.0;
 
-/* for rinex stuff */
-int last_gps_week = 0;
-double last_gps_second = 0.0;
+Position nominalPos(4205736.0, 166192.0, 4776266.0, Position::Cartesian);      // Clamart
+
+#define RECORD_NR 1000
+#define SAT_NR 100
+#define UNUSED -1
+
+struct satellite {
+       int satID; /* -1 if unused */
+       #define MID_28_RECEIVED 0x01
+       #define MID_30_RECEIVED 0x02
+       int flag;
+       unsigned int tt;
+       double gps_second;
+       double pseudorange;
+       double tit;
+       double snr_avg;
+};
+
+struct record {
+       /* from MID 2 */
+       double posx, posy, posz;
+       double velx, vely, velz;
+       /* from MID 7 */
+       int gps_week;
+       double gps_second;
+       double bias;
+       /* from MID 28 */
+       double sat_second;
+       unsigned int sat_tt;
+       struct satellite sat[SAT_NR];
+       /* from MID 30 */
+       double sat_second_t0;
+       /* computed */
+       double bias_diff;
+       double bias_diff_diff;
+
+       double sat_second_diff;
+       double sat_second_diff_diff;
+
+       int sat_tt_diff;
+       int sat_tt_diff_diff;
+
+       double sat_second_t0_diff;
+       double sat_second_t0_diff_diff;
+
+       double my_bias;
+       double my_bias_diff;
+       double my_bias_diff_diff;
+};
+
+struct record record_tab[RECORD_NR];
+int record_nr = 0;
+
+void record_init(struct record *r) {
+       int i;
+
+       memset(r, 0, sizeof(*r));
+
+       for (i=0; i<SAT_NR; i++)
+               r->sat[i].satID = UNUSED;
+}
+
+void record_all_init() {
+       int i;
+
+       for (i=0; i<RECORD_NR; i++)
+               record_init(&record_tab[i]);
+}
+
+void record_add_mid_2(struct record *r,
+                     double gps_second, double posx, double posy, double posz,
+                     double velx, double vely, double velz) {
+       r->posx = posx;
+       r->posy = posy;
+       r->posz = posz;
+       r->velx = velx;
+       r->vely = vely;
+       r->velz = velz;
+}
+
+void record_add_mid_7(struct record * r,
+                     int gps_week, double gps_second, double bias) {
+       r->gps_week = gps_week;
+       r->gps_second = gps_second;
+       r->bias = bias;
+}
+
+void record_add_mid_28(struct record *r,
+                      int satID, unsigned int tt,
+                      double gps_second, double pseudorange,
+                      double tit, double snr_avg) {
+       if ((0<=satID) && (satID<SAT_NR)) {
+
+               r->sat_second = gps_second;
+               r->sat_tt = tt;
+
+               r->sat[satID].satID = satID;
+               r->sat[satID].flag |= MID_28_RECEIVED;
+               r->sat[satID].tt = tt;
+               r->sat[satID].gps_second = gps_second;
+               r->sat[satID].pseudorange = pseudorange;
+               r->sat[satID].tit = tit;
+               r->sat[satID].snr_avg = snr_avg;
+       }
+}
+
+int record_add_mid_30(struct record *r,
+                     int satID,
+                     double gps_second, double posx, double posy, double posz,
+                     double velx, double vely, double velz)
+{
+       if ((0<=satID) && (satID<SAT_NR)) {
+               r->sat[satID].flag |= MID_30_RECEIVED;
+               /* TBD */
+       }
+
+       r->sat_second_t0 = gps_second;
+
+       return 0;
+}
+
 
 int rinex_open(const char *filename) {
        r_out.open(filename, ios::out|ios::trunc);
@@ -142,13 +261,6 @@ int rinex_data_write() {
        if (r_data.obs.size() == 0)
                return 0;
 
-       // fill in the missing pieces
-       r_data.epochFlag = 0;
-       r_data.time.setGPSfullweek(last_gps_week, last_gps_second);
-       r_data.numSvs = r_data.obs.size();
-       r_data.clockOffset = 0.0;
-       /* r_data_.obs = set by rinex_data_add_pseudorange */
-       
        // write the data epoch
        r_out << r_data;
 
@@ -160,7 +272,6 @@ int rinex_data_write() {
 
 /* from Message ID 28 */
 int rinex_data_add_pseudorange_snr(int satNR,
-                                  double gps_second,
                                   double pseudorange,
                                   double snr) {
 
@@ -171,9 +282,6 @@ int rinex_data_add_pseudorange_snr(int satNR,
        if (ssi > 9)
                ssi = 9;
 
-       // we only set GPS second
-       last_gps_second = gps_second;
-
        SatID sat(satNR, SatID::systemGPS);
        r_data.obs[sat][RinexObsHeader::C1].data = pseudorange;
        r_data.obs[sat][RinexObsHeader::C1].lli = 0;
@@ -187,67 +295,13 @@ int rinex_data_add_pseudorange_snr(int satNR,
 }
 
 /* from Message ID 7 */
-int rinex_data_add_time_clock_bias(int gps_week,
-                                  double gps_second, double clock_bias) {
+int rinex_data_add_time(int gps_week, double gps_second) {
        
-       // set extended GPS week only
-       last_gps_week = gps_week;
-       
-       // adjust pseudorange
-       //last_gps_second = floor(gps_second) + clock_bias;
-       last_gps_second = gps_second;
-
-       /* compute min,max pseudorange */
-       double pseudorange_min, pseudorange_max;
-       int first = 1;
-
-       RinexObsData::RinexSatMap::iterator it;
-       for (it = r_data.obs.begin(); it != r_data.obs.end(); ++it) {
-               double range = (*it).second[RinexObsHeader::C1].data;
-               if (first) {
-                       first = 0;
-                       pseudorange_min = range;
-                       pseudorange_max = range;
-               } else {
-                       if (pseudorange_min > range)
-                               pseudorange_min = range;
-                       if (pseudorange_max < range)
-                               pseudorange_max = range;
-               }
-       }
-
-       printf("  pseudorange min - max %.6f - %.6f\n",
-              pseudorange_min, pseudorange_max);
-
-       /* make sure pseudorange are in [15000000 - 30000000] */
-       #define PSEUDORANGE_MIN 15000000.0
-       #define PSEUDORANGE_MAX 30000000.0
-       if ((pseudorange_min < PSEUDORANGE_MIN) &&
-           (pseudorange_max > PSEUDORANGE_MAX)) {
-               printf("  impossible pseudorange adjustement detected\n");
-       }
-
-       if (pseudorange_min < PSEUDORANGE_MIN) {
-               /* TBD */
-       }
-
-       if (pseudorange_max > PSEUDORANGE_MAX) {
-               double diff = pseudorange_max - PSEUDORANGE_MAX;
-               double old_gps_second = last_gps_second;
-
-               printf("RINEX : GPS second %.12f => %.12f\n",
-                      old_gps_second, last_gps_second);
-               RinexObsData::RinexSatMap::iterator it;
-               for (it = r_data.obs.begin(); it != r_data.obs.end(); ++it) {
-                       double old_val = (*it).second[RinexObsHeader::C1].data;
-                       double new_val = old_val - diff;
-                       (*it).second[RinexObsHeader::C1].data = new_val;
-
-                       printf("RINEX satID %3u : pseudorange adjust %f => %f\n",
-                              (*it).first.id,
-                              old_val, new_val);
-               }
-       }
+       // fill in the missing pieces
+       r_data.epochFlag = 0;
+       r_data.time.setGPSfullweek(gps_week, gps_second);
+       r_data.numSvs = r_data.obs.size();
+       r_data.clockOffset = 0.0;
 
        /* write data */
        rinex_data_write();
@@ -271,22 +325,38 @@ double tab_get_avg() {
        return avg;
 }
 
+void parse_mid_2(const char *buf) {
+       int mid;
+       double gps_second, posx, posy, posz, velx, vely, velz;
+
+       sscanf(buf,
+              "%d %lf %lf %lf %lf %lf %lf %lf",
+              &mid, &gps_second, &posx, &posy, &posz, &velx, &vely, &velz);
+
+       if (record_nr < RECORD_NR) {
+               record_add_mid_2(&record_tab[record_nr],
+                                gps_second, posx, posy, posz, velx, vely, velz);
+       }
+}
+
 void parse_mid_7(const char *buf) {
-       int gps_week;
+       int mid, gps_week;
        double gps_second, bias;
        int diff_tt, diff_diff_tt;
        double diff_gps_second, diff_diff_gps_second;
 
        mid_7_count ++;
 
-       sscanf(buf, "7 %u %lf %lf",
-               &gps_week, &gps_second, &bias);
+       sscanf(buf, "%d %d %lf %lf",
+              &mid, &gps_week, &gps_second, &bias);
 
        printf("MID 7 : %u %.12f %.12f [mid_7_count %d]\n",
               gps_week, gps_second, bias, mid_7_count);
 
-       rinex_data_add_time_clock_bias(gps_week, gps_second,
-                                      bias = local_second - gps_second);
+       if ((record_nr < RECORD_NR) /*&& (mid_7_count > 1000)*/) {
+               record_add_mid_7(&record_tab[record_nr++],
+                                gps_week, gps_second, bias);
+       }
 
        diff_tt = last_tt - prev_tt;
        diff_diff_tt = diff_tt - prev_diff_tt;
@@ -396,16 +466,17 @@ void parse_mid_7(const char *buf) {
 }
 
 void parse_mid_28(const char *buf) {
-       int satID;
+       int mid, satID;
        unsigned int tt;
        double gps_second, pseudorange, tit, snr_avg;
 
-       sscanf(buf,"28 %d %u %lf %lf %lf %lf\n",
-              &satID, &tt, &gps_second, &pseudorange, &tit, &snr_avg);
+       sscanf(buf,"%d %d %u %lf %lf %lf %lf\n",
+              &mid, &satID, &tt, &gps_second, &pseudorange, &tit, &snr_avg);
 
-       if (tit > 1.0) {
-               rinex_data_add_pseudorange_snr(satID, gps_second,
-                                              pseudorange, snr_avg);
+       if ((record_nr < RECORD_NR) /*&& (mid_7_count > 1000)*/) {
+               record_add_mid_28(&record_tab[record_nr],
+                                 satID, tt, gps_second, pseudorange,
+                                 tit, snr_avg);
        }
 
        printf("MID 28 : %2d %10u %19.12f %18.6f %6.3f %6.3f\n",
@@ -415,6 +486,160 @@ void parse_mid_28(const char *buf) {
        local_second = gps_second;
 }
 
+void parse_mid_30(const char *buf) {
+       int mid, satID;
+       double gps_second, posx, posy, posz, velx, vely, velz;
+
+       sscanf(buf, "%d %d %lf %lf %lf %lf %lf %lf %lf\n",
+              &mid, &satID, &gps_second, &posx, &posy, &posz, &velx, &vely, &velz);
+
+       if ((record_nr < RECORD_NR)) {
+               record_add_mid_30(&record_tab[record_nr], satID,
+                                 gps_second, posx, posy, posz, velx, vely, velz);
+       }
+}
+
+double distance(double x1, double y1, double z1,
+               double x2, double y2, double z2) {
+       return (sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)+(z1-z2)*(z1-z2)));
+}
+
+void print_data() {
+       int i,j;
+
+       printf("Printing data ...\n");
+
+       for (i=0; i<record_nr; i++) {
+               struct record *r = &record_tab[i];
+
+               printf("record %4d :\n", i);
+               printf("  gps_week %u gps_second %.12f bias %.12f\n",
+                      r->gps_week, r->gps_second, r->bias);
+               printf("  sat_second\t%.12f diff %.12f diff_diff %.12f\n",
+                      r->sat_second,
+                      r->sat_second_diff, r->sat_second_diff_diff);
+               printf("  sat_tt\t%10u diff %10d diff_diff %10d\n",
+                      r->sat_tt, r->sat_tt_diff, r->sat_tt_diff_diff);
+               printf("  sat_second_t0\t%.12f diff %.12f diff_diff %.12f\n",
+                      r->sat_second_t0, r->sat_second_t0_diff, r->sat_second_t0_diff_diff);
+               printf("  bias\t%.12f diff %.12f diff_diff %.12f\n",
+                      r->bias, r->bias_diff, r->bias_diff_diff);
+               printf("  my_bias\t%.12f diff %.12f diff_diff %.12f\n",
+                      r->my_bias, r->my_bias_diff, r->my_bias_diff_diff);
+               printf("  distance %.3f\n",
+                      distance(nominalPos.X(), nominalPos.Y(), nominalPos.Z(),
+                               r->posx, r->posy, r->posz));
+               for (j=0; j<SAT_NR; j++) {
+                       if (r->sat[j].satID != UNUSED) {
+                               printf("  satID %2u tt=%u gps_second %.12f "
+                                      "pseudorange %f tit %4.1f snr_avg %4.1f\n",
+                                      r->sat[j].satID,
+                                      r->sat[j].tt,
+                                      r->sat[j].gps_second,
+                                      r->sat[j].pseudorange,
+                                      r->sat[j].tit,
+                                      r->sat[j].snr_avg);
+                       }
+               }
+       }
+}
+
+void save_data() {
+       int i,j;
+
+       printf("Saving data ...\n");
+
+       for (i=0; i<record_nr; i++) {
+               struct record *r = &record_tab[i];
+
+               /* condition on record :
+                4 satellites with tit >= 30.0 */
+
+               int usable_sat = 0;
+               for (j=0; j<SAT_NR; j++) {
+                       if (r->sat[j].satID == UNUSED)
+                               continue;
+                       if (r->sat[j].tit < 30.0)
+                               continue;
+
+                       usable_sat++;
+               }
+
+               //if (usable_sat < 4)
+               //      continue;
+
+               for (j=0; j<SAT_NR; j++) {
+                       if (r->sat[j].satID == UNUSED)
+                               continue;
+                       //if (r->sat[j].tit < 30.0)
+                       //      continue;
+                       if ((r->sat[j].flag &
+                            (MID_28_RECEIVED | MID_30_RECEIVED)) != 
+                           (MID_28_RECEIVED | MID_30_RECEIVED))
+                               continue;
+
+                       //double bias = r->bias;
+                       //double bias = r->sat_second - r->gps_second;
+                       //double bias = r->sat_second - r->sat_second_t0;
+                       //r->sat[j].pseudorange -= bias * C_GPS_M,
+                               
+                       rinex_data_add_pseudorange_snr(r->sat[j].satID,
+                                                      r->sat[j].pseudorange,
+                                                      r->sat[j].snr_avg);
+               }
+
+               rinex_data_add_time(r->gps_week, r->sat_second);
+               printf("  wrote record %4d : GPS second : %.12f\n", i, r->gps_second);
+       }
+
+       //rinex_close();
+}
+
+
+void process_data() {
+       int i;
+
+       printf("Processing data ...\n");
+
+       for (i=0; i<record_nr; i++) {
+               struct record *r = &record_tab[i];
+
+               r->my_bias = r->sat_second - r->sat_second_t0;
+
+               if (i>=1) {
+                       struct record *r_p = &record_tab[i-1];
+                       r->bias_diff = r->bias - r_p->bias;
+                       r->sat_second_diff = r->sat_second - r_p->sat_second;
+                       r->sat_tt_diff = r->sat_tt - r_p->sat_tt;
+                       r->sat_second_t0_diff = r->sat_second_t0 - r_p->sat_second_t0;
+                       r->my_bias_diff = r->my_bias - r_p->my_bias;
+                       if (i>=2) {
+                               r->bias_diff_diff = r->bias_diff -
+                                       r_p->bias_diff;
+                               r->sat_second_diff_diff = r->sat_second_diff -
+                                       r_p->sat_second_diff;
+                               r->sat_tt_diff_diff = r->sat_tt_diff -
+                                       r_p->sat_tt_diff;
+                               r->sat_second_t0_diff_diff = r->sat_second_t0_diff -
+                                       r_p->sat_second_t0_diff;
+                               r->my_bias_diff_diff = r->my_bias_diff - r_p->my_bias_diff;
+                       } else {
+                               r->bias_diff_diff = 0.0;
+                               r->sat_second_diff_diff = 0.0;
+                               r->sat_tt_diff_diff = 0.0;
+                               r->sat_second_t0_diff_diff = 0.0;
+                               r->my_bias_diff_diff = 0.0;
+                       }
+               } else {
+                       r->bias_diff = 0.0;
+                       r->sat_second_diff = 0.0;
+                       r->sat_tt_diff = 0.0;
+                       r->sat_second_t0_diff = 0.0;
+                       r->my_bias_diff = 0.0;
+               }
+       }
+}
+
 void usage() {
        printf("sirf3-post file.raw [-rinex file.obs]\n");
        exit (-1);
@@ -426,6 +651,8 @@ int main(int argc, const char *argv[]) {
        char buf[1024];
        int i;
 
+       record_all_init();
+
        for (i=1; i<argc; i++) {
                if (strcmp(argv[i],"-rinex")==0 && i+1<argc) {
                        const char * rinex = argv[++i];
@@ -451,18 +678,34 @@ int main(int argc, const char *argv[]) {
                }
 
                switch (mid) {
+               case 2:
+                       parse_mid_2(buf);
+                       break;
                case 7:
                        parse_mid_7(buf);
                        break;
                case 28:
                        parse_mid_28(buf);
                        break;
+               case 30:
+                       parse_mid_30(buf);
+                       break;
                default:
                        printf("unknown MID %d\n", mid);
                        break;
                }
+
+               /* stop after a while ;-) */
+               if (record_nr >= RECORD_NR)
+                       break;
        }
 
+       printf("Record of data : \n");
+
+       process_data();
+       save_data();
+       print_data();
+
        fclose (file);
        return 0;
 }
index 57fbabe..f559ad8 100644 (file)
--- a/sirf3.cpp
+++ b/sirf3.cpp
@@ -138,20 +138,21 @@ int decode_sirf_msg_2(unsigned char * buf, int n)
        unsigned char mode;
        const char * altmode = "";
        const char * pmode = "";
+       double gps_second, posx, posy, posz, velx, vely, velz;
 
        printf("Measure Navigation Data Out\n");
        p += 1;
-       printf("  X-position : %d m\n", get_sirf_4s(buf+p));
+       printf("  X-position : %f m\n", posx=get_sirf_4s(buf+p));
        p += 4;
-       printf("  Y-position : %d m\n", get_sirf_4s(buf+p));
+       printf("  Y-position : %f m\n", posy=get_sirf_4s(buf+p));
        p += 4;
-       printf("  Z-position : %d m\n", get_sirf_4s(buf+p));
+       printf("  Z-position : %f m\n", posz=get_sirf_4s(buf+p));
        p += 4;
-       printf("  X-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
+       printf("  X-velocity : %.3f m/s\n", velx=(double)get_sirf_2s(buf+p) / 8.0);
        p += 2;
-       printf("  Y-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
+       printf("  Y-velocity : %.3f m/s\n", vely=(double)get_sirf_2s(buf+p) / 8.0);
        p += 2;
-       printf("  Z-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
+       printf("  Z-velocity : %.3f m/s\n", velz=(double)get_sirf_2s(buf+p) / 8.0);
        p += 2;
        /* Mode 1 */
        mode = buf[p];
@@ -201,9 +202,9 @@ int decode_sirf_msg_2(unsigned char * buf, int n)
        mode = buf[p];
        printf("  Mode 2 : %02x\n", mode);
        p += 1;
-       printf("  GPS Week : %u\n", get_sirf_2u(buf+p));
+       printf("  GPS week : %u\n", get_sirf_2u(buf+p));
        p += 2;
-       printf("  GPS Time of Week : %f\n", (double)get_sirf_4u(buf+p)/100.0);
+       printf("  GPS second : %.12f\n", gps_second=(double)get_sirf_4u(buf+p)*0.01);
        p += 4;
        printf("  Satellites used : %d\n", n_sat = buf[p]);
        p += 1;
@@ -211,6 +212,10 @@ int decode_sirf_msg_2(unsigned char * buf, int n)
                        printf("  Channel %2d SatID: %3u\n", i+1, buf[p+i]);
        }
 
+       fprintf(file,
+               "%3u %.12f %.3f %.3f %.3f %.3f %.3f %.3f\n",
+               2, gps_second, posx, posy, posz, velx, vely, velz);
+
        return 0;
 }
 
@@ -220,10 +225,10 @@ int decode_sirf_msg_4(unsigned char *buf, int n)
 
        printf("Measured Tracker Data Out\n");
        p += 1;
-       printf("  GPS Week : %d\n", get_sirf_2s(buf + p));
+       printf("  GPS week : %d\n", get_sirf_2s(buf + p));
        p += 2;
-       printf("  GPS Time of Week : %.3f\n",
-              (double)get_sirf_4u(buf + p) / 100.0);
+       printf("  GPS second : %.12f\n",
+              (double)get_sirf_4u(buf + p) * 0.01);
        p += 4;
        printf("  Channels : %d\n", buf[p]);
        p += 1;
@@ -251,20 +256,19 @@ int decode_sirf_msg_7(unsigned char *buf, int n)
 
        printf("Clock Status Data\n");
        p += 1;
-       printf("  Extended GPS Week : %u\n",
-              gps_week = get_sirf_2u(buf + p));
+       printf("  GPS week : %u\n", gps_week = get_sirf_2u(buf + p));
        p += 2;
-       printf("  GPS Time of Week : %f s\n",
-              gps_second = (double)get_sirf_4u(buf + p) / 100.0);
+       printf("  GPS second : %.12f s\n",
+              gps_second = (double)get_sirf_4u(buf + p) * 0.01);
        p += 4;
        printf("  Satellites used : %u\n", buf[p]);
        p += 1;
        printf("  Clock Drift : %u Hz\n", get_sirf_4u(buf + p));
        p += 4;
        bias = (double)get_sirf_4u(buf + p) * 1e-9;
-       printf("  Clock Bias : %.9f ns\n", bias);
+       printf("  Clock Bias : %.12f ns\n", bias);
        p += 4;
-       printf("  Estimated GPS Time : %u ms\n", get_sirf_4u(buf + p));
+       printf("  Estimated GPS second : %.12f ms\n", get_sirf_4u(buf + p) * 0.001);
        p += 4;
 
        /* record : 7 <GPS week> <GPS second> <bias> */
@@ -280,7 +284,7 @@ int decode_sirf_msg_8(unsigned char *buf, int n)
 
        printf("50 BPS Data\n");
        p += 1;
-       printf("  Channel %d, SV# %d\n", buf[p], buf[p+1]);
+       printf("  Channel %d, SatID %d\n", buf[p], buf[p+1]);
        p += 2;
        for (i=0; i<10; i++) {
                printf("  Word %d : %08x\n", i, get_sirf_4u(buf+p));
@@ -343,12 +347,12 @@ int decode_sirf_msg_28(unsigned char *buf, int n)
        tt=get_sirf_4u(buf+p);
        tt_in_seconds = (double)tt / (double)gps_clock;
        /* divided by GPS clock freq */
-       printf("  Time Tag = 0x%08x (%.9f s)\n", tt, tt_in_seconds);
+       printf("  Time Tag = %u (%.9f s)\n", tt, tt_in_seconds);
        p += 4;
        printf("  SatID : %3u\n", satID=buf[p]);
        p += 1;
        gps_second=get_sirf_dbl(buf+p);
-       printf("  GPS SW Time : %.9f s [delta with TT : %.9f s]\n",
+       printf("  GPS SW Time : %.12f s [delta with TT : %.12f s]\n",
               gps_second, gps_second - tt_in_seconds);
        p += 8;
        printf("  Pseudorange = %.3f m\n", pseudorange = get_sirf_dbl(buf+p));
@@ -383,6 +387,44 @@ int decode_sirf_msg_28(unsigned char *buf, int n)
        return 0;
 }
 
+int decode_sirf_msg_30(unsigned char *buf, int n)
+{
+       int p = 0, satID;
+       double gps_second, posx, posy, posz, velx, vely, velz;
+
+       printf("Navigation Library SV State Data - SatID : %3u\n", satID = buf[p+1]);
+       p += 2;
+       printf("  GPS second : %.12f s\n", gps_second = get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Position X : %f m\n", posx = get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Position Y : %f m\n", posy = get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Position Z : %f m\n", posz = get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Velocity X : %f m/s\n", velx = get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Velocity Y : %f m/s\n", vely = get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Velocity Z : %f m/s\n", velz = get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Clock bias : %.12f s\n", get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Clock drift : %.12f s/s\n", get_sirf_sgl(buf+p));
+       p += 4;
+       p += 1;
+       p += 4;
+       p += 4;
+       printf("  Ionospheric delay : %f m\n", get_sirf_sgl(buf+p));
+       p += 4;
+
+       fprintf(file,
+               "%3u %3u %.12f %.3f %.3f %.3f %.3f %.3f %.3f\n",
+               30, satID, gps_second, posx, posy, posz, velx, vely, velz);
+
+       return 0;
+}
+
 int decode_sirf_msg_31(unsigned char *buf, int n)
 {
        printf("Navigation Library Initialization Data\n");
@@ -401,7 +443,7 @@ int decode_sirf_msg_41(unsigned char *buf, int n)
        p += 2;
        printf("  GPS week : %u\n", get_sirf_2u(buf + p));
        p += 2;
-       printf("  GPS seconds : %.3f\n", (double)get_sirf_4u(buf + p) * 0.001);
+       printf("  GPS second : %.12f\n", (double)get_sirf_4u(buf + p) * 0.001);
        p += 4;
                
        printf("  Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
@@ -522,31 +564,7 @@ int decode_sirf_msg(unsigned char *buf, int n, int *ack) {
                break;
                
        case 30: /* Navigation Library SV State Data */
-               printf("Navigation Library SV State Data - SatID : %3u\n", buf[p+1]);
-               p += 2;
-               printf("  GPS Time : %f s\n", get_sirf_dbl(buf+p));
-               p += 8;
-               printf("  Position X : %f m\n", get_sirf_dbl(buf+p));
-               p += 8;
-               printf("  Position Y : %f m\n", get_sirf_dbl(buf+p));
-               p += 8;
-               printf("  Position Z : %f m\n", get_sirf_dbl(buf+p));
-               p += 8;
-               printf("  Velocity X : %f m/s\n", get_sirf_dbl(buf+p));
-               p += 8;
-               printf("  Velocity Y : %f m/s\n", get_sirf_dbl(buf+p));
-               p += 8;
-               printf("  Velocity Z : %f m/s\n", get_sirf_dbl(buf+p));
-               p += 8;
-               printf("  Clock bias : %f s\n", get_sirf_dbl(buf+p));
-               p += 8;
-               printf("  Clock drift : %f s/s\n", get_sirf_sgl(buf+p));
-               p += 4;
-               p += 1;
-               p += 4;
-               p += 4;
-               printf("  Ionospheric delay : %f m\n", get_sirf_sgl(buf+p));
-               p += 4;
+               decode_sirf_msg_30(buf + 4, n - 8);
                break;
 
        case 31:
@@ -775,13 +793,13 @@ int main(int argc, const char *argv[])
                return -1;
        }
 
-       /* switch to 4800 bauds */
+       /* switch to 9600 bauds */
        if (tcgetattr(fd, &term) != 0) {
                perror("tcgetattr");
        }
        cfmakeraw(&term);
-       cfsetispeed(&term, B115200);
-       cfsetospeed(&term, B115200);
+       cfsetispeed(&term, B9600);
+       cfsetospeed(&term, B9600);
        /* 8N1 */
        term.c_cflag &= ~(CSIZE|PARENB|CSTOPB);
        term.c_cflag |=   CS8;
@@ -791,43 +809,42 @@ int main(int argc, const char *argv[])
 
        /* switch from NMEA to SiRF binary format */
 
-       const char to_sirf[] = "$PSRF100,0,9600,8,1,0*0C\n";
+       const char to_sirf[] = "$PSRF100,0,9600,8,1,0*0C\r\n";
        /* -1 : we do not want to send the null terminating character */
-       //write(fd, to_sirf, sizeof(to_sirf)-1);
+       write(fd, to_sirf, sizeof(to_sirf)-1);
 
        /* Set Binary Serial Port.  Note : the effect of changing the baud
           rate is not immediate at all */
 
        /* Tested value : 
-            1200         1200 bit/s
+            1200         1200 bit/s - no ACK recv
             2400         2400 bit/s
             4800         4800 bit/s
-            9600         9600 bit/s
+            9600         9600 bit/s - no ACK recv
            19200        19200 bit/s
            38400        38400 bit/s
            57600        57600 bit/s
           115200       115200 bit/s
        */
 
-       unsigned int baud = 115200;
+       unsigned int baud = 9600;
        unsigned char msg_134[] = { 134, 
                                    (baud >> 24) & 0xff,
                                    (baud >> 16) & 0xff,
                                    (baud >>  8) & 0xff,
                                    (baud >>  0) & 0xff,
                                    8, 1, 0, 0 };
-       sirf_msg_send(fd, msg_134, sizeof(msg_134));
+       //sirf_msg_send(fd, msg_134, sizeof(msg_134));
        /*
        if (tcgetattr(fd, &term) != 0) {
                perror("tcgetattr");
        }
-       cfsetispeed(&term, B1200);
-       cfsetospeed(&term, B1200);
+       cfsetispeed(&term, B115200);
+       cfsetospeed(&term, B115200);
        if (tcsetattr(fd, TCSANOW, &term) != 0) {
                perror("tcsetattr");
        }
        */
-
        /* Poll Software Version */
        unsigned char msg_132[] = { 132, 0x00 };
        sirf_msg_send(fd, msg_132, sizeof(msg_132));
@@ -856,19 +873,23 @@ int main(int argc, const char *argv[])
                                    0x00, 0x00, 0x00, 0x00,
                                    0x00, 0x00,
                                    12,
+                                   /* Clear ephemeris */
+                                   /*(1<<1) |*/
                                    /* Clear memory */
-                                   /* (1<<2) | */
+                                   /*(1<<2) |*/
+                                   /* Factory reset => 4800, 8N1, NMEA */
+                                   /*(1<<3) |*/
                                    /* Enable raw track & debug data */
                                    (1<<4) | (1<<5) };
        sirf_msg_send(fd, msg_128, sizeof(msg_128));
 
-       /* Enable MID 29 */
-       unsigned char msg_166_29[] = { 166, 0, 29, 1, 0, 0, 0, 0 };
-       sirf_msg_send(fd, msg_166_29, sizeof(msg_166_29));
+       /* Enable MID 28 */
+       unsigned char msg_166_28[] = { 166, 0, 28, 1, 0, 0, 0, 0 };
+       sirf_msg_send(fd, msg_166_28, sizeof(msg_166_28));
 
        /* Enable MID 52 : we got a NACK since this feature is not available */
-       unsigned char msg_166[] = { 166,  0, 52, 1, 0, 0, 0, 0 };
-       sirf_msg_send(fd, msg_166, sizeof(msg_166));
+       unsigned char msg_166_52[] = { 166,  0, 52, 1, 0, 0, 0, 0 };
+       sirf_msg_send(fd, msg_166_52, sizeof(msg_166_52));
 
        /* Try to enable all messages. We got a ACK for :
             0, Undocumented (never received)
@@ -876,19 +897,19 @@ int main(int argc, const char *argv[])
                => X,Y,Z,dX,dY,dZ,T, HDOP, sat used
                used by gpsd to report longitude, latitude
             4, Measured Tracker Data Out
-               => T, SID, Azimuth, Elevation, SNR
+               => T, SatID, Azimuth, Elevation, SNR
             7, Clock Status Data
             8, 50 BPS Data
             9, CPU Throughput
            13, Visible List
-               => SID, Azimuth, Elevation
+               => SatID, Azimuth, Elevation
            18, OkToSend (never received)
            27, DGPS Status Format
            28, Navigation Library Measurement Data
-               => SID, T, Pseudorange, Carrier freq+phase
+               => SatID, T, Pseudorange, Carrier freq+phase
            29, Navigation Library DGPS Data (never received)
            30, Navigation Library SV State Data
-               => SID, T, X,Y,Z,dX,dY,dZ, Clock bias+drift, IONO
+               => SatID, T, X,Y,Z,dX,dY,dZ, Clock bias+drift, IONO
            31, Navigation Library Initialization Data
            41, Geodetic Navigation Data
            46, Test Mode 3/4/5/6 (never received)