From b427fb2ca6ede7c6dca9c9fc587dc353ad6b1f3c Mon Sep 17 00:00:00 2001
From: bert hubert <bert.hubert@netherlabs.nl>
Date: Tue, 4 Feb 2020 21:09:10 +0100
Subject: [PATCH] our primordial but very fast rinex nav parser + generator

---
 rinex.cc | 213 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 rinex.hh | 128 +++++++++++++++++++++++++++++++++
 2 files changed, 341 insertions(+)
 create mode 100644 rinex.cc
 create mode 100644 rinex.hh

diff --git a/rinex.cc b/rinex.cc
new file mode 100644
index 0000000..8cfc4c5
--- /dev/null
+++ b/rinex.cc
@@ -0,0 +1,213 @@
+#include "rinex.hh"
+#include <string>
+#include <string.h>
+#include <stdexcept>
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+
+/*
+Header:
+     3.03           N: GNSS NAV DATA    M: MIXED            RINEX VERSION / TYPE
+sbf2rin-13.2.1                          20191217 004932 UTC PGM / RUN BY / DATE 
+GPSA   8.3819E-09 -1.4901E-08 -5.9605E-08  1.1921E-07       IONOSPHERIC CORR    
+GPSB   9.4208E+04 -1.3107E+05 -1.3107E+05  8.5197E+05       IONOSPHERIC CORR    
+GAL    3.0500E+01  3.3203E-01  2.9907E-03  0.0000E+00       IONOSPHERIC CORR    
+GPUT  5.8207660913E-11 3.996802889E-15 122400 2084          TIME SYSTEM CORR    
+GAUT  0.0000000000E+00 0.000000000E+00  86400 2084          TIME SYSTEM CORR    
+GPGA  4.0745362639E-09 9.325873407E-15 172800 2084          TIME SYSTEM CORR    
+    18                                                      LEAP SECONDS        
+                                                            END OF HEADER       
+*/
+
+RINEXReader::RINEXReader(std::string_view fname)
+{
+  d_fname=fname;
+  d_fp = gzopen(&fname[0], "r");
+  if(!d_fp)
+    throw runtime_error("Unable to open "+(string)fname+": "+strerror(errno));
+
+  try {
+    skipFileHeader();
+  }
+  catch(...) {
+    gzclose(d_fp);
+    throw;
+  }
+}
+
+void RINEXReader::skipFileHeader()
+{
+  char line[300];
+  bool eoh=false;
+  int lines=0;
+  while(gzgets(d_fp, line, sizeof(line))) {
+    if(strstr(line, "END OF HEADER")) {
+      eoh=true;
+      break;
+    }
+    if(lines++ > 250)
+      break;
+  }
+  if(!eoh) {
+    throw std::runtime_error("Did not find RINEX END OF HEADER in "+d_fname);
+  }
+}
+
+RINEXReader::~RINEXReader()
+{
+  if(d_fp)
+    gzclose(d_fp);
+}
+
+bool RINEXReader::get(RINEXEntry& entry)
+{
+  char line[300];
+  struct tm tm;
+  memset(&tm, 0, sizeof(tm));
+
+  /* 
+G02 2019 12 16 00 00 00-3.670863807201E-04-7.389644451905E-12 0.000000000000E+00
+     7.400000000000E+01-9.337500000000E+01 4.647693595094E-09-1.766354782990E+00
+    -4.515051841736E-06 1.956839906052E-02 2.739951014519E-06 5.153594841003E+03
+     8.640000000000E+04 3.296881914139E-07-3.996987526460E-01 2.700835466385E-07
+     9.574821167563E-01 3.221250000000E+02-1.689421056425E+00-8.195698526598E-09
+    -2.167947446570E-10 1.000000000000E+00 2.084000000000E+03 0.000000000000E+00
+     2.000000000000E+00 0.000000000000E+00-1.769512891769E-08 7.400000000000E+01
+     7.921800000000E+04 4.000000000000E+00
+  */
+  
+
+  //  SV   YR  MN DY HR MN SS_______________====================______________________
+  //  G02 2019 12 16 00 00 00-3.670863807201E-04-7.389644451905E-12 0.000000000000E+00
+
+  for(;;) {
+    if(!(gzgets(d_fp, line, sizeof(line))))
+      return false;
+    //    cerr<<"Line: "<<line;
+    if(strstr(line, "RINEX")) {
+      //      cerr<<"skipFileHeader"<<endl;
+      skipFileHeader();
+      continue; 
+    }
+    
+    //    cout<<"Line: '"<<line<<"'"<<endl;
+    bool unknownConstellation=false;
+
+    char constellation = *line;
+    if(*line=='G')
+      entry.gnss=0;
+    else if(*line=='E')
+      entry.gnss = 2;
+    else if(*line=='C')
+      entry.gnss = 3;
+    else if(*line=='E')
+      entry.gnss = 6;
+    else
+      unknownConstellation = true;
+
+    if(constellation=='S' || constellation=='R') {
+      for(int n=0 ; n < 3; ++n) {
+        if(!gzgets(d_fp, line, sizeof(line))) 
+          return false;
+      }
+      continue;
+
+    }
+    
+    line[24]=0;
+    char gnss;
+    if(sscanf(line, "%c%02d %d %d %d %d %d %d",
+           &gnss, &entry.sv, &tm.tm_year, &tm.tm_mon, &tm.tm_mday,
+              &tm.tm_hour, &tm.tm_min, &tm.tm_sec) != 8) {
+      throw std::runtime_error("Failed to parse '"+string(line)+"'");
+    }
+
+    bool skip=false;
+    /*
+    if(tm.tm_year != 2019 || tm.tm_mon != 7)
+      skip=true;
+
+    if((entry.sv == 33 || entry.sv == 36 || entry.sv == 13 || entry.sv == 15)
+       && tm.tm_mon < 3)
+      skip = true;
+    */
+    tm.tm_mon -= 1;
+    tm.tm_year -= 1900;
+
+    entry.t=timegm(&tm);
+
+    // skip 5 lines, store 6th
+    for(int n=0 ; n < 6; ++n) {
+      if(!gzgets(d_fp, line, sizeof(line))) 
+        return false;
+      if(n==2) {
+        line[23]=0;
+        double toe;
+        sscanf(line+4, "%lf", &toe);
+        entry.toe = toe;
+      }
+      //      cerr<<"Line "<<n<<": "<<line;
+    }
+    char tmp=line[23];
+    line[23]=0;
+    sscanf(line+4, "%lf", &entry.sisa);
+    line[23]=tmp;
+
+    tmp=line[42];
+    line[42]=0;
+
+    double health;
+    sscanf(line+23, "%lf", &health);
+    entry.health = health; // yeah..
+    
+    //last line
+    if(!gzgets(d_fp, line, sizeof(line)))
+      return false;
+
+    line[23]=0;
+    double tow;
+    sscanf(line+4, "%lf", &tow);
+    entry.tow = tow;
+    
+    if(skip)
+      continue;
+    if(unknownConstellation)
+      continue;
+    return true;
+  }
+  return false;
+}
+
+RINEXNavWriter::RINEXNavWriter(std::string_view fname) : d_ofs((string) fname)
+{
+  extern const char* g_gitHash;
+
+  d_ofs <<"      3.03          N: GNSS NAV DATA    M: MIXED            RINEX VERSION / TYPE\n";
+  // 0                 20                             40                  60
+  // sbf2rin-13.2.0    galmon.eu                      20190923 000219 UTC PGM / RUN BY / DATE 
+
+  time_t now=time(0);
+  struct tm tm;
+  gmtime_r(&now, &tm);
+
+  
+  d_ofs << fmt::format("{:<20}{:<20}{:<20}PGM / RUN BY / DATE\n",
+                      "galmon-"+string(g_gitHash),
+                      "galmon.eu project",
+                     fmt::sprintf("%4d%02d%02d %02d%02d%02d UTC",
+                                  tm.tm_year +1900,
+                                  tm.tm_mon + 1,
+                                  tm.tm_mday,
+                                  tm.tm_hour,
+                                  tm.tm_min,
+                                  tm.tm_sec));
+  /*
+"GAL    2.9500E+01  7.0312E-02  1.0590E-02  0.0000E+00        IONOSPHERIC CORR    \n"
+"GAUT -9.3132257462E-10 8.881784197E-16      0 2072           TIME SYSTEM CORR    \n"
+  */
+  d_ofs<<"   18                                                       LEAP SECONDS        \n";
+  d_ofs<<"                                                            END OF HEADER       \n";
+
+}
diff --git a/rinex.hh b/rinex.hh
new file mode 100644
index 0000000..20da83e
--- /dev/null
+++ b/rinex.hh
@@ -0,0 +1,128 @@
+#pragma once
+#include <string>
+#include <time.h>
+#include <stdio.h>
+#include <zlib.h>
+#include <fstream>
+#include "navmon.hh"
+#include <time.h>
+#include "fmt/format.h"
+#include "fmt/printf.h"
+
+struct RINEXEntry
+{
+  time_t t;
+  int gnss;
+  int sv;
+  double sisa;
+  int health;
+  int toe;
+  int tow;
+};
+
+class RINEXReader
+{
+public:
+  RINEXReader(std::string_view fname);
+  bool get(RINEXEntry& rinex);
+  ~RINEXReader();
+private:
+  void skipFileHeader();
+  std::string d_fname;
+  gzFile d_fp{0};
+  time_t d_time{0};
+};
+
+class RINEXNavWriter
+{
+public:
+  explicit RINEXNavWriter(std::string_view fname);
+  template<typename T>
+  void emitEphemeris(const SatID& sid, const T& t);
+private:
+  std::ofstream d_ofs;
+  void emit(double n)
+  {
+    if(n >= 0)
+      d_ofs << fmt::format(" {:18.12E}", n);
+    else
+      d_ofs << fmt::format("{:019.12E}", n);
+  }
+};
+
+template<typename T>
+void RINEXNavWriter::emitEphemeris(const SatID& sid, const T& e)
+{
+
+  /*
+E01 2019 09 21 23 30 00-6.949011585675E-04-7.943867785798E-12 0.000000000000E+00
+     1.090000000000E+02 9.746875000000E+01 2.820474626946E-09 2.393449606726E+00
+     4.611909389496E-06 2.816439373419E-04 7.767230272293E-06 5.440614154816E+03
+     6.030000000000E+05 1.862645149231E-09-1.121206798215E+00-4.656612873077E-08
+     9.859399710315E-01 1.802500000000E+02-2.137974852171E+00-5.485228481783E-09
+    -1.789360248322E-10 5.170000000000E+02 2.071000000000E+03
+     3.120000000000E+00 0.000000000000E+00-1.862645149231E-09-2.095475792885E-09
+     6.036660000000E+05
+  */
+
+  using std::endl;
+  time_t then = utcFromGST(e.wn, (int)e.tow);
+  struct tm tm;
+  gmtime_r(&then, &tm);
+  
+  d_ofs << makeSatPartialName(sid)<<" " << fmt::sprintf("%04d %02d %02d %02d %02d %02d", tm.tm_year + 1900, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec);
+
+  emit(ldexp(e.af0, -34));
+  emit(ldexp(e.af1, -46));
+  emit(ldexp(e.af2, -59));
+  d_ofs<<"\n    ";
+
+  emit(e.iodnav);
+  emit(e.getCrs());
+  emit(e.getDeltan());
+  emit(e.getM0());
+  d_ofs<<"\n    ";
+
+
+  emit(e.getCuc());
+  emit(e.getE());
+  emit(e.getCus());
+  emit(e.getSqrtA());
+  d_ofs<<"\n    ";
+
+  emit(e.getT0e());
+  emit(e.getCic());
+  emit(e.getOmega0());
+  emit(e.getCis());
+
+  d_ofs<<"\n    ";
+  emit(e.getI0());
+  emit(e.getCrc());
+  emit(e.getOmega());
+  emit(e.getOmegadot());
+
+  d_ofs<<"\n    ";
+  emit(e.getIdot());
+  emit(257);
+  emit(e.wn + 1024); // so it aligns with GPS
+
+
+  d_ofs<<"\n    ";
+  emit(numSisa(e.sisa));
+  int health=0;
+  health |= e.e1bdvs;
+  health |= (e.e1bhs << 2);
+  // don't have e5advs
+  // don't have e5ahs
+  health |= (e.e5bdvs << 6);
+  health |= (e.e5bhs << 8);
+  emit(health); // HEALTH
+  emit(ldexp(e.BGDE1E5a, -32));
+  emit(ldexp(e.BGDE1E5b, -32));
+
+  d_ofs<<"\n    ";
+  emit(e.tow); // XXX
+
+  d_ofs<<std::endl;
+  
+}