Added a small program to post process RAW file and generate a RINEX file
authorBenoit Papillault <benoit.papillault@free.fr>
Fri, 30 Dec 2011 11:10:43 +0000 (12:10 +0100)
committerBenoit Papillault <benoit.papillault@free.fr>
Fri, 30 Dec 2011 11:10:43 +0000 (12:10 +0100)
sirf3-post.cpp [new file with mode: 0644]

diff --git a/sirf3-post.cpp b/sirf3-post.cpp
new file mode 100644 (file)
index 0000000..a5d2275
--- /dev/null
@@ -0,0 +1,468 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include "RinexObsStream.hpp"
+#include "RinexObsHeader.hpp"
+#include "RinexObsData.hpp"
+
+#include "DayTime.hpp"
+#include "icd_200_constants.hpp"
+
+using namespace std;
+using namespace gpstk;
+
+RinexObsStream r_out;
+RinexObsHeader r_header;
+RinexObsData   r_data;
+
+unsigned int last_tt = 0; /* set by parse_mid_28 */
+
+unsigned int first_tt;
+unsigned int prev_tt = 0;
+unsigned int prev_diff_tt = 0;
+
+double local_second = 0.0;
+double first_gps_second = 0.0;
+double prev_gps_second = 0.0;
+double prev_bias = 0.0;
+double prev_diff_gps_second = 0.0;
+
+int mid_7_count = 0;
+int first_mid_7_count = 0;
+
+int alpha_count = 0;
+double alpha = 1.0 / 16370015.0;
+double beta = 0.0;
+double period = 16369007.0 * alpha;
+
+#define TAB_SIZE 100
+int tab_val[TAB_SIZE] = { 0, };
+int tab_pos = 0;
+
+int sum_val = 0;
+int sum_count = 0;
+
+double last_tt_sec = 0.0;
+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;
+
+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_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;
+
+       // reset r_data
+       r_data.obs.clear();
+
+       return 0;
+}
+
+/* from Message ID 28 */
+int rinex_data_add_pseudorange_snr(int satNR,
+                                  double gps_second,
+                                  double pseudorange,
+                                  double snr) {
+
+       // compute ssi
+       int ssi = (int)(snr / 6.0);
+       if (ssi < 0)
+               ssi = 0;
+       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;
+       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 gps_week,
+                                  double gps_second, double clock_bias) {
+       
+       // 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);
+               }
+       }
+
+       /* write data */
+       rinex_data_write();
+
+       return 0;
+}
+
+void tab_add_val(int val) {
+       tab_val[tab_pos] = val;
+       tab_pos = (tab_pos + 1) % TAB_SIZE;
+}
+
+double tab_get_avg() {
+       int i;
+       double avg = 0.0;
+
+       for (i=0; i<TAB_SIZE; i++)
+               avg += (double)tab_val[i];
+       avg = avg / (double)TAB_SIZE;
+
+       return avg;
+}
+
+void parse_mid_7(const char *buf) {
+       int 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);
+
+       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);
+
+       diff_tt = last_tt - prev_tt;
+       diff_diff_tt = diff_tt - prev_diff_tt;
+
+       /* VERY IMPORTANT LINE - we supposed that bias is computed again the
+          beginning of the second. This is no longer the case. gps_second
+          is the GPS time (as opposed to local time) when this signal was
+          TRANSMITTED. If we substrat bias, we get the LOCAL (receiver)
+          clock time */
+       gps_second = gps_second - bias;
+       diff_gps_second = gps_second - prev_gps_second;
+       diff_diff_gps_second = diff_gps_second - prev_diff_gps_second;
+
+       if (mid_7_count == 1) {
+               first_tt = last_tt;
+               first_gps_second = gps_second;
+               first_mid_7_count = mid_7_count;
+               alpha_count = 0;
+       }
+
+       if (mid_7_count > 2) {
+               double diff_tt_sec, last_diff_tt_sec;
+               double avg;
+
+               printf("\tsec %.12f diff_sec %.12f diff_diff_sec %.12f\n",
+                      gps_second, diff_gps_second, diff_diff_gps_second);
+               printf("\ttt %u diff_tt %d diff_diff_tt %d\n",
+                      last_tt, diff_tt, diff_diff_tt);
+
+               /* adjusting alpha first, detecting rollover */
+               if ((gps_second < prev_gps_second) ||
+                   (last_tt < prev_tt) ||
+                   (bias < prev_bias)) {
+                       printf("\tResetting alpha...\n");
+                       first_tt = last_tt;
+                       first_gps_second = gps_second;
+                       first_mid_7_count = mid_7_count;
+                       alpha_count = 0;
+               }
+
+               diff_tt_sec = (double)diff_tt * alpha;
+               switch (diff_diff_tt) {
+               case -1:
+               case 0:
+               case 1:
+                       /* replace with the smoothed value (more
+                          accurate) */
+                       diff_tt_sec = period;
+                       break;
+               }
+
+               printf("\tdiff_tt_sec %.12f - diff_sec = %.12f%c [diff_diff_tt_sec %.12f]\n",
+                      diff_tt_sec, diff_tt_sec - diff_gps_second,
+                      ((diff_tt_sec - diff_gps_second) > alpha/2.0) ||
+                      ((diff_tt_sec - diff_gps_second) < -alpha/2.0) ? 'W' : ' ',
+                      diff_diff_tt_sec);
+
+               alpha_count ++;
+               if (alpha_count > 100) {
+                       printf("\tComputing alpha with :\n"
+                              "\ttt %u -> %u sec %.12f -> %.12f\n"
+                              "\tmid_7_count %d first_mid_7_count %d\n",
+                              first_tt, last_tt, first_gps_second, gps_second,
+                              mid_7_count, first_mid_7_count);
+                       alpha = (gps_second - first_gps_second) /
+                               (double)(last_tt - first_tt);
+                       beta  = gps_second - (double)last_tt * alpha;
+                       printf("\talpha %.12g beta %.12g 1/alpha %.12g\n",
+                              alpha, beta, 1.0/alpha);
+                       period = (gps_second - first_gps_second) /
+                               (double)(mid_7_count - first_mid_7_count);
+                       printf("\tperiod %.12f\n", period);
+                       alpha_count = 0;
+
+                       /* FIXME */
+                       last_tt_sec = gps_second - diff_tt_sec;
+               }
+
+               last_tt_sec += diff_tt_sec;
+               last_diff_tt_sec = last_tt_sec - gps_second;
+               printf("\tlast_tt_sec %.12f - sec = %.12f diff %.12f\n",
+                      last_tt_sec, last_diff_tt_sec,
+                      last_diff_tt_sec - prev_diff_tt_sec);
+               prev_diff_tt_sec = last_diff_tt_sec;
+
+               tab_add_val(diff_diff_tt);
+               avg = tab_get_avg();
+
+               printf("\tavg %.12f avg_sec %.12f [count %d]\n",
+                      avg, avg*alpha, mid_7_count);
+
+               sum_val   += diff_diff_tt;
+               sum_count += 1;
+
+               printf("\tsum_count %d avg_sec %.12f step %.12f\n",
+                       sum_count,
+                       (double)sum_val / (double)sum_count * alpha,
+                       alpha / (double) sum_count);
+       }
+
+       prev_tt = last_tt;
+       prev_diff_tt = diff_tt;
+
+       prev_gps_second = gps_second;
+       prev_bias = bias;
+       prev_diff_gps_second = diff_gps_second;
+}
+
+void parse_mid_28(const char *buf) {
+       int 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);
+
+       if (tit > 1.0) {
+               rinex_data_add_pseudorange_snr(satID, gps_second,
+                                              pseudorange, snr_avg);
+       }
+
+       printf("MID 28 : %2d %10u %19.12f %18.6f %6.3f %6.3f\n",
+              satID, tt, gps_second, pseudorange, tit, snr_avg);
+
+       last_tt = tt;
+       local_second = gps_second;
+}
+
+void usage() {
+       printf("sirf3-post file.raw [-rinex file.obs]\n");
+       exit (-1);
+}
+
+int main(int argc, const char *argv[]) {
+       FILE * file;
+       const char * filename = NULL;
+       char buf[1024];
+       int i;
+
+       for (i=1; i<argc; i++) {
+               if (strcmp(argv[i],"-rinex")==0 && i+1<argc) {
+                       const char * rinex = argv[++i];
+                       rinex_open(rinex);
+                       rinex_header_write();
+               } else if (filename == NULL) {
+                       filename = argv[i];
+               } else 
+                       usage();
+       }
+
+       if (filename == NULL)
+               usage();
+
+       file = fopen(filename, "r");
+
+       while (fgets(buf, sizeof(buf), file) != NULL) {
+               int mid;
+
+               if (sscanf(buf, "%d", &mid) != 1) {
+                       printf("error: %s", buf);
+                       continue;
+               }
+
+               switch (mid) {
+               case 7:
+                       parse_mid_7(buf);
+                       break;
+               case 28:
+                       parse_mid_28(buf);
+                       break;
+               default:
+                       printf("unknown MID %d\n", mid);
+                       break;
+               }
+       }
+
+       fclose (file);
+       return 0;
+}