Backup
authorBenoit Papillault <benoit.papillault@free.fr>
Tue, 27 Dec 2011 19:04:07 +0000 (20:04 +0100)
committerBenoit Papillault <benoit.papillault@free.fr>
Tue, 27 Dec 2011 19:04:07 +0000 (20:04 +0100)
daytime.cpp [new file with mode: 0644]
gpstk-solution6.cpp
sirf2.cpp [new file with mode: 0644]

diff --git a/daytime.cpp b/daytime.cpp
new file mode 100644 (file)
index 0000000..c0eef69
--- /dev/null
@@ -0,0 +1,27 @@
+#include <iostream>
+#include "DayTime.hpp"
+
+using namespace std;
+using namespace gpstk;
+
+int main() {
+
+       DayTime t;
+
+       // display GPS week , GPS seconds
+       cout << "Time : " << endl;
+       cout << "  GPS week / GPS seconds : "   // 1667 / 210433
+            << t.GPSfullweek() << " / " << t.GPSsow()
+            << endl;
+
+       cout << "  Y-M-D h:m:s : "              // 2011-12-20 10:27:13
+            << t.year() << "-" << t.month() << "-" << t.day() << " "
+            << t.hour() << ":" << t.minute() << ":" << t.second()
+            << endl;
+
+       cout << "  Year / Day of year : "       // 2011 / 354
+            << t.DOYyear() << " / " << t.DOYday()
+            << endl;
+
+       return 0;
+}
index 96564ec..9180f88 100644 (file)
@@ -6,73 +6,62 @@
 #include <iostream>
 #include <iomanip>
 
-   // Class for handling satellite observation parameters RINEX files
+// Class for handling satellite observation parameters RINEX files
 #include "RinexObsStream.hpp"
 
-   // Classes for handling RINEX Broadcast ephemeris files
+// Classes for handling RINEX Broadcast ephemeris files
 #include "RinexNavStream.hpp"
 #include "RinexNavHeader.hpp"
 #include "RinexNavData.hpp"
 
-   // Class in charge of the GPS signal modelling
+// Class in charge of the GPS signal modelling
 #include "ModelObs.hpp"
 
-   // Class to store satellite broadcast navigation data
+// Class to store satellite broadcast navigation data
 #include "GPSEphemerisStore.hpp"
+#include "SP3EphemerisStore.hpp"
 
-   // Class to model the tropospheric delays
+// Class to model the tropospheric delays
 #include "TropModel.hpp"
 
-   // Classes to model and store ionospheric delays
+// Classes to model and store ionospheric delays
 #include "IonoModel.hpp"
 #include "IonoModelStore.hpp"
 
-   // Class to solve the equation system using Least Mean Squares
+// Class to solve the equation system using Least Mean Squares
 #include "SolverLMS.hpp"
 
-   // Class to solve the equation system using Weighted-Least Mean Squares
+// Class to solve the equation system using Weighted-Least Mean Squares
 #include "SolverWMS.hpp"
 
-   // Class to solve equations systems using a simple code-based Kalman filter
+// Class to solve equations systems using a simple code-based Kalman filter
 #include "CodeKalmanSolver.hpp"
 
-   // Class defining the GNSS data structures
+// Class defining the GNSS data structures
 #include "DataStructures.hpp"
 
-   // Class to filter out observables grossly out of limits
+// Class to filter out observables grossly out of limits
 #include "SimpleFilter.hpp"
 
-   // Class for easily changing reference base from ECEF to NEU
+// Class for easily changing reference base from ECEF to NEU
 #include "XYZ2NEU.hpp"
 
-   // Class to detect cycle slips using just one frequency
+// Class to detect cycle slips using just one frequency
 #include "OneFreqCSDetector.hpp"
 
-   // Class to detect cycle slips using LI combination
+// Class to detect cycle slips using LI combination
 #include "LICSDetector.hpp"
 
-   // Class to detect cycle slips using the Melbourne-Wubbena combination
+// Class to detect cycle slips using the Melbourne-Wubbena combination
 #include "MWCSDetector.hpp"
 
-   // Class to compute weights according to Appendix J of MOPS C (RTCA/DO-229C)
+// Class to compute weights according to Appendix J of MOPS C (RTCA/DO-229C)
 #include "ComputeMOPSWeights.hpp"
 
-   // Class to smooth code observables (by default, C1)
-#include "CodeSmoother.hpp"
-
-   // Class to smooth the PC combination
-#include "PCSmoother.hpp"
-
-   // Classes to compute several combinations
-#include "ComputePC.hpp"
-#include "ComputeLC.hpp"
-#include "ComputeLI.hpp"
-#include "ComputeMelbourneWubbena.hpp"
-
-   // Class to compute single differences between receiver stations
+// Class to compute single differences between receiver stations
 #include "DeltaOp.hpp"
 
-   // Class to synchronize two GNSS Data Structures data streams.
+// Class to synchronize two GNSS Data Structures data streams.
 #include "Synchronize.hpp"
 
 #include "geometry.hpp"                   // DEG_TO_RAD
 using namespace std;
 using namespace gpstk;
 
+// class for weight 
+
+class ComputeElevationWeights : public WeightBase, public ProcessingClass
+{
+public:
+       ComputeElevationWeights() {};
+
+       double getWeight(const SatID& sat,
+                        typeValueMap& tvMap) {
+               double elevation = tvMap(TypeID::elevation);
+               if (elevation < 45.0)
+                       return elevation;
+               return 90.0;
+       };
+
+       virtual satTypeValueMap& Process(const DayTime& time,
+                                        satTypeValueMap& gData) {
+               
+               // Loop through all the satellites
+               satTypeValueMap::iterator it;
+               for (it = gData.begin(); it != gData.end(); ++it ) {
+                       (*it).second[TypeID::weight] =
+                               getWeight((*it).first, (*it).second);
+               }
+
+               return gData;
+       };
+
+       virtual gnssSatTypeValue& Process(gnssSatTypeValue& gData) {
+               Process(gData.header.epoch, gData.body); return gData;
+       };
+
+       virtual gnssRinex& Process(gnssRinex& gData) {
+               Process(gData.header.epoch, gData.body); return gData;
+       };
+
+       virtual int getIndex(void) const {
+               return 6; /* FIXME */
+       };
+
+       virtual string getClassName(void) const {
+               return "ComputeElevationWeights";
+       };
+};
+
+void display_rinex(const gnssRinex& gRin) {
+       cout << gRin.header.epoch
+            << " " << gRin.header.epoch.second() << endl;
+       gRin.body.dump(cout, 1);
+}
+
 void display_position(const Position& p,
                      const Position& nominalPos) {
        Position pos = p;
@@ -116,11 +156,18 @@ gnssRinex keepOnlySatGPS(const gnssRinex& rin) {
        return result;
 }
 
-int main(void)
+int main(int argc, const char *argv[])
 {
-       const char obsReference[] = "crei354a.11o";
-       const char obsMobile[] = "mlvl354a.11o";
-       const char navReference[] = "crei354a.11n";
+       // first set
+       //const char obsReference[] = "crei354a.11o";
+       //const char obsMobile[] = "mlvl354a.11o";
+       //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[] = "4.ooo";
+       const char navReference[] = "crei361o.11n";
+       const char sp3File[] = "igu16682_12.sp3";
        
        // reference station nominal position
        Position nominalPosRef  (4166409.705,   182770.997,     4809802.953);
@@ -128,107 +175,79 @@ int main(void)
        Position nominalPos_true(4201577.209,   189859.856,     4779064.567);   
        Position nominalPos     (4201000.000,   189000.000,     4770000.000);   
 
-       Position lastPosition; /* only valid if i > 1 */
+       // initial set to nominalPos
+       Position lastPosition = nominalPos;
 
-      ////////// Initialization phase //////////
+       ////////// Initialization phase //////////
 
-      //////////// COMMON OBJECTS //////////////
+       //////////// COMMON OBJECTS //////////////
 
-   RinexNavData rNavData;              // Object to store Rinex navigation data
-   GPSEphemerisStore bceStore;         // Object to store satellites ephemeris
-   RinexNavHeader rNavHeader;          // Object to read the header of Rinex
+       RinexNavData rNavData;         // Object to store Rinex navigation data
+       GPSEphemerisStore bceStore;    // Object to store satellites ephemeris
+       SP3EphemerisStore sp3Store;
+       RinexNavHeader rNavHeader;     // Object to read the header of Rinex
                                        // navigation data files
-   IonoModelStore ionoStore;           // Object to store ionospheric models
-   IonoModel ioModel;                  // Declare a Ionospheric Model object
-
-   cout << "reading navigation file : " << navReference << endl;
+       IonoModelStore ionoStore;      // Object to store ionospheric models
+       IonoModel ioModel;             // Declare a Ionospheric Model object
 
-      // Create the input navigation file stream
-   RinexNavStream rnavin(navReference);
+       cout << "reading navigation file :\t" << navReference << endl;
 
-      // We need to read ionospheric parameters (Klobuchar model) from header
-   rnavin >> rNavHeader;
+       // Create the input navigation file stream
+       RinexNavStream rnavin(navReference);
 
-      // Let's feed the ionospheric model (Klobuchar type) from data in the
-      // Navigation file header
-   ioModel.setModel(rNavHeader.ionAlpha, rNavHeader.ionBeta);
+       // We need to read ionospheric parameters (Klobuchar model) from header
+       rnavin >> rNavHeader;
 
-   for (int i = 0; i < 4; i++)
-          cout << "ionAlpha = " << rNavHeader.ionAlpha[i] << endl;
-   for (int i = 0; i < 4; i++)
-          cout << "ionBeta  = " << rNavHeader.ionBeta[i]  << endl;
+       // Let's feed the ionospheric model (Klobuchar type) from data in the
+       // Navigation file header
+       ioModel.setModel(rNavHeader.ionAlpha, rNavHeader.ionBeta);
 
-      // Beware: In this case, the same model will be used for the
-      // full data span
-   ionoStore.addIonoModel(DayTime::BEGINNING_OF_TIME, ioModel);
+       for (int i = 0; i < 4; i++)
+               cout << "ionAlpha = " << rNavHeader.ionAlpha[i] << endl;
+       for (int i = 0; i < 4; i++)
+               cout << "ionBeta  = " << rNavHeader.ionBeta[i]  << endl;
 
-      // Storing the ephemeris in "bceStore"
-   while (rnavin >> rNavData)
-   {
-      bceStore.addEphemeris(rNavData);
-   }
+       // Beware: In this case, the same model will be used for the
+       // full data span
+       ionoStore.addIonoModel(DayTime::BEGINNING_OF_TIME, ioModel);
 
-   bceStore.SearchPast();  // This is the default
+       // Storing the ephemeris in "bceStore"
+       while (rnavin >> rNavData) {
+               bceStore.addEphemeris(rNavData);
+       }
 
-      // Declare a MOPSTropModel object, setting the defaults
-   MOPSTropModel mopsTM( nominalPos_true.getAltitude(),
-                         nominalPos_true.getGeodeticLatitude(),
-                         30 );
+       sp3Store.loadFile(sp3File);
 
-      // Declare a simple filter object. By default, it filters C1 with
-      // default limits
-   SimpleFilter filter;
+       bceStore.SearchPast();  // This is the default
 
-      // This is the GNSS data structure that will hold all the
-      // GNSS-related information
-   gnssRinex gOriginal;
+       // Declare a MOPSTropModel object, setting the defaults
+       MOPSTropModel mopsTM;
 
-      ////////////////////////////////////////
+       // Declare a simple filter object. By default, it filters C1 with
+       // default limits
+       SimpleFilter filter;
 
+       // This is the GNSS data structure that will hold all the
+       // GNSS-related information
+       gnssRinex gOriginal;
 
+       //////////// CASE #1 OBJECTS ////////////
 
-      //////////// CASE #1 OBJECTS ////////////
+       // Declare a SolverLMS object
+       SolverLMS solver;
 
-      // Declare a SolverLMS object
-   SolverLMS solver;
+       //////////// CASE #3 OBJECTS ////////////
 
-      ////////////////////////////////////////
-
-      //////////// CASE #3 OBJECTS ////////////
-
-      // This object will compute the appropriate MOPS weights
-   ComputeMOPSWeights mopsW(nominalPos, bceStore);
+       // This object will compute the appropriate MOPS weights
+       ComputeMOPSWeights mopsW;
    
-   // Declare a solver object using Weighted-Least-Mean-Squares and
-   // a topocentric reference system (NEU)
-   SolverWMS solverWMS;
-
-      ////////////////////////////////////////
-
-
-
-      //////////// CASE #4 OBJECTS ////////////
-
-      // Let's declare a cycle slip detector using just one frequency
-   OneFreqCSDetector markCSC1;
-
-      // Declare an object to smooth code (C1 by default)
-   CodeSmoother smoothC1;
-   smoothC1.setMaxWindowSize(8);   // Configure smoother for 30 s sampling data
-
-      ////////////////////////////////////////
-
-
+       // Declare a solver object using Weighted-Least-Mean-Squares and
+       // a topocentric reference system (NEU)
+       SolverWMS solverWMS;
 
       //////////// CASE #5 OBJECTS ////////////
 
-      // Let's declare another cycle slip detector using just one frequency
-   OneFreqCSDetector markCSC1case5;
-
-      // Declare another object to smooth code (C1 by default)
-   CodeSmoother smoothC1case5;
-   smoothC1case5.setMaxWindowSize(8);   // Configure for 30 s sampling data
-
       // The core of this case is to add a new equation to the equation system
       // Such equation states that there are NO changes in height for the
       // rover.
@@ -263,75 +282,8 @@ int main(void)
       ////////////////////////////////////////
 
 
-
-      //////////// CASE #6 OBJECTS ////////////
-
-      // Object to compute the PC (ionosphere-free) combination.
-   ComputePC getPC;
-      // Use C1 instead of P1. P1 observables are declared in available RINEX
-      // files, but often they are indeed missing (like in this case). When
-      // that happens, this step is mandatory
-   getPC.useC1();
-
-      // Declare a simple filter object to screen PC
-   SimpleFilter pcFilter;
-   pcFilter.setFilteredType(TypeID::PC);
-
-      // Declare the modeler object for PC, setting all the parameters
-      // in one pass
-   ModelObs modelPC(nominalPos, mopsTM, bceStore, TypeID::PC, false);
-      // Take notice that PC combination doesn't use ionosphere modelling, nor
-      // TGD computation.
-      // WARNING: When using C1 instead of P1 to compute PC combination, be
-      // aware that instrumental errors will NOT cancel, introducing a bias
-      // that must be taken into account by other means. This will not work out
-      // in this example.
-
-      ////////////////////////////////////////
-
-
-
-      //////////// CASE #7 OBJECTS ////////////
-
-      // Objects to compute several common combinations.
-   ComputeLC getLC;
-   ComputeLI getLI;
-   ComputeMelbourneWubbena getMW;
-   getMW.useC1();      // Use C1 instead of P1
-
-      // Objects to mark cycle slips
-   LICSDetector markCSLI;      // Checks LI cycle slips
-   MWCSDetector markCSMW;      // Checks Merbourne-Wubbena cycle slips
-
-      // Object to smooth the PC combination. Defaults are usually fine
-   PCSmoother smoothPC;
-
-      ////////////////////////////////////////
-
-
-
-      //////////// CASE #8 OBJECTS ////////////
-
-      // Objects to mark cycle slips
-   LICSDetector markCSLIcase8;      // Checks LI cycle slips
-   MWCSDetector markCSMWcase8;      // Checks Merbourne-Wubbena cycle slips
-
-      // Object to smooth the PC combination. Defaults are usually fine
-   PCSmoother smoothPCcase8;
-
-      ////////////////////////////////////////
-
-
-
       //////////// CASE #9 OBJECTS ////////////
 
-      // Objects to mark cycle slips
-   LICSDetector markCSLIcase9;      // Checks LI cycle slips
-   MWCSDetector markCSMWcase9;      // Checks Merbourne-Wubbena cycle slips
-
-      // Object to smooth the PC combination. Defaults are usually fine
-   PCSmoother smoothPCcase9;
-
       // Declare a new Kalman solver
    CodeKalmanSolver solverK9;
 
@@ -345,14 +297,13 @@ int main(void)
       // reference station data
    gnssRinex gRef;
 
-      // Create the input observation file stream for REFERENCE STATION
+   // Create the input observation file stream for REFERENCE STATION
    RinexObsStream rinRef(obsReference);
+   cout << "reading observation file (reference) :\t" << obsReference << endl;
 
       // Declare a MOPSTropModel object for the reference station, setting
       // the defaults
-   MOPSTropModel mopsTMRef( nominalPosRef.getAltitude(),
-                            nominalPosRef.getGeodeticLatitude(),
-                            30 );
+   MOPSTropModel mopsTMRef;
 
       // Declare the modeler object, setting all the parameters in one pass
       // Given that in this example we are using a fixed GPS station with known
@@ -366,7 +317,10 @@ int main(void)
    //            bceStore, TypeID::C1);
 
    ModelObs model;
-   model.setDefaultEphemeris(bceStore);
+   model.setDefaultIonoModel(ionoStore);
+   model.setDefaultTropoModel(mopsTM);
+   model.setDefaultEphemeris(sp3Store /*bceStore*/);
+   model.setMinElev(1.0);
 
       // Declare the appropriate modeler object for a reference station
    //ModelObsFixedStation modelRef(nominalPosRef,
@@ -374,9 +328,15 @@ int main(void)
    //                           bceStore, TypeID::C1);
 
    ModelObs modelRef;
-   modelRef.setDefaultEphemeris(bceStore);
+   modelRef.setDefaultIonoModel(ionoStore);
+   modelRef.setDefaultTropoModel(mopsTMRef);
+   modelRef.setDefaultEphemeris(sp3Store /*bceStore*/);
    modelRef.Prepare(nominalPosRef);
 
+   // use simple model without iono & tropo corrections
+   ModelObs myModelRef;
+   ModelObs myModel;
+   
       // Create an object to compute the single differences of prefit residuals
    DeltaOp delta;      // By default, it will work on code prefit residuals
 
@@ -395,6 +355,8 @@ int main(void)
       // Declare a new Kalman solver
    CodeKalmanSolver solverK12;
 
+   ComputeElevationWeights elevationW;
+
       ////////////////////////////////////////
 
       //////// End of initialization phase  ////////
@@ -412,15 +374,21 @@ int main(void)
       // Create the input observation file stream
       // This is a fixed station, but here it will play as "rover"
    RinexObsStream rin(obsMobile);
-      // Please note that data was collected in year 2002, when the Sun
-      // was very active
 
       // Loop over all data epochs
    while (rin >> gOriginal) {
 
           // remove glonass satellite
           gOriginal = keepOnlySatGPS(gOriginal);
-
+          gOriginal.keepOnlyTypeID(TypeID::C1);
+
+          // shift pseudorange by 37900000 m
+          /*
+          for (satTypeValueMap::iterator it = gOriginal.body.begin();
+               it != gOriginal.body.end(); it++) {
+                  (*it).second[TypeID::C1] -= 37900000.0;
+          }
+          */
           cout << "--- " << ++i << " ---" << endl;
           cout << "  Ref.   :";
           display_position(nominalPosRef, nominalPos_true);
@@ -431,9 +399,49 @@ int main(void)
           cout << "  Last   :";
           display_position(lastPosition, nominalPos_true);
 
-          // prepare the model using the last position (if available)
-          if (i > 1)
-                  model.Prepare(lastPosition);
+          cout << "gOriginal : " << endl;
+          display_rinex(gOriginal);
+
+          // update MOPSTropModel
+          mopsTMRef.setReceiverLatitude(nominalPosRef.getGeodeticLatitude());
+          mopsTMRef.setReceiverHeight(nominalPosRef.getAltitude());
+          mopsTMRef.setDayOfYear(gOriginal.header.epoch.DOYday());
+
+          mopsTM.setReceiverLatitude(lastPosition.getGeodeticLatitude());
+          mopsTM.setReceiverHeight(lastPosition.getAltitude());
+          mopsTM.setDayOfYear(gOriginal.header.epoch.DOYday());
+
+          // update mopsW
+          mopsW.setPosition(lastPosition);
+          mopsW.setDefaultEphemeris(sp3Store /*bceStore*/);
+
+          myModelRef.setDefaultEphemeris(sp3Store /*bceStore*/);
+          myModel.setDefaultEphemeris(sp3Store /*bceStore*/);
+   
+          model.Prepare(lastPosition);
+          myModelRef.Prepare(nominalPosRef);
+          myModel.Prepare(lastPosition);
+
+          // CASE #0
+          // We are using LMS + C1 + no tropo/iono model
+          gnssRinex gRin0(gOriginal);
+          try {
+                  gRin0 >> filter >> myModel;
+                  cout << "gRin0 : " << endl;
+                  display_rinex(gRin0);
+                  gRin0 >> solver;
+                  display_rinex(gRin0);
+
+          Position pos(myModel.rxPos.X()+solver.getSolution(TypeID::dx),
+                       myModel.rxPos.Y()+solver.getSolution(TypeID::dy),
+                       myModel.rxPos.Z()+solver.getSolution(TypeID::dz));
+             
+                  cout << "  Case 0 :";
+                  display_position(pos, nominalPos_true);
+          }
+          catch (Exception e) {
+                  cout << "Case 0. Exception : " << e.what() << endl;
+          }
 
    //////////////////////////// CASE #1  ////////////////////////////
 
@@ -455,12 +463,12 @@ int main(void)
 
              // Get your results out of the solver object. In ECEF system
              // by default
-             Position solPos( (model.rxPos.X() + solver.solution[0]),
-                              (model.rxPos.Y() + solver.solution[1]),
-                              (model.rxPos.Z() + solver.solution[2]) );
+             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_position(solPos, nominalPos_true);
+             display_position(pos, nominalPos_true);
       }
       catch(...)
       {
@@ -492,6 +500,9 @@ int main(void)
              
              cout << "  Case 3 :";
              display_position(pos, nominalPos_true);
+             double altitude = pos.asGeodetic().getAltitude();
+             if ((0.0 < altitude) && (altitude < 300.0))
+                     lastPosition = pos;
       }
       catch (Exception e)
       {
@@ -500,219 +511,26 @@ int main(void)
 
    ////////////////////////// END OF CASE #3  //////////////////////////
 
-
-
-   //////////////////////////// CASE #4  ////////////////////////////
-
-         // This case does about the same as a modern GPS aircraft receiver,
-         // except for SBAS corrections and RAIM: C1smoothed + MOPS weights
-         // + WMS
-
+      // Case #4 : use elevation as weight
          // Let's make a working copy
       gnssRinex gRin4(gOriginal);
 
-      try
-      {
-
-             gRin4 >> filter >> markCSC1 >> smoothC1 >> model >> mopsW
-                   >> solverWMS;
-            // The "markCSC1" object will try to detect cycle slips using just
-            // one frequency data (C1 and L1 observables), marking the CS flags
-            // Then, "smoothC1" will use the former information to smooth C1
-            // observations using phase data (L1)
-
-            // BEWARE: Both cycle slip detectors and "smoothers" are objects
-            // that store their internal state, so you MUST NOT use the SAME
-            // object to process DIFFERENT data streams
+      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_position(pos, nominalPos_true);
       }
-      catch(...)
-      {
-         cerr << "Case 4. Exception at epoch: " << gRin4.header.epoch << endl;
-      }
-   ////////////////////////// END OF CASE #4  //////////////////////////
-
-
-
-   //////////////////////////// CASE #5  ////////////////////////////
-
-         // This case is like the former, but now let's suppose that one of
-         // the unknowns is indeed known: In this case dH is constant an equal
-         // to zero (i.e.: the "rover" doesn't change altitude), and we assign
-         //  a high "weight" to this information.
-
-         // Let's make a working copy
-      gnssRinex gRin5(gOriginal);
-      try {
-            // First, the typical processing up to the change of reference
-            // frame. Please note that all changes are stored in gRin5 GNSS
-            // data structure
-         gRin5 >> filter >> markCSC1case5 >> smoothC1case5 >> model
-               >> mopsW;
-            // Remember that both cycle slip detectors and "smoothers" are
-            // objects that store their internal state, so you MUST NOT use
-            // the SAME object to process DIFFERENT data streams (please
-            // compare with case #4)
-
-            // Now, let's insert the new equation data, including its
-            // corresponding weight
-         gRin5.body[satEq] = equTVMap;
-
-            // Let's continue processing data as usual
-         gRin5 >> solverWMS;
-      }
-      catch(...)
-      {
-         cerr << "Case 5. Exception at epoch: " << gRin5.header.epoch << endl;
-      }
-
-   ////////////////////////// END OF CASE #5  //////////////////////////
-
-
-
-   //////////////////////////// CASE #6  ////////////////////////////
-
-         // This case uses de PC combination plus a WMS solver
-
-         // Let's make a working copy
-      gnssRinex gRin6(gOriginal);
-      try {
-
-         gRin6 >> getPC >> pcFilter >> modelPC >> mopsW >> solverWMS;
-            // First, we need to compute the PC combination with "getPC" and
-            // insert it into the "gRin6" data structure.
-            // Then, use "pcFilter" to take out grossly out of range
-            // results in PC.
-            // After that, use an specific model ("modelPC") for this
-            // combination. It doesn't use ionospheric model nor TGD.
-            // The remaining steps are similar to the other cases.
-
-             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 6 :";
-             display_position(pos, nominalPos_true);
-      }
-      catch(...)
-      {
-         cerr << "Case 6. Exception at epoch: " << gRin6.header.epoch << endl;
-      }
-
-   ////////////////////////// END OF CASE #6  //////////////////////////
-
-
-
-   //////////////////////////// CASE #7  ////////////////////////////
-
-         // This case uses the smoothed-PC combination plus WMS
-
-         // Let's make a working copy
-      gnssRinex gRin7(gOriginal);
-      try
-      {
-
-         gRin7 >> getPC >> getLC >> getLI >> getMW >> markCSLI >> markCSMW
-               >> smoothPC >> pcFilter >> modelPC >> mopsW >> solverWMS;
-            // In addition to PC, we will also neet LC ("getLC"), LI ("getLI")
-            // and MW ("getMW") combinations:
-            //
-            // - LC (as well as PC) is needed by "smoothPC" in order to smooth
-            //   PC data. Also, the smoother works better with cycle slip
-            //   information, and therefore:
-            //
-            //   - LI feeds "markCSLI": The LI-based cycle slip detector
-            //   - MW feeds "markCSMW": The MW-based cycle slip detector
-            //
-            // - The remaining steps are essentially the same
-        Position pos((modelPC.rxPos.X()+solverWMS.getSolution(TypeID::dx)),
-                     (modelPC.rxPos.Y()+solverWMS.getSolution(TypeID::dy)),
-                     (modelPC.rxPos.Z()+solverWMS.getSolution(TypeID::dz)));
-
-        cout << "  Case 7 :";
-        display_position(pos, nominalPos_true);
-      }
-      catch(...)
-      {
-         cerr << "Case 7. Exception at epoch: " << gRin7.header.epoch << endl;
-      }
-
-   ////////////////////////// END OF CASE #7  //////////////////////////
-
-
-
-   //////////////////////////// CASE #8  ////////////////////////////
-
-         // This case uses the smoothed-PC combination + WMS + information
-         // about dH (constant and equal to zero with a high confidence).
-         // It is a mix of the former case (#7) and case #5.
-
-         // Let's make a working copy
-      gnssRinex gRin8(gOriginal);
-      try
-      {
-            // First, let's process data up to the change of reference frame
-         gRin8 >> getPC >> getLC >> getLI >> getMW >> markCSLIcase8
-               >> markCSMWcase8 >> smoothPCcase8 >> pcFilter >> modelPC
-               >> mopsW;
-            // Remember that both cycle slip detectors and "smoothers" are
-            // objects that store their internal state, so you MUST NOT use
-            // the SAME object to process DIFFERENT data streams (compare with
-            // case #7).
-
-            // Now, let's insert the new equation data, including its weight.
-            // It is the same equation as case #5.
-         gRin8.body[satEq] = equTVMap;
-
-            // Let's continue processing data as usual
-         gRin8 >> solverWMS;
-      }
-      catch(...)
-      {
-         cerr << "Case 8. Exception at epoch: " << gRin8.header.epoch << endl;
-      }
-
-   ////////////////////////// END OF CASE #8  //////////////////////////
-
-
-
-   //////////////////////////// CASE #9  ////////////////////////////
-
-         // This case uses the smoothed-PC combination, exactly like case #7,
-         // but solves the equation system using a simple Kalman filter.
-
-         // Let's make a working copy
-      gnssRinex gRin9(gOriginal);
-      try
-      {
-
-         gRin9 >> getPC >> getLC >> getLI >> getMW >> markCSLIcase9
-               >> markCSMWcase9 >> smoothPCcase9 >> pcFilter >> modelPC
-               >> mopsW >> solverK9;
-            // VERY IMPORTANT: Note that in this case the coordinates are
-            // handled as constants, whereas the receiver clock is modeled as
-            // white noise.
-
-        Position pos((modelPC.rxPos.X()+solverK9.getSolution(TypeID::dx)),
-                     (modelPC.rxPos.Y()+solverK9.getSolution(TypeID::dy)),
-                     (modelPC.rxPos.Z()+solverK9.getSolution(TypeID::dz)));
-
-        cout << "  Case 9 :";
-        display_position(pos, nominalPos_true);
-      }
-      catch(...)
+      catch (Exception e)
       {
-         cerr << "Case 9. Exception at epoch: " << gRin9.header.epoch << endl;
+             cout << "Exception : " << e.what() << endl;
       }
 
-   ////////////////////////// END OF CASE #9  //////////////////////////
-
+      continue;
 
 
    //////////////////////////// CASE #10  ////////////////////////////
@@ -726,7 +544,7 @@ int main(void)
          // First, let's synchronize and process reference station data
       try
       {
-             gRef >> synchro >> filter >> modelRef;
+             gRef >> synchro >> filter >> myModelRef;
             // 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.
@@ -737,7 +555,7 @@ int main(void)
             // The "delta" object will take care of proper differencing.
             // We must tell it which GNSS data structure will be used
             // as reference
-         delta.setRefData(gRef.body);
+             delta.setRefData(gRef.body);
 
       }
       catch(SynchronizeException& e)   // THIS IS VERY IMPORTANT IN ORDER TO
@@ -756,14 +574,14 @@ at epoch: " << gRef.header.epoch << endl;
       try
       {
 
-             gRin10 >> filter >> model >> delta >> solver;
+             gRin10 >> filter >> myModel >> delta >> solver;
             // This is very similar to cases #1 and #2, but we insert a "delta"
             // object that will adjust code prefit residuals BEFORE solving the
             // system of equations.
 
-             Position pos((model.rxPos.X()+solver.getSolution(TypeID::dx)),
-                          (model.rxPos.Y()+solver.getSolution(TypeID::dy)),
-                          (model.rxPos.Z()+solver.getSolution(TypeID::dz)));
+             Position pos((myModel.rxPos.X()+solver.getSolution(TypeID::dx)),
+                          (myModel.rxPos.Y()+solver.getSolution(TypeID::dy)),
+                          (myModel.rxPos.Z()+solver.getSolution(TypeID::dz)));
              
              cout << "  Case 10:";
              display_position(pos, nominalPos_true);
@@ -794,11 +612,11 @@ at epoch: " << gRef.header.epoch << endl;
          // obtained from Case #10.
 
     try {
-           gRin11 >> filter >> model >> delta >> mopsW  >> solverWMS;
+           gRin11 >> filter >> myModel >> delta >> mopsW  >> solverWMS;
            // Like case #10, but now with "mopsW" and "solverWMS"
-           Position pos((model.rxPos.X()+solverWMS.getSolution(TypeID::dx)),
-                        (model.rxPos.Y()+solverWMS.getSolution(TypeID::dy)),
-                        (model.rxPos.Z()+solverWMS.getSolution(TypeID::dz)));
+           Position pos(myModel.rxPos.X()+solverWMS.getSolution(TypeID::dx),
+                        myModel.rxPos.Y()+solverWMS.getSolution(TypeID::dy),
+                        myModel.rxPos.Z()+solverWMS.getSolution(TypeID::dz));
            
            cout << "  Case 11:";
            display_position(pos, nominalPos_true);
@@ -829,19 +647,21 @@ at epoch: " << gRef.header.epoch << endl;
 
       try
       {
-
-         gRin12 >> filter >> model >> delta >> mopsW  >> solverK12;
+             if (i > 1) {
+                     gRin12 >> filter >> myModel >> delta
+                            >> mopsW  >> solverK12;
             // Like case #11, but now with "solverK12"
             // VERY IMPORTANT: Note that in this case the coordinates are
             // handled as constants, whereas the receiver clock is modeled as
             // white noise.
 
-        Position pos((modelPC.rxPos.X()+solverK12.getSolution(TypeID::dx)),
-                     (modelPC.rxPos.Y()+solverK12.getSolution(TypeID::dy)),
-                     (modelPC.rxPos.Z()+solverK12.getSolution(TypeID::dz)));
+             Position pos(myModel.rxPos.X()+solverK12.getSolution(TypeID::dx),
+                          myModel.rxPos.Y()+solverK12.getSolution(TypeID::dy),
+                          myModel.rxPos.Z()+solverK12.getSolution(TypeID::dz));
 
-        cout << "  Case 12:";
-        display_position(pos, nominalPos_true);
+             cout << "  Case 12:";
+             display_position(pos, nominalPos_true);
+             }
       }
       catch(...)
       {
@@ -850,6 +670,45 @@ at epoch: " << gRef.header.epoch << endl;
       }
 
    ////////////////////////// END OF CASE #12  //////////////////////////
+
+
+      /// Case #13
+      // Let's make a working copy
+      gnssRinex gRin13(gOriginal);
+
+      try {
+             gRin13 >> filter >> myModel >> delta >> elevationW >> solverWMS;
+
+             Position pos(myModel.rxPos.X()+solverWMS.getSolution(TypeID::dx),
+                          myModel.rxPos.Y()+solverWMS.getSolution(TypeID::dy),
+                          myModel.rxPos.Z()+solverWMS.getSolution(TypeID::dz));
+           
+             cout << "  Case 13:";
+             display_position(pos, nominalPos_true);
+      }
+      catch(...)
+      {
+             cout << "Exception #13 " << endl;
+      }
+
+      /// Case #14 : DGPS + iono/trop + WMS solver
+      // Let's make a working copy
+      gnssRinex gRin14(gOriginal);
+
+      try {
+             gRin14 >> filter >> model >> delta >> mopsW >> 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 14:";
+             display_position(pos, nominalPos_true);
+      }
+      catch(...)
+      {
+             cout << "Exception #14 " << endl;
+      }
    }
 
    return 0;
diff --git a/sirf2.cpp b/sirf2.cpp
new file mode 100644 (file)
index 0000000..c83c57e
--- /dev/null
+++ b/sirf2.cpp
@@ -0,0 +1,1081 @@
+/*
+  Author : Benoit Papillault <benoit.papillault@free.fr>
+  Creation : 2011-07-03
+  Goal : Understanding SiRF protocol for GPS devices
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/select.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <string.h>
+
+#include "RinexObsStream.hpp"
+#include "RinexObsHeader.hpp"
+#include "RinexObsData.hpp"
+
+#include "DayTime.hpp"
+#include "icd_200_constants.hpp"
+
+using namespace std;
+using namespace gpstk;
+
+const char *g_rinex = NULL;
+
+int    last_ext_gps_week = 0;
+double last_gps_seconds = 0.0;
+
+RinexObsStream r_out;
+RinexObsHeader r_header;
+RinexObsData   r_data;
+
+int rinex_open(const char *filename) {
+       r_out.open(filename, ios::out|ios::trunc);
+
+       return 0; 
+}
+
+int rinex_header_write() {
+       // fill in the header
+       DayTime now;
+
+       /* for 2.11 header, we need : 
+          versionValid
+          runByValid
+          markerNameValid
+          observerValid
+          receiverValid
+          antennaTypeValid
+          antennaPositionValid
+          antennaOffsetValid
+          waveFactValid
+          obsTypeValid
+          firstTimeValid
+          endValid
+       */
+       r_header.version = 2.11;
+       r_header.fileType = "Observation";
+       r_header.system.system = SatID::systemGPS;
+       r_header.valid |= RinexObsHeader::versionValid;
+
+       r_header.fileProgram = "sirf2";
+       r_header.fileAgency = "Benoit Papillault";
+       r_header.date = now.printf("%Y%m%d");
+       r_header.valid |= RinexObsHeader::runByValid;
+
+       r_header.markerName = "GPSL"; // GPS from LUCEOR
+       r_header.valid |= RinexObsHeader::markerNameValid;
+
+       r_header.observer = "Automatic";
+       r_header.agency = "LUCEOR";
+       r_header.valid |= RinexObsHeader::observerValid;
+
+       r_header.recNo = "BU-353";
+       r_header.recType = "SiRF binary";
+       r_header.recVers = "GSW3.5.0";
+       r_header.valid |= RinexObsHeader::receiverValid;
+
+       r_header.antNo = "USB";
+       r_header.antType = "Integrated";
+       r_header.valid |= RinexObsHeader::antennaTypeValid;
+
+       /* FIXME : should be a real ECEF position ? */
+       r_header.antennaPosition[0] = 0.0;
+       r_header.antennaPosition[1] = 0.0;
+       r_header.antennaPosition[2] = 0.0;
+       r_header.valid |= RinexObsHeader::antennaPositionValid;
+
+       r_header.antennaOffset[0] = 0.0;
+       r_header.antennaOffset[1] = 0.0;
+       r_header.antennaOffset[2] = 0.0;
+       r_header.valid |= RinexObsHeader::antennaOffsetValid;
+
+       r_header.wavelengthFactor[0] = 1;
+       r_header.wavelengthFactor[1] = 1;
+       r_header.valid |= RinexObsHeader::waveFactValid;
+
+       r_header.numObs = 2;
+       r_header.obsTypeList.push_back(RinexObsHeader::C1);
+       r_header.obsTypeList.push_back(RinexObsHeader::S1);
+       r_header.valid |= RinexObsHeader::obsTypeValid;
+
+       r_header.firstObs = now;
+       r_header.firstSystem.system = SatID::systemGPS;
+       r_header.valid |= RinexObsHeader::firstTimeValid;
+
+       r_header.valid |= RinexObsHeader::endValid;
+
+       // write the header
+       r_out << r_header;
+
+       return 0;
+}
+
+// write RINEX data record (some part are already filled)
+int rinex_data_write() {
+
+       // write nothing if no satellite have been seen
+       if (r_data.obs.size() == 0)
+               return 0;
+
+       // fill in the missing pieces
+       r_data.epochFlag = 0;
+       r_data.time.setGPSfullweek(last_ext_gps_week, last_gps_seconds);
+       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;
+
+       // reset r_data
+       r_data.obs.clear();
+
+       return 0;
+}
+
+/* from Message ID 28 */
+int rinex_data_add_pseudorange_snr(int satNR,
+                                  double gps_seconds,
+                                  double pseudorange,
+                                  double snr) {
+       printf("RINEX range\t| GPS seconds %6f SatID %3u C1 %f\n",
+              gps_seconds, satNR, pseudorange);
+
+       // compute ssi
+       int ssi = (int)(snr / 6.0);
+       if (ssi < 0)
+               ssi = 0;
+       if (ssi > 9)
+               ssi = 9;
+
+       // we only set GPS seconds
+       last_gps_seconds = gps_seconds;
+
+       SatID sat(satNR, SatID::systemGPS);
+       r_data.obs[sat][RinexObsHeader::C1].data = pseudorange;
+       r_data.obs[sat][RinexObsHeader::C1].lli = 0;
+       r_data.obs[sat][RinexObsHeader::C1].ssi = ssi;
+
+       r_data.obs[sat][RinexObsHeader::S1].data = (double)snr;
+       r_data.obs[sat][RinexObsHeader::S1].lli = 0;
+       r_data.obs[sat][RinexObsHeader::S1].ssi = ssi;
+
+       return 0;
+}
+
+/* from Message ID 7 */
+int rinex_data_add_time_clock_bias(int ext_gps_week,
+                                  double gps_seconds, double clock_bias) {
+       printf("RINEX time\t| GPS week %5u GPS seconds %6f\n",
+              ext_gps_week, gps_seconds);
+       
+       // set extended GPS week only
+       last_ext_gps_week = ext_gps_week;
+       
+       // adjust using clock bias
+       last_gps_seconds -= clock_bias;
+       
+       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 - clock_bias * C_GPS_M;
+               (*it).second[RinexObsHeader::C1].data = new_val;
+
+               printf("RINEX satID %3u : pseudo range %f => %f\n",
+                      (*it).first.id, old_val, new_val);
+       }
+
+       /* write data */
+       rinex_data_write();
+
+       return 0;
+}
+
+/* return 1 if message is a valid SiRF message, 0 if invalid */
+
+int check_sirf_msg(unsigned char *buf, int n) {
+       int length, crc, computed_crc, i;
+
+       /* check size (at least 8 bytes) */
+       if (n<8) {
+               printf("msg too small (%d bytes)\n", n);
+               return 0;
+       }
+
+       /* check start pattern */
+       if ((buf[0] != 0xa0) || (buf[1] != 0xa2)) {
+               printf("invalid start pattern : 0x%02x 0x%02x\n",
+                      buf[0], buf[1]);
+               return 0;
+       }
+  
+       /* check length */
+       length = ((buf[2]<<8) | (buf[3]));
+       if (length & 0x8000) {
+               printf("invalid length : 0x%04x bytes\n", length);
+               return 0;
+       }
+
+       if (length > 912) {
+               printf("warning : length (%d bytes) is above 912\n", length);
+       }
+
+       if (4 + length + 4 != n) {
+               printf("invalid length : %d bytes, buf buffer is %d bytes\n",
+                      length, n);
+               return 0;
+       }
+  
+       /* check checksum */
+       crc = ((buf[n-4]<<8) | (buf[n-3]));
+       if (crc & 0x8000) {
+               printf("invalid crc : 0x%04x\n", crc);
+               return 0;
+       }
+
+       computed_crc = 0;
+       for (i=4; i<n-4; i++) {
+               computed_crc += buf[i];
+       }
+       computed_crc &= 0x7fff;
+
+       if (computed_crc != crc) {
+               printf("invalid crc : 0x%04x computed crc : 0x%04x\n",
+                      crc, computed_crc);
+               return 0;
+       }
+  
+       /* check stop pattern */
+       if ((buf[n-2] != 0xb0) || (buf[n-1] != 0xb3)) {
+               printf("invalid stop pattern : 0x%02x 0x%02x\n",
+                      buf[n-2], buf[n-1]);
+               return 0;
+       }
+
+       return 1;
+}
+
+double get_sirf_dbl(unsigned char *buf) {
+  double r;
+  unsigned char * ptr = (unsigned char *)&r;
+
+  ptr[0] = buf[3];
+  ptr[1] = buf[2];
+  ptr[2] = buf[1];
+  ptr[3] = buf[0];
+  ptr[4] = buf[7];
+  ptr[5] = buf[6];
+  ptr[6] = buf[5];
+  ptr[7] = buf[4];
+
+  if (sizeof(double) == 8) {
+    return r;
+  } else {
+    printf("get_sirf_dbl implementation error\n");
+  }
+}
+
+int get_sirf_2s(unsigned char *buf) {
+       return (((signed char)buf[0])<<8) | buf[1];
+}
+
+int get_sirf_4s(unsigned char *buf) {
+       return (((signed char)buf[0])<<24)|(buf[1]<<16)|(buf[2]<<8)|buf[3];
+}
+
+unsigned int get_sirf_2u(unsigned char *buf) {
+       return (buf[0]<<8) | buf[1];
+}
+
+unsigned int get_sirf_4u(unsigned char *buf) {
+       return (buf[0]<<24) | (buf[1]<<16) | (buf[2]<<8) | buf[3];
+}
+
+float get_sirf_sgl(unsigned char *buf) {
+  float r;
+  unsigned char *ptr = (unsigned char *)&r;
+
+  ptr[0] = buf[3];
+  ptr[1] = buf[2];
+  ptr[2] = buf[1];
+  ptr[3] = buf[0];
+
+  if (sizeof(float) == 4) {
+    return r;
+  } else {
+    printf("get_sirf_sgl implementation error\n");
+  }
+}
+
+int decode_sirf_msg_2(unsigned char * buf, int n)
+{
+       int i, p = 0, n_sat;
+       unsigned char mode;
+       const char * altmode = "";
+       const char * pmode = "";
+
+       printf("Measure Navigation Data Out\n");
+       p += 1;
+       printf("  X-position : %d m\n", get_sirf_4s(buf+p));
+       p += 4;
+       printf("  Y-position : %d m\n", get_sirf_4s(buf+p));
+       p += 4;
+       printf("  Z-position : %d m\n", get_sirf_4s(buf+p));
+       p += 4;
+       printf("  X-velocity : %f m/s\n", (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);
+       p += 2;
+       printf("  Z-velocity : %f m/s\n", (double)get_sirf_2s(buf+p) / 8.0);
+       p += 2;
+       /* Mode 1 */
+       mode = buf[p];
+
+       switch ((mode >> 4) & 3) {
+       case 0:
+               altmode = "No altitude hold applied"; break;
+       case 1 :
+               altmode = "Holding of altitude from KF"; break;
+       case 2:
+               altmode = "Holding of altitude from user input"; break;
+       case 3:
+               altmode = "Always hold altitude"; break;
+       }
+
+       switch ( mode & 7) {
+       case 0:
+               pmode = "No navigation solution"; break;
+       case 1:
+               pmode = "1-SV solution (Kalman filter)"; break;
+       case 2:
+               pmode = "2-SV solution (Kalman filter)"; break;
+       case 3:
+               pmode = "3-SV solution (Kalman filter)"; break;
+       case 4:
+               pmode = ">3S-SV solution (Kalman filter)"; break;
+       case 5:
+               pmode = "2D solution (least squares)"; break;
+       case 6:
+               pmode = "3D solution (least squares)"; break;
+       case 7:
+               pmode = "Dead-Reckoning solution (no satellites)"; break;
+       }
+
+       printf("  Mode 1 : %02x %s %s %s %s %s\n", 
+              mode ,
+              mode & (1<<7) ? "DGPS:Yes" : "DGPS:No",
+              mode & (1<<6) ? "DOP mask exceeded " : "",
+              altmode, 
+              mode & (1<<3) ? "TicklePower position " : "",
+              pmode);
+       p += 1;
+       /* HDOP */
+       printf("  HDOP : %f\n", (double)(unsigned)buf[p] / 5.0);
+       p += 1;
+       /* Mode 2 */
+       mode = buf[p];
+       printf("  Mode 2 : %02x\n", mode);
+       p += 1;
+       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);
+       p += 4;
+       printf("  Satellites used : %d\n", n_sat = buf[p]);
+       p += 1;
+       for (i=0; (i<12) & (i<n_sat); i++) {
+                       printf("  Channel %2d SatID: %3u\n", i+1, buf[p+i]);
+       }
+
+       return 0;
+}
+
+int decode_sirf_msg_4(unsigned char *buf, int n)
+{
+       int i, p = 0;
+
+       printf("Measured Tracker Data Out\n");
+       p += 1;
+       printf("  GPS Week : %d\n", get_sirf_2s(buf + p));
+       p += 2;
+       printf("  GPS Time of Week : %u\n", get_sirf_4u(buf + p));
+       p += 4;
+       printf("  Channels : %d\n", buf[p]);
+       p += 1;
+       for (;p<n;) {
+               printf("  SatID: %3u Azimuth: %f Elevation: %f\n",
+                      buf[p], (double)buf[p+1]/2.0*3.0, (double)buf[p+2]/2.0);
+               p += 3;
+               printf("    State : %02x\n", get_sirf_2u(buf + p));
+               p += 2;
+               for (i=0; i<10; i++) {
+                       printf("    SNR %2d: %d dB\n", i+1, buf[p+i]);
+               }
+               p+= 10;
+       }
+
+       return 0;
+}
+
+int decode_sirf_msg_7(unsigned char *buf, int n)
+{
+       int p = 0;
+       int ext_gps_week;
+       double gps_seconds;
+       double clock_bias;
+
+       printf("Clock Status Data\n");
+       p += 1;
+       printf("  Extended GPS Week : %u\n",
+              ext_gps_week = get_sirf_2u(buf + p));
+       p += 2;
+       printf("  GPS Time of Week : %f s\n",
+              gps_seconds = (double)get_sirf_4u(buf + p) / 100.0);
+       p += 4;
+       printf("  Satellites used : %u\n", buf[p]);
+       p += 1;
+       printf("  Clock Drift : %u Hz\n", get_sirf_4u(buf + p));
+       p += 4;
+       printf("  Clock Bias : %u ns\n", get_sirf_4u(buf + p));
+       clock_bias = 1e-9 * (double) get_sirf_4u(buf + p);
+       p += 4;
+       printf("  Estimated GPS Time : %u ms\n", get_sirf_4u(buf + p));
+       p += 4;
+
+       rinex_data_add_time_clock_bias(ext_gps_week, gps_seconds, clock_bias);
+
+       return 0;
+}
+
+int decode_sirf_msg_8(unsigned char *buf, int n)
+{
+       int i, p = 0;
+
+       printf("50 BPS Data\n");
+       p += 1;
+       printf("  Channel %d, SV# %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));
+               p += 4;
+       }
+
+       return 0;
+}
+
+int decode_sirf_msg_13(unsigned char *buf, int n)
+{
+       int p = 0, i, n_sat;
+
+       printf("Visible List\n");
+       p += 1;
+       n_sat = buf[p];
+       p += 1;
+       for (i=0; i<n_sat; i++) {
+               printf("  SatID %3u : Azimuth %3d Elevation %3d\n",
+                      buf[p], get_sirf_2s(buf+p+1), get_sirf_2s(buf+p+3));
+               p += 5;
+       }
+
+       return 0;
+}
+
+int decode_sirf_msg_27(unsigned char *buf, int n)
+{
+       int i, p = 0;
+
+       printf("DGPS Status Format\n");
+       p += 1;
+       printf("  DGPS source = %d\n", buf[p]);
+       p += 1;
+       for (i=0; i<12; i++) {
+               printf("  SatID %3u: Age: %u s, DGPS correction : %d m\n",
+                      buf[p + 14 + 3 * i], buf[p + i],
+                      get_sirf_2s(buf + p + 14 + 3 * i + 1));
+       }
+
+       return 0;
+}
+
+int decode_sirf_msg_28(unsigned char *buf, int n)
+{
+       int p = 0;
+       int satID;
+       double gps_seconds;
+       double pseudorange;
+       double snr_avg = 0.0;
+       int i;
+       int tit;
+
+       printf("Navigation Library Measurement Data - Channel %d\n", buf[p+1]);
+       p += 2;
+       printf("  Time Tag = 0x%08x\n", get_sirf_4u(buf+p));
+       p += 4;
+       printf("  SatID : %3u\n", satID=buf[p]);
+       p += 1;
+       printf("  GPS SW Time : %f s\n", gps_seconds=get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Pseudorange = %f m\n", pseudorange = get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Carrier frequency = %f m/s\n", get_sirf_sgl(buf+p));
+       p += 4;
+       printf("  Carrier phase = %f m\n", get_sirf_dbl(buf+p));
+       p += 8;
+       printf("  Time in track = %u ms\n", tit = get_sirf_2u(buf+p));
+       p += 2;
+       printf("  Sync flags = 0x%x\n", buf[p]);
+       p += 1;
+       for (i=0; i<10; i++) {
+               int snr;
+
+               printf("  SNR %3u dB-Hz\n", snr = buf[p+i]);
+               snr_avg += 0.1 * snr;
+       }
+       p += 10;
+       /* ... */
+
+       /* pseudorange is only valid if time in track is > 0 */
+       if (tit > 0) 
+               rinex_data_add_pseudorange_snr(satID, gps_seconds,
+                                              pseudorange, snr_avg);
+
+       return 0;
+}
+
+int decode_sirf_msg_31(unsigned char *buf, int n)
+{
+       printf("Navigation Library Initialization Data\n");
+       /* TBD */
+
+       return 0;
+}
+
+int decode_sirf_msg_50(unsigned char *buf, int n)
+{
+       int p = 0;
+
+       printf("SBAS Parameters\n");
+       p += 1;
+       printf("  SBAS SatID : %3u\n", buf[p]);
+       p += 1;
+       printf("  SBAS Mode : %02x %s%s\n", buf[p],
+              buf[p] == 0 ? "Testing" : "",
+              buf[p] == 1 ? "Integrity" : "");
+       p += 1;
+       printf("  DGPS Timeout : %u s\n", buf[p]);
+       p += 1;
+       printf("  Flag bits : %02x\n", buf[p]);
+       p += 1;
+       
+       return 0;
+}
+
+int decode_sirf_msg_225(unsigned char *buf, int n)
+{
+       int p = 0;
+
+       printf("Statistics Channel\n");
+       p += 1;
+       /* Message Sub ID : we got 0 or 8 ... undocumented */
+       printf("  Message Sub ID : %d\n", buf[p]);
+       p += 1;
+
+       return 0;
+}
+
+int decode_sirf_msg_255(unsigned char *buf, int n)
+{
+       int p = 0, len;
+       char msg[1024];
+
+       printf("Development Data\n");
+       p += 1;
+
+       if (n < sizeof(msg))
+               len = n;
+       else    len = sizeof(msg) -1;
+       memcpy(msg, buf + p, len);
+       msg[len] = 0;
+
+       printf("  %s\n", msg);
+
+       return 0;
+}
+
+/* (buf,n) is a full SiRF message, including start/stop sequence */
+
+int decode_sirf_msg(unsigned char *buf, int n, int *ack) {
+       int p = 4;
+
+       switch (buf[p]) {
+       case 2: /* Measured Navigation Data Out */
+               decode_sirf_msg_2(buf + 4, n - 8);
+               break;
+    
+       case 4: /* Measured Tracker Data Out */
+               decode_sirf_msg_4(buf + 4, n - 8);
+               break;
+               
+       case 6: /* Software Version String */
+               printf("Software Version String : %s\n", buf+p+1);
+               /* for GPS USB : GSW3.5.0_3.5.00.00-SDK-3EP2.01 */
+               p += 1;
+               break;
+               
+       case 7: /* Clock Status Data */
+               decode_sirf_msg_7(buf + 4, n - 8);
+               break;
+               
+       case 8: /* 50 BPS Data */
+               decode_sirf_msg_8(buf + 4, n - 8);
+               break;
+               
+       case 9: /* CPU Throughput */
+               printf("CPU Throughput\n");
+               break;
+               
+       case 11: /* Command Acknowledgment */
+               printf("Command ACK ID : %d\n",
+                      buf[p+1]);
+               *ack = buf[p+1];
+               p += 2;
+               break;
+               
+       case 12: /* Command Negative Acknowledgment */
+               printf("Command NACK ID : %d\n",
+                      buf[p+1]);
+               *ack = buf[p+1];
+               p += 2;
+               break;
+
+       case 13: /* Visible List */
+               decode_sirf_msg_13(buf + 4, n - 8);
+               break;
+
+       case 27: /* GPS Status Format */
+               decode_sirf_msg_27(buf + 4, n - 8);
+               break;
+               
+       case 28: /* Navigation Library Measurement Data */
+               decode_sirf_msg_28(buf + 4, n - 8);
+               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;
+               break;
+
+       case 31:
+               decode_sirf_msg_31(buf + 4, n - 8);
+               break;
+               
+       case 41: /* Geodetic Navigation Data */
+               printf("Geodetic Navigation Data\n");
+               p += 1;
+               p += 2;
+               p += 2;
+               p += 2;
+               p += 4;
+               
+               printf("Y-M-D : %04d-%02d-%02d H:M:S : %02d:%02d:%02.3f\n",
+                      (buf[p]<<8)|buf[p+1], buf[p+2], buf[p+3],
+                      buf[p+4], buf[p+5], ((buf[p+6]<<8)|buf[p+7])/1000.0);
+               break;
+
+       case 50:
+               decode_sirf_msg_50(buf + 4, n - 8);
+               break;
+
+       case 225:
+               decode_sirf_msg_225(buf + 4, n - 8);
+               break;
+               
+       case 255: /* Development Data */
+               decode_sirf_msg_255(buf + 4, n - 8);
+               break;
+
+       default:
+               printf("Message ID %d is not decoded yet ...\n", buf[p]);
+               break;
+       }
+
+       fflush(stdout);
+       return 0;
+}
+
+void dump_msg(unsigned char *buf, int n) {
+       int i;
+       
+       printf("%d bytes message :", n);
+       for (i=0; i<n; i++) {
+               printf(" 0x%02x", buf[i]);
+       }
+       printf("\n");
+}
+
+/* return 0 on success, -1 on error */
+
+int do_read(int fd, int * ack) {
+       /* SiRF message size is limited to 912 bytes */
+       int i, n;
+
+       static unsigned char sbuffer[912*2];
+       static int p = 0;
+
+       if ((sizeof(sbuffer) - p) == 0) {
+               printf("buffer full -> empty\n");
+               p = 0;
+               return -1;
+       }
+
+       n = read(fd, sbuffer + p, sizeof(sbuffer) - p);
+       if (n <= 0) {
+               perror("read");
+               return -1;
+       }
+
+       if (n == 0) {
+               /* nothing to do */
+               return 0;
+       }
+
+       /* for debug only :
+       printf("%d bytes received (total = %d):", n, p+n);
+       for (i=0; i<n; i++) {
+               printf(" 0x%02x", sbuffer[p+i]);
+       }
+       printf("\n");
+       */
+
+       /* update p with the bytes just received */
+       p += n;
+
+       /* small parsing of received data in (sbuffer, p) : 
+          - start pattern = 0xa0 0xa2
+          - stop  pattern = 0xb0 0xb3
+
+          SiRF message format is :
+          <0xa0 0xa2><length (2 bytes)><length bytes><CRC (2 byte)><0xb0 0xb3>
+       */
+
+       for (;;) {
+               int start_found = 0, stop_found = 0;
+
+               /* search for start pattern */
+               for (i=0; i<p-1; i++) {
+                       if ((sbuffer[i] == 0xa0) && (sbuffer[i+1] == 0xa2)) {
+                               start_found = 1;
+
+                               if (i>0) {
+                                       printf("recv: %d bytes skipped\n", i);
+                                       memmove(sbuffer, sbuffer+i, p-i);
+                                       p -= i;
+                               }
+                               break;
+                       }
+               }
+
+               /* search for stop pattern */
+               for (i=0; i<p-1; i++) {
+                       if ((sbuffer[i] == 0xb0) && (sbuffer[i+1] == 0xb3)) {
+                               stop_found = 1;
+         
+                               if (start_found && stop_found) {
+                                       printf("recv: Message ID %d (%d bytes)\n",
+                                              sbuffer[4], i+2); 
+                                       if (check_sirf_msg(sbuffer, i+2)) {
+                                               decode_sirf_msg(sbuffer, i+2,
+                                                               ack);
+                                       }
+                               }
+         
+                               if (!start_found)
+                                       printf("recv: %d bytes skipped\n",i+2);
+                               memmove(sbuffer, sbuffer+(i+2), p-(i+2));
+                               p -= (i+2);
+
+                               break;
+                       }
+               }
+
+               if (!stop_found)
+                       break;
+       }
+       
+       return 0;
+}
+
+
+/* 
+ * (buf,n) is the payload of the message. This function adds the start/stop
+ * sequences, and the length/checksum fields. Returns 0 on success, -1 on
+ * error.
+ */
+
+int sirf_msg_send(int fd, unsigned char *buf, int n) {
+       unsigned char sbuf[1024];
+       int i;
+       unsigned int crc;
+       int total_len = 4 + n + 4; 
+       int cmd = buf[0];
+
+       if (total_len > sizeof(sbuf)) {
+               printf("%s: message too large\n", __func__);
+               return -1;
+       }
+
+       /* 0xa0, 0xa2 is the start sequence */
+       sbuf[0] = 0xa0;
+       sbuf[1] = 0xa2;
+       sbuf[2] = n>>8;
+       sbuf[3] = n&0xff;
+       memcpy(sbuf+4, buf, n);
+
+       /* compute checksum on the payload */
+       crc = 0;
+       for (i=0; i<n; i++)
+               crc += buf[i];
+       crc &= 0x7fff;
+       
+       sbuf[4+n+0] = (crc & 0xff00)>>8;
+       sbuf[4+n+1] = (crc & 0x00ff)>>0;
+       /* 0xb0, 0xb3 is the end sequence */
+       sbuf[4+n+2] = 0xb0;
+       sbuf[4+n+3] = 0xb3;
+
+       if ((n=write(fd, sbuf, total_len)) != total_len) {
+               printf("%s: written only %d bytes out of %d\n", __func__,
+                      n, total_len);
+               return -1;
+       }
+
+       printf("send: Message ID %d (%d bytes)\n", buf[0], total_len);
+
+       /* wait for ACK */
+       for (;;) {
+               int ack = 0;
+               if (do_read(fd, &ack) < 0) {
+                       printf("do_read failed!\n");
+                       return -1;
+               }
+               if (ack == cmd) {
+                       printf("got ACK for cmd %d\n", ack);
+                       break;
+               }
+       }
+
+       return 0;
+}
+
+void usage(const char *argv0) {
+       printf("usage: %s [-rinex rinex.obs]\n"
+              "  -rinex rinex.obs : record a RINEX observation file\n",
+              argv0);
+       exit (-1);
+}
+
+int main(int argc, const char *argv[])
+{
+       const char device[] = "/dev/ttyUSB0";
+       int fd;
+       struct termios term;
+       int i;
+
+       for (i=1 ; i<argc; i++) {
+               if (strcmp(argv[i],"-rinex")==0 && i+1<argc) {
+                       g_rinex = argv[++i];
+                       rinex_open(g_rinex);
+                       rinex_header_write();
+               } else
+                       usage(argv[0]);
+       }
+
+       /* open serial device */
+       fd = open(device, O_RDWR /*| O_NONBLOCK*/);
+       if (fd < 0) {
+               perror(device);
+               return -1;
+       }
+
+       /* switch to 4800 bauds */
+       if (tcgetattr(fd, &term) != 0) {
+               perror("tcgetattr");
+       }
+       cfmakeraw(&term);
+       cfsetispeed(&term, B115200);
+       cfsetospeed(&term, B115200);
+       /* 8N1 */
+       term.c_cflag &= ~(CSIZE|PARENB|CSTOPB);
+       term.c_cflag |=   CS8;
+       if (tcsetattr(fd, TCSANOW, &term) != 0) {
+               perror("tcsetattr");
+       }
+
+       /* switch from NMEA to SiRF binary format */
+
+       const char to_sirf[] = "$PSRF100,0,9600,8,1,0*0C\n";
+       /* -1 : we do not want to send the null terminating character */
+       //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
+            2400         2400 bit/s
+            4800         4800 bit/s
+            9600         9600 bit/s
+           19200        19200 bit/s
+           38400        38400 bit/s
+           57600        57600 bit/s
+          115200       115200 bit/s
+       */
+
+       unsigned int baud = 115200;
+       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));
+       /*
+       if (tcgetattr(fd, &term) != 0) {
+               perror("tcgetattr");
+       }
+       cfsetispeed(&term, B1200);
+       cfsetospeed(&term, B1200);
+       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));
+
+       /* Initialize GPS/DR Navigation */
+       /*
+       unsigned char msg_172[] = { 0xac, 0x01, 
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00,
+                                   0x00,
+                                   (1<<4)|(1<<5)};
+       sirf_msg_send(fd, msg_172, sizeof(msg_172));
+       */
+
+       /* Initialize Data Source */
+       unsigned char msg_128[] = { 128,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00, 0x00, 0x00,
+                                   0x00, 0x00,
+                                   12,
+                                   /* Clear memory */
+                                   /* (1<<2) | */
+                                   /* 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 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));
+
+       /* Try to enable all messages. We got a ACK for :
+            0, Undocumented (never received)
+            2, Measure Navigation Data Out
+               => X,Y,Z,dX,dY,dZ,T, HDOP, sat used
+               used by gpsd to report longitude, latitude
+            4, Measured Tracker Data Out
+               => T, SV ID, Azimuth, Elevation, SNR
+            7, Clock Status Data
+            8, 50 BPS Data
+            9, CPU Throughput
+           13, Visible List
+               => SV ID, Azimuth, Elevation
+           18, OkToSend (never received)
+           27, DGPS Status Format
+           28, Navigation Library Measurement Data
+               => SV ID, T, Pseudorange, Carrier freq+phase
+           29, Navigation Library DGPS Data (never received)
+           30, Navigation Library SV State Data
+               => SV ID, 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)
+           50, SBAS Parameters
+           55, Test Mode 4 Track Data (never received)
+          128, Undocumented (never received)
+          255, Development Data
+        */
+       /*
+       for (i=0; i<256; i++) {
+               printf("Trying to enable message %d\n", i);
+               msg_166[2] = i;
+               sirf_msg_send(fd, msg_166, sizeof(msg_166));
+       }
+       */
+       for (;;) {
+               fd_set fdr;
+               int r;
+
+               FD_ZERO(&fdr);
+               FD_SET(fd, &fdr);
+
+               r = select(FD_SETSIZE, &fdr, NULL, NULL, NULL);
+               if (r < 0) {
+                       perror("select");
+                       break;
+               }
+
+               if (FD_ISSET(fd, &fdr)) {
+                       int ack = 0;
+                       if (do_read(fd, &ack) < 0)
+                               break;
+                       if (ack != 0) {
+                               printf("ACK for cmd %d\n", ack);
+                       }
+               }
+       }
+
+       close (fd);
+       return 0;
+}