add TLEs

.gitmodules

@@ -1,3 +1,6 @@
 [submodule "ext/powerblog"]
 	path = ext/powerblog
 	url = https://github.com/ahupowerdns/powerblog.git
+[submodule "ext/sgp4"]
+	path = ext/sgp4
+	url = https://github.com/dnwrnr/sgp4.git

Makefile

@@ -1,8 +1,11 @@
-CXXFLAGS:= -std=gnu++17 -Wall -O3 -MMD -MP -ggdb -fno-omit-frame-pointer -Iext/CLI11  -Iext/fmt-5.2.1/include/ -Iext/powerblog/ext/simplesocket -Iext/powerblog/ext/ -I/usr/local/opt/openssl/include/ 
+CXXFLAGS:= -std=gnu++17 -Wall -O3 -MMD -MP -ggdb -fno-omit-frame-pointer -Iext/CLI11 \
+	 -Iext/fmt-5.2.1/include/ -Iext/powerblog/ext/simplesocket -Iext/powerblog/ext/ \
+	 -I/usr/local/opt/openssl/include/  \
+	 -Iext/sgp4/libsgp4/
 
 # CXXFLAGS += -Wno-delete-non-virtual-dtor
 
-PROGRAMS = navparse ubxtool navnexus navrecv navdump testrunner navdisplay
+PROGRAMS = navparse ubxtool navnexus navrecv navdump testrunner navdisplay tlecatch
 
 all: navmon.pb.cc $(PROGRAMS)
 
@@ -14,10 +17,10 @@ clean:
 H2OPP=ext/powerblog/h2o-pp.o
 SIMPLESOCKETS=ext/powerblog/ext/simplesocket/swrappers.o ext/powerblog/ext/simplesocket/sclasses.o  ext/powerblog/ext/simplesocket/comboaddress.o 
 
-navparse: navparse.o ext/fmt-5.2.1/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o
+navparse: navparse.o ext/fmt-5.2.1/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o
 	$(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -L/usr/local/opt/openssl/lib/  -lh2o-evloop -lssl -lcrypto -lz  -lcurl -lprotobuf  # -lwslay
 
-navdump: navdump.o ext/fmt-5.2.1/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o navmon.o
+navdump: navdump.o ext/fmt-5.2.1/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o navmon.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o
 	$(CXX) -std=gnu++17 $^ -o $@ -pthread  -lprotobuf
 
 navdisplay: navdisplay.o ext/fmt-5.2.1/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o
@@ -30,6 +33,8 @@ navnexus: navnexus.o ext/fmt-5.2.1/src/format.o  $(SIMPLESOCKETS) ubx.o bits.o n
 navrecv: navrecv.o ext/fmt-5.2.1/src/format.o $(SIMPLESOCKETS) navmon.pb.o storage.o
 	$(CXX) -std=gnu++17 $^ -o $@ -pthread -lprotobuf  
 
+tlecatch: tlecatch.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc))
+	$(CXX) -std=gnu++17 $^ -o $@ -pthread -lprotobuf  
 
 navmon.pb.cc: navmon.proto
 	protoc --cpp_out=./ navmon.proto

ephemeris.cc

@@ -1,4 +1,5 @@
 #include "ephemeris.hh"
+#include "minivec.hh"
 /* |            t0e tow     |  - > tow - t0e, <3.5 days, so ok
 
    | t0e                tow |   -> tow - t0e > 3.5 days, so 
@@ -31,3 +32,27 @@ int ephAge(int tow, int t0e)
   }
 }
 
+// all axes start at earth center of gravity
+// x-axis is on equator, 0 longitude
+// y-axis is on equator, 90 longitude
+// z-axis is straight up to the north pole
+// https://en.wikipedia.org/wiki/ECEF#/media/File:Ecef.png
+std::pair<double,double> getLongLat(double x, double y, double z)
+{
+  Point core{0,0,0};
+  Point LatLon0{1,0,0};
+
+  Point pos{x, y, z};
+  
+  Point proj{x, y, 0}; // in equatorial plane now
+  Vector flat(core, proj);
+  Vector toLatLon0{core, LatLon0};
+  double longitude = acos( toLatLon0.inner(flat) / (toLatLon0.length() * flat.length()));
+
+
+  Vector toUs{core, pos};
+  double latitude =  acos( flat.inner(toUs) / (toUs.length() * flat.length()));
+
+
+  return std::make_pair(longitude, latitude);
+}

ephemeris.hh

@@ -184,3 +184,5 @@ DopplerData doDoppler(int wn, int tow, const Point& us, const T& eph, double fre
 
   return ret;
 }
+
+std::pair<double,double> getLongLat(double x, double y, double z);

ext/sgp4

@@ -0,0 +1 @@
+Subproject commit 6b47861cd47a6e31841260c47a52b579f8cf2fa9

galileo.hh

@@ -96,10 +96,38 @@ struct GalileoMessage
   struct Almanac
   {
     int svid{-1};
+    int t0almanac, wnalmanac;
     int af0, af1;
     int e1bhs, e5bhs;
-  } alma1, alma2, alma3;
+
+    uint32_t e, deltaSqrtA;
+    int32_t M0, Omega0, deltai, omega, Omegadot;
+
+    double getMu() const { return 3.986005    * pow(10.0, 14.0); }
+    double getOmegaE()    const { return 7.2921151467 * pow(10.0, -5.0);} // rad/s
+
+    uint32_t getT0e() const { return 600 * t0almanac; }
+    double getSqrtA() const { return sqrt(29600000) + ldexp(deltaSqrtA,     -9);   }
+    double getE()     const { return ldexp(e,         -16);   }
+
+
+    double getI0()        const { return M_PI*56.0/180.0 + ldexp(deltai * M_PI,       -14);   } // radians
+    double getOmega0()    const { return ldexp(Omega0 * M_PI,   -15);   } // radians
+    double getOmegadot()  const { return ldexp(Omegadot * M_PI, -33);   } // radians/s
+    double getOmega()     const { return ldexp(omega * M_PI,    -15);   } // radians
+    double getM0()    const { return ldexp(M0 * M_PI, -15);   } // radians    
+
+    double getIdot()      const { return 0;   } // radians/s
+    double getCic()       const { return 0;   } // radians
+    double getCis()       const { return 0;   } // radians
+    double getCuc()   const { return 0;   } // radians
+    double getCus()   const { return 0;   } // radians
+    double getCrc()   const { return 0;   } // meters
+    double getCrs()   const { return 0;   } // meters
+    double getDeltan()const { return 0; } //radians/s
     
+  } alma1, alma2, alma3;
+
   
   // an ephemeris word
   void parse1(std::basic_string_view<uint8_t> page)
@@ -206,43 +234,61 @@ struct GalileoMessage
   void parse7(std::basic_string_view<uint8_t> page)
   {
     iodalmanac = getbitu(&page[0], 6, 4);
-    wnalmanac = getbitu(&page[0], 10, 2);
+    alma1.wnalmanac = wnalmanac = getbitu(&page[0], 10, 2);
-    t0almanac = getbitu(&page[0], 12, 10);
+    alma1.t0almanac = t0almanac = getbitu(&page[0], 12, 10);
     alma1.svid     = getbitu(&page[0], 22, 6);
+    alma1.deltaSqrtA = getbitu(&page[0], 28, 13);
+    alma1.e = getbitu(&page[0], 41, 11);
+    alma1.omega = getbits(&page[0], 52, 16);
+    alma1.deltai = getbits(&page[0], 68, 11);
+    alma1.Omega0 = getbits(&page[0], 79, 16);
+    alma1.Omegadot = getbits(&page[0], 95, 11);
+    alma1.M0 = getbits(&page[0], 106, 16);
     
   }
   // almanac
   void parse8(std::basic_string_view<uint8_t> page)
   {
+    iodalmanac = getbitu(&page[0], 6, 4);
     alma1.af0 = getbits(&page[0], 10, 16);
     alma1.af1 = getbits(&page[0], 26, 13);
     alma1.e5bhs = getbitu(&page[0], 39, 2);
     alma1.e1bhs = getbitu(&page[0], 41, 2);
+
     alma2.svid     = getbitu(&page[0], 43, 6);
+    
+    alma2.deltaSqrtA = getbitu(&page[0], 49, 13);
+    alma2.e = getbitu(&page[0], 62, 11);
+    alma2.omega = getbits(&page[0], 73, 16);
+    alma2.deltai = getbits(&page[0], 89, 11);
+    alma2.Omega0 = getbits(&page[0], 100, 16);
+    alma2.Omegadot = getbits(&page[0], 116, 11);
   }
 
   // almanac
   void parse9(std::basic_string_view<uint8_t> page)
   {
+    iodalmanac = getbitu(&page[0], 6, 4);
+    alma2.wnalmanac = wnalmanac = getbitu(&page[0], 10, 2);
+    alma2.t0almanac = t0almanac = getbits(&page[0], 12, 10);
+
+    alma2.M0 = getbits(&page[0], 22, 16);
     alma2.af0 = getbits(&page[0], 38, 16);
     alma2.af1 = getbits(&page[0], 54, 13);
     alma2.e5bhs = getbitu(&page[0], 67, 2);
     alma2.e1bhs = getbitu(&page[0], 69, 2);
     
     alma3.svid = getbitu(&page[0], 71, 6);
-    
-    
   }
 
   // almanac + more time stuff (GPS)
   void parse10(std::basic_string_view<uint8_t> page)
   {
-
+    iodalmanac = getbitu(&page[0], 6, 4);
     alma3.af0 = getbits(&page[0], 53, 16);
     alma3.af1 = getbits(&page[0], 69, 13);
     alma3.e5bhs = getbitu(&page[0], 82, 2);
     alma3.e1bhs = getbitu(&page[0], 84, 2);
-
     
     a0g = getbits(&page[0], 86, 16);
     a1g = getbits(&page[0], 102, 12);

glonass.hh

@@ -42,8 +42,19 @@ struct GlonassMessage
      Various GLONASS things relate to "the day", so it is important to note which day we are at
   */
   uint8_t hour, minute, seconds, P1;
-  int32_t x, dx, ddx;
+  int32_t x{0}, dx, ddx; // 2^-11 km, 2^-20 km/s, 2^-30 km/s^2
+
+  double getX() { return ldexp(x*1000.0, -11); }
+  double getY() { return ldexp(y*1000.0, -11); }
+  double getZ() { return ldexp(z*1000.0, -11); }
+
+  double getdX() { return ldexp(dx*1000.0, -20); }
+  double getdY() { return ldexp(dy*1000.0, -20); }
+  double getdZ() { return ldexp(dz*1000.0, -20); }
+
   
+  double getRadius() { return sqrt(getX()*getX() + getY()*getY() + getZ()*getZ()); }
+   
   void parse1(std::basic_string_view<uint8_t> gstr)
   {
     hour = getbitu(&gstr[0], 9, 5);
@@ -56,7 +67,7 @@ struct GlonassMessage
   }
 
   uint8_t Bn, Tb, P2;
-  int32_t y, dy, ddy;
+  int32_t y{0}, dy, ddy;
 
   /* The GLONASS ephemeris centered on the "Tb-th" interval, from the start of the Moscow day.
      An interval is 15 minutes long, plus a spacer of length described by P1. If P1 is zero, there is no spacer.
@@ -74,7 +85,7 @@ struct GlonassMessage
     
   }
 
-  int32_t z, dz, ddz;
+  int32_t z{0}, dz, ddz;
   bool l_n;
   bool P, P3;
   uint16_t gamman;
@@ -136,12 +147,20 @@ struct GlonassMessage
 
   int nA;
   bool CnA;
-  
+  int32_t lambdana; // 2^-20 semi-circles
+  int32_t deltaina; // 2^-20 semi-circles
+
+  double getLambdaNaDeg()
+  {
+    return ldexp(180.0*lambdana, -20);
+  }
   
   void parse6_8_10_12_14(std::basic_string_view<uint8_t> gstr)
   {
     CnA = getbitu(&gstr[0], 85-80, 1);
     nA = getbitu(&gstr[0], 85-77, 5);
+    lambdana = getbitsglonass(&gstr[0], 85-62, 21);
+    deltaina = getbitsglonass(&gstr[0], 85-41, 18);
   }
   
 };

html/doalles.js

@@ -14,7 +14,7 @@ function maketable(str, arr)
         enter().
         append("tr");
     
-    var columns = ["sv", "iod", "aodc/e", "eph-age-m", "latest-disco", "sisa", "delta_hz_corr", "health", "a0", "a1","a0g", "a1g", "sources", "db", "elev", "last-seen-s"];    
+    var columns = ["sv", "iod", "aodc/e", "eph-age-m", "latest-disco", "time-disco", "sisa", "delta_hz_corr", "health", "a0", "a1","a0g", "a1g", "sources", "db", "elev", "last-seen-s"];    
     
     // append the header row
     thead.append("tr")
@@ -53,6 +53,8 @@ function maketable(str, arr)
                 else if(column == "aodc/e") {
                     if(row["aodc"] != null && row["aode"] != null)
                         ret.value = row["aodc"]+"/"+row["aode"];
+                    else if(row["aode"] != null)
+                        ret.value = row["aode"];
                     else
                         ret.value="";
                 }
@@ -82,9 +84,10 @@ function maketable(str, arr)
                 }
                 else if(column == "latest-disco" && row[column] != null) 
                     ret.value = ((100*row[column]).toFixed(2))+" cm";
+                else if(column == "time-disco" && row[column] != null) 
+                    ret.value = row[column].toFixed(2)+" ns";
                 else if(column == "health")  {
                     ret.value = row[column];
-                    console.log(row["healthissue"]);
                     if(row["healthissue"] == 1) {
                         ret.color="orange";
                     }
@@ -99,6 +102,10 @@ function maketable(str, arr)
                     ret.color="orange";
                 if(column == "sisa" && parseInt(row[column]) > 312)
                     ret.color="#ffaaaa";
+                var myRe = RegExp('[0-9]* m');
+                if(column == "sisa" && myRe.test(row[column]))
+                    ret.color="#ff2222";
+
                 if(column == "sisa" && row[column]=="NO SIS AVAILABLE")
                     ret.color="#ff2222";                    
                 return ret;

html/index.html

@@ -54,8 +54,18 @@ tr:nth-child(odd) {background: #FFF}
   <p>
     The official Galileo constellation status can be found on the <a href="https://www.gsc-europa.eu/system-status/Constellation-Information">European GNSS Service Centre page</a>, which also lists "NAGUs", <a href="https://www.gsc-europa.eu/system-status/user-notifications">notifications about outages or changes</a>.
   </p>
+  <p>
+    Official GLONASS status can be found on <a href="https://www.glonass-iac.ru/en/GLONASS/">this page</a> from the Russian Information and Analysis Center for Positioning, Navigation and Timing.
+  </p>
+  <p>
+    Sadly reduced status updates on GPS can be found on <a href="https://www.navcen.uscg.gov/?Do=constellationStatus">this page</a> from the US Department of Homeland Security.
+  </p>
+  <p>
+    Information on the status of BeiDou can be found on <a href="http://www.csno-tarc.cn/system/constellation&ce=english">this page</a> from the Chinese Test and Assessment Research Center of China Satellite Navigation Office.
+  </p>
   <p>
     Feedback is very welcome on bert@hubertnet.nl or <a href="https://twitter.com/PowerDNS_Bert">@PowerDNS_Bert</a>.
+  </p>
     <script src="d3.v4.min.js"></script>
     <script src="ext/moment-with-locales.js"></script>
   <script src="doalles.js"></script>

navdump.cc

@@ -1,4 +1,3 @@
-
 #include <string>
 #include <stdio.h>
 #include <iostream>
@@ -22,6 +21,7 @@
 #include "beidou.hh"
 #include "galileo.hh"
 #include "navmon.hh"
+#include "tle.hh"
 
 #include <unistd.h>
 using namespace std;
@@ -62,7 +62,15 @@ string beidouHealth(int in)
 }
 
 int main(int argc, char** argv)
+try
 {
+  TLERepo tles;
+  tles.parseFile("active.txt");
+  /*  tles.parseFile("galileo.txt");
+  tles.parseFile("glo-ops.txt");
+  tles.parseFile("gps-ops.txt");
+  tles.parseFile("beidou.txt");*/
+  
   for(;;) {
     char bert[4];
     int res = readn2(0, bert, 4);
@@ -86,23 +94,26 @@ int main(int argc, char** argv)
     NavMonMessage nmm;
     nmm.ParseFromString(string(buffer, len));
 
-    if(nmm.type() == NavMonMessage::ReceptionDataType)
+    //    if(nmm.type() == NavMonMessage::ReceptionDataType)
-      continue;
+    //      continue;
 
     
     cout<<humanTime(nmm.localutcseconds())<<" "<<nmm.localutcnanoseconds()<<" ";
     cout<<"src "<<nmm.sourceid()<< " ";
     if(nmm.type() == NavMonMessage::ReceptionDataType) {
-      cout<<"receptiondata for "<<nmm.rd().gnssid()<<","<<nmm.rd().gnsssv()<<", db "<<nmm.rd().db()<<" ele "<<nmm.rd().el() <<" azi "<<nmm.rd().azi()<<endl;
+      cout<<"receptiondata for "<<nmm.rd().gnssid()<<","<<nmm.rd().gnsssv()<<", db "<<nmm.rd().db()<<" ele "<<nmm.rd().el() <<" azi "<<nmm.rd().azi()<<" prRes "<<nmm.rd().prres() << endl;
     }
     else if(nmm.type() == NavMonMessage::GalileoInavType) {
       basic_string<uint8_t> inav((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
       static map<int, GalileoMessage> gms;
+      static map<pair<int, int>, GalileoMessage> gmwtypes;
       static map<int, GalileoMessage> oldgm4s;
-      GalileoMessage& gm = gms[nmm.gi().gnsssv()];
+      int sv = nmm.gi().gnsssv();
+      GalileoMessage& gm = gms[sv];
       int wtype = gm.parse(inav);
-
+      gm.tow = nmm.gi().gnsstow();
-      cout << "gal inav for "<<nmm.gi().gnssid()<<","<<nmm.gi().gnsssv()<<" tow "<< nmm.gi().gnsstow()<<" wtype "<< wtype << endl;
+      gmwtypes[{nmm.gi().gnsssv(), wtype}] = gm;
+      cout << "gal inav for "<<nmm.gi().gnssid()<<","<<nmm.gi().gnsssv()<<" tow "<< nmm.gi().gnsstow()<<" wtype "<< wtype<<" ";
       static uint32_t tow;
       if(wtype == 4) {
         //              2^-34       2^-46
@@ -113,7 +124,7 @@ int main(int argc, char** argv)
           auto newOffset = gm.getAtomicOffset(tow);
           cout<<"  Timejump: "<<oldOffset.first - newOffset.first<<" after "<<(gm.getT0c() - oldgm4.getT0c() )<<" seconds";
         }
-        cout<<endl;
+
         oldgm4s[nmm.gi().gnsssv()] = gm;
       }
 
@@ -123,21 +134,38 @@ int main(int argc, char** argv)
       if(wtype < 7)
         gm = GalileoMessage{};
 
-
+      if(wtype >=7 && wtype<=10)
+        cout<<" ioda "<<gm.iodalmanac;
       // af0 af1 scaling in almanac: 2^-19, 2^2^-38 plus "truncated"
       if(wtype == 7) {
-        cout<<"  t0a "<<gm.t0almanac<<", alma sv1 "<<gm.alma1.svid<<", t0a age: "<< ephAge(gm.t0almanac *600, tow) << endl;
+        cout<<"  t0a "<<gm.t0almanac<<", alma sv1 "<<gm.alma1.svid<<", t0a age: "<< ephAge(gm.t0almanac *600, tow) << " ";
+        if(gm.alma1.svid) {
+          Point satpos;
+          getCoordinates(0, gm.tow, gm.alma1, &satpos);
+          cout<< "("<<satpos.x/1000<<", "<<satpos.y/1000<<", "<<satpos.z/1000<<")";
+          
+          auto match = tles.getBestMatch(nmm.localutcseconds(), satpos.x, satpos.y, satpos.z);
+          cout<<" best-tle-match "<<match.name <<" distance "<<match.distance /1000<<" km ";
+          cout <<" tle-e "<<match.e <<" eph-e " <<gm.alma1.getE() <<" tle-ran "<<match.ran;
+          cout<<" norad " <<match.norad <<" int-desig " << match.internat;
+        }
       }
-      else if(wtype == 8 && gm.alma1.svid > 0) {
+      else if(wtype == 8 && gm.tow - gmwtypes[{sv,7}].tow < 5 && gmwtypes[{sv,7}].alma1.svid && gm.iodalmanac == gmwtypes[{sv,7}].iodalmanac) {
-        cout<<"  "<<gm.alma1.svid<<" af0 "<<gm.alma1.af0<<" af1 "<< gm.alma1.af1 <<" e5bhs "<< gm.alma1.e5bhs<<" e1bhs "<< gm.alma1.e1bhs<<endl;
+        cout<<"  "<<gmwtypes[{sv,7}].alma1.svid<<" af0 "<<gm.alma1.af0<<" af1 "<< gm.alma1.af1 <<" e5bhs "<< gm.alma1.e5bhs<<" e1bhs "<< gm.alma1.e1bhs <<" sv9 "<<gm.alma2.svid;
       }
-      else if(wtype == 9 && gm.alma2.svid > 0) {
+      else if(wtype == 9 && gm.tow - gmwtypes[{sv,8}].tow < 30 && gm.iodalmanac == gmwtypes[{sv,8}].iodalmanac) {
-        cout<<"  "<<gm.alma2.svid<<" af0 "<<gm.alma2.af0<<" af1 "<< gm.alma2.af1 <<" e5bhs "<< gm.alma2.e5bhs<<" e1bhs "<< gm.alma2.e1bhs<<endl;
+        if(gmwtypes[{sv,8}].alma2.svid)
+          cout<<"  "<<gmwtypes[{sv,8}].alma2.svid<<" af0 "<<gm.alma2.af0<<" af1 "<< gm.alma2.af1 <<" e5bhs "<< gm.alma2.e5bhs<<" e1bhs "<< gm.alma2.e1bhs;
+        else
+          cout<<"  empty almanac slot";
       }
-      else if(wtype == 10 && gm.alma3.svid > 0){
+      else if(wtype == 10 && gm.tow - gmwtypes[{sv,9}].tow < 5 && gmwtypes[{sv,9}].alma3.svid && gm.iodalmanac == gmwtypes[{sv,9}].iodalmanac) {
-        cout<<"  "<<gm.alma3.svid<<" af0 "<<gm.alma3.af0<<" af1 "<< gm.alma3.af1 <<" e5bhs "<< gm.alma3.e5bhs<<" e1bhs "<< gm.alma3.e1bhs<<endl;
+        if(gm.alma3.e1bhs != 0) {
+          cout <<" gm.tow "<<gm.tow<<" gmwtypes.tow "<< gmwtypes[{sv,9}].tow <<" ";
+        }
+        cout<<"  "<<gmwtypes[{sv,9}].alma3.svid <<" af0 "<<gm.alma3.af0<<" af1 "<< gm.alma3.af1 <<" e5bhs "<< gm.alma3.e5bhs<<" e1bhs "<< gm.alma3.e1bhs <<" a0g " << gm.a0g <<" a1g "<< gm.a1g <<" t0g "<<gm.t0g <<" wn0g "<<gm.wn0g;
       }
-      
+      cout<<endl;      
     }
     else if(nmm.type() == NavMonMessage::GPSInavType) {
       int sv = nmm.gpsi().gnsssv();
@@ -165,9 +193,11 @@ int main(int argc, char** argv)
     else if(nmm.type() == NavMonMessage::BeidouInavTypeD1) {
       int sv = nmm.bid1().gnsssv();
       auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid1().contents().c_str(), nmm.bid1().contents().size()));
-      BeidouMessage bm;
       uint8_t pageno;
+      static map<int, BeidouMessage> bms;
+      auto& bm = bms[sv];
       int fraid = bm.parse(cond, &pageno);
+
       cout<<"BeiDou "<<sv<<": "<<bm.sow<<", FraID "<<fraid;
       if(fraid == 1) {
         static map<int, BeidouMessage> msgs;
@@ -180,6 +210,17 @@ int main(int argc, char** argv)
         cout<<" wn "<<bm.wn<<" t0c "<<(int)bm.t0c<<" aodc "<< (int)bm.aodc <<" aode "<< (int)bm.aode <<" sath1 "<< (int)bm.sath1 << " urai "<<(int)bm.urai << " af0 "<<bm.a0 <<" af1 " <<bm.a1 <<" af2 "<<bm.a2;
         auto offset = bm.getAtomicOffset();
         cout<< ", "<<offset.first<<"ns " << (offset.second * 3600) <<" ns/hour "<< ephAge(bm.sow, bm.t0c*8)<<endl;
+      }
+      else if(fraid == 3 && bm.sow) {
+        Point sat;
+        getCoordinates(0, bm.sow, bm, &sat);
+        TLERepo::Match second;
+        auto match = tles.getBestMatch(nmm.localutcseconds(), sat.x, sat.y, sat.z, &second);
+        cout<<" best-tle-match "<<match.name <<" dist "<<match.distance /1000<<" km";
+        cout<<" norad " <<match.norad <<" int-desig " << match.internat;
+        cout<<" 2nd-match "<<second.name << " dist "<<second.distance/1000<<" km";
+
+
       }
       else if(fraid == 4 && 1<= pageno && pageno <= 24) {
         cout <<" pageno "<< (int) pageno<<" AmEpID "<< getbitu(&cond[0], beidouBitconv(291), 2);
@@ -223,7 +264,7 @@ int main(int argc, char** argv)
       
       int strno = gm.parse(std::basic_string<uint8_t>((uint8_t*)nmm.gloi().contents().c_str(), nmm.gloi().contents().size()));
 
-      cout<<"Glonass R"<<nmm.gloi().gnsssv()<<" @ "<<nmm.gloi().freq() <<" strno "<<strno;
+      cout<<"Glonass R"<<nmm.gloi().gnsssv()<<" @ "<< ((int)nmm.gloi().freq()-7) <<" strno "<<strno;
       if(strno == 1) {
         cout << ", hour "<<(int)gm.hour <<" minute " <<(int)gm.minute <<" seconds "<<(int)gm.seconds;
         // start of period is 1st of January 1996 + (n4-1)*4, 03:00 UTC
@@ -232,19 +273,40 @@ int main(int argc, char** argv)
       }
       if(strno == 2)
         cout<<" Tb "<<(int)gm.Tb <<" Bn "<<(int)gm.Bn;
-      else if(strno == 4)
+      else if(strno == 4) {
         cout<<", taun "<<gm.taun <<" NT "<<gm.NT <<" FT " << (int) gm.FT <<" En " << (int)gm.En;
+        if(gm.x && gm.y && gm.z) {
+          auto longlat = getLongLat(gm.getX(), gm.getY(), gm.getZ());
+          cout<<" long "<< 180* longlat.first/M_PI <<" lat " << 180*longlat.second/M_PI<<" rad "<<gm.getRadius();
+          cout << " Tb "<<(int)gm.Tb <<" H"<<((gm.Tb/4.0) -3) << " UTC ("<<gm.getX()/1000<<", "<<gm.getY()/1000<<", "<<gm.getZ()/1000<<") -> ";
+          cout << "("<<gm.getdX()/1000<<", "<<gm.getdY()/1000<<", "<<gm.getdZ()/1000<<")";
+          time_t now = nmm.localutcseconds();
+          struct tm tm;
+          memset(&tm, 0, sizeof(tm));
+          gmtime_r(&now, &tm);
+          tm.tm_hour = (gm.Tb/4.0) - 3;
+          tm.tm_min = (gm.Tb % 4)*15;
+          tm.tm_sec = 0;
+
+          auto match = tles.getBestMatch(timegm(&tm), gm.getX(), gm.getY(), gm.getZ());
+          cout<<" best-tle-match "<<match.name <<" distance "<<match.distance /1000<<" km";
+          cout<<" norad " <<match.norad <<" int-desig " << match.internat;
+        }
+      }
       else if(strno == 5)
         cout<<", n4 "<< (int)gm.n4 << " l_n " << gm.l_n;
       else if(strno == 7 || strno == 9 || strno == 11 || strno ==13 ||strno ==15) {
         cout << " l_n "<< gm.l_n;
       }
-      else if(strno == 6 || strno ==8 || strno == 10 || strno ==12 ||strno ==14)
+      else if(strno == 6 || strno ==8 || strno == 10 || strno ==12 ||strno ==14) {
-        cout<<" nA "<< gm.nA <<" CnA "<<gm.CnA;
+        cout<<" nA "<< gm.nA <<" CnA " << gm.CnA <<" LambdaNaDeg "<< gm.getLambdaNaDeg();
+      }
       cout<<endl;
     }
     else if(nmm.type() == NavMonMessage::ObserverPositionType) {
-      cout<<"ECEF"<<endl;
+      auto lonlat = getLongLat(nmm.op().x(), nmm.op().y(), nmm.op().z());
+      cout<<"ECEF "<<nmm.op().x()<<", "<<nmm.op().y()<<", "<<nmm.op().z()<< " lon "<< 180*lonlat.first/M_PI << " lat "<<
+        180*lonlat.second/M_PI<<endl;
     }
     else if(nmm.type() == NavMonMessage::RFDataType) {
       cout<<"RFdata for "<<nmm.rfd().gnssid()<<","<<nmm.rfd().gnsssv()<<endl;
@@ -254,4 +316,5 @@ int main(int argc, char** argv)
     }
   }
 }
-
+ catch(EofException& ee)
+   {}

navparse.cc

@@ -22,19 +22,22 @@
 #include "glonass.hh"
 #include "beidou.hh"
 #include "galileo.hh"
+#include "tle.hh"
 #include <optional>
+#include <Tle.h>
 using namespace std;
 struct EofException{};
 
 Point g_ourpos(3922.505 * 1000,  290.116 * 1000, 5004.189 * 1000);
 
+TLERepo g_tles;
 struct GNSSReceiver
 {
   Point position; 
 };
 
 
-int g_dtLS{18};
+int g_dtLS{18}, g_dtLSBeidou{4};
 uint64_t utcFromGST(int wn, int tow)
 {
   return (935280000 + wn * 7*86400 + tow - g_dtLS); 
@@ -50,7 +53,6 @@ double utcFromGPS(int wn, double tow)
   return (315964800 + wn * 7*86400 + tow - g_dtLS); 
 }
 
-
 string humanFt(uint8_t ft)
 {
   static const char* ret[]={"100 cm", "200 cm", "250 cm", "400 cm", "500 cm", "7 m", "10 m", "12 m", "14 m", "16 m", "32 m", "64 m", "128 m", "256 m", "512 m", "NONE"};
@@ -223,7 +225,8 @@ struct SVStat
   int t0eMSB{-1}, t0eLSB{-1}, aode{-1}, aodc{-1};
   BeidouMessage beidouMessage, oldBeidouMessage;
   BeidouMessage lastBeidouMessage1, lastBeidouMessage2;
-
+  TLERepo::Match tleMatch;
+  
   // new galileo
   GalileoMessage galmsg;
 
@@ -394,8 +397,10 @@ uint64_t nanoTime(int gnssid, int wn, int tow)
     offset = 315964800;
   if(gnssid == 2) // Galileo, 22-08-1999
     offset = 935280000;
-  if(gnssid == 3) // Beidou, 01-01-2006
+  if(gnssid == 3) {// Beidou, 01-01-2006 - I think leap seconds count differently in Beidou!! XXX
     offset = 1136073600;
+    return 1000000000ULL*(offset + wn * 7*86400 + tow - g_dtLSBeidou); 
+  }
   if(gnssid == 6) { // GLONASS
     offset = 820368000;
     return 1000000000ULL*(offset + wn * 7*86400 + tow);  // no leap seconds in glonass
@@ -543,9 +548,16 @@ double getElevation(const Point& p)
 }
 
 
+
+
 int main(int argc, char** argv)
 try
 {
+//  g_tles.parseFile("active.txt");
+  g_tles.parseFile("galileo.txt");
+  g_tles.parseFile("glo-ops.txt");
+  g_tles.parseFile("gps-ops.txt");
+  g_tles.parseFile("beidou.txt");
   signal(SIGPIPE, SIG_IGN);
   InfluxPusher idb(argc > 3 ? argv[3] : "galileo");
   MiniCurl::init();
@@ -649,13 +661,26 @@ try
             if(hzCorrection)
               item["delta_hz_corr"] = s.second.deltaHz.second - (1561.098/1575.42)* (*hzCorrection);
           }
+          if(s.second.tleMatch.distance >=0) {
+            item["best-tle"] = s.second.tleMatch.name;
+            item["best-tle-dist"] = s.second.tleMatch.distance /1000.0;
+            item["best-tle-norad"] = s.second.tleMatch.norad;
+            item["best-tle-int-desig"] = s.second.tleMatch.internat;
 
+          }
         }
         else if(s.first.first == 6) { // glonass
           if(s.second.glonassMessage.FT < 16)
             item["sisa"] = humanFt(s.second.glonassMessage.FT);
           item["aode"] = s.second.aode;
           item["iod"] = s.second.glonassMessage.Tb;
+          if(s.second.tleMatch.distance >=0) {
+            item["best-tle"] = s.second.tleMatch.name;
+            item["best-tle-dist"] = s.second.tleMatch.distance /1000.0;
+            item["best-tle-norad"] = s.second.tleMatch.norad;
+            item["best-tle-int-desig"] = s.second.tleMatch.internat;
+          }
+
         }
         if(s.second.completeIOD()) {
           item["iod"]=s.second.getIOD();
@@ -677,6 +702,15 @@ try
           
           // this should actually use local time!
           getCoordinates(0, s.second.tow, s.second.liveIOD(), &p);
+          auto match = g_tles.getBestMatch(
+                                           s.first.first ?
+                                           utcFromGST((int)s.second.wn, (int)s.second.tow) : utcFromGPS(s.second.wn, s.second.tow),
+                                           p.x, p.y, p.z);
+
+          item["best-tle"] = match.name;
+          item["best-tle-norad"] = match.norad;
+          item["best-tle-int-desig"] = match.internat;
+          item["best-tle-dist"] = match.distance/1000.0;
           
           Vector core2us(core, our);
           Vector dx(our, p); //  = x-ourx, dy = y-oury, dz = z-ourz;
@@ -991,11 +1025,11 @@ try
       pair<int,int> id{nmm.rfd().gnssid(), nmm.rfd().gnsssv()};
       if(id.first == 3 && g_svstats[id].oldBeidouMessage.sow >= 0 && g_svstats[id].oldBeidouMessage.sqrtA != 0) {
         auto res = doDoppler(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_ourpos, g_svstats[id].oldBeidouMessage, 1561.098 * 1000000);
+        Point p;
+        getCoordinates(0, nmm.rfd().rcvtow(), g_svstats[id].oldBeidouMessage, &p);
 
         if(isnan(res.preddop)) {
           cerr<<"Problem with doppler calculation for C"<<id.second<<": "<<endl;
-          Point p;
-          getCoordinates(0, nmm.rfd().rcvtow(), g_svstats[id].oldBeidouMessage, &p, false);
           exit(1);
         }
         
@@ -1123,20 +1157,27 @@ try
       if(fraid == 3) {
         svstat.t0eLSB = bm.t0eLSB;
         Point oldpoint, newpoint;
-        if(bm.sow - svstat.lastBeidouMessage2.sow == 6 && svstat.oldBeidouMessage.sow >= 0 && svstat.oldBeidouMessage.getT0e() != svstat.beidouMessage.getT0e()) {
+        if(bm.sow - svstat.lastBeidouMessage2.sow == 6 && svstat.oldBeidouMessage.sow >= 0) {
-          getCoordinates(svstat.wn, svstat.tow, svstat.oldBeidouMessage, &oldpoint);
           getCoordinates(svstat.wn, svstat.tow, bm, &newpoint);
-          Vector jump(oldpoint ,newpoint);
+
-          cout<<fmt::sprintf("Discontinuity C%02d (%f,%f,%f) -> (%f, %f, %f), jump: %f, seconds: %f\n",
+          auto match = g_tles.getBestMatch(nanoTime(3, svstat.wn, svstat.tow)/1000000000.0, newpoint.x, newpoint.y, newpoint.z);
-                             id.second, oldpoint.x, oldpoint.y, oldpoint.z,
+          svstat.tleMatch = match;
-                             newpoint.x, newpoint.y, newpoint.z, jump.length(), (double)bm.getT0e() - svstat.oldBeidouMessage.getT0e());
+          
-          double hours = (bm.getT0e() - svstat.oldBeidouMessage.getT0e())/3600;
+          if(svstat.oldBeidouMessage.getT0e() != svstat.beidouMessage.getT0e()) {
-          if(hours < 4) {
+            getCoordinates(svstat.wn, svstat.tow, svstat.oldBeidouMessage, &oldpoint);
-            svstat.latestDisco = jump.length();
+            Vector jump(oldpoint ,newpoint);
-            idb.addValue(id, "eph-disco", jump.length());
+            cout<<fmt::sprintf("Discontinuity C%02d (%f,%f,%f) -> (%f, %f, %f), jump: %f, seconds: %f\n",
+                               id.second, oldpoint.x, oldpoint.y, oldpoint.z,
+                               newpoint.x, newpoint.y, newpoint.z, jump.length(), (double)bm.getT0e() - svstat.oldBeidouMessage.getT0e());
+            double hours = (bm.getT0e() - svstat.oldBeidouMessage.getT0e())/3600;
+            if(hours < 4) {
+              svstat.latestDisco = jump.length();
+              idb.addValue(id, "eph-disco", jump.length());
+            }
+            else
+              svstat.latestDisco = -1;
           }
-          else
+          
-            svstat.latestDisco = -1;
         }
         if(bm.sqrtA) // only copy in if complete
           svstat.oldBeidouMessage = bm;
@@ -1148,7 +1189,9 @@ try
 
       if(fraid==5 && pageno == 10) {
         svstat.a0 = bm.a0utc;
-        svstat.a1 = bm.a1utc;        
+        svstat.a1 = bm.a1utc;
+        g_dtLSBeidou = bm.deltaTLS;
+        //        cout<<"Beidou leap seconds: "<<g_dtLSBeidou<<endl;
       }
       Point core, sat;
 
@@ -1158,15 +1201,17 @@ try
     }
     else if(nmm.type()== NavMonMessage::BeidouInavTypeD2) {
       auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid2().contents().c_str(), nmm.bid2().contents().size()));
+      /*
       int fraid = getbitu(&cond[0], beidouBitconv(16), 3);
       int sow = getbitu(&cond[0], beidouBitconv(19), 20);
       int pnum = getbitu(&cond[0], beidouBitconv(43), 4);
       int pre = getbitu(&cond[0], beidouBitconv(1), 11);
-      (void) pre;
+
       //      cout<<"C"<< nmm.bid2().gnsssv() << " sent D2 message, pre "<<pre<<" sow "<<sow<<", FraID "<<fraid;
       //      if(fraid == 1)
       //        cout <<" pnum "<<pnum;
       //      cout<<endl;
+      */
     }
     else if(nmm.type()== NavMonMessage::GlonassInavType) {
       pair<int,int> id{nmm.gloi().gnssid(), nmm.gloi().gnsssv()};
@@ -1195,8 +1240,21 @@ try
             idb.addValue(id, "clock_jump_ns", svstat.timeDisco);
           }
         }
+
+        if(gm.x && gm.y && gm.z) {
+          time_t now = nmm.localutcseconds();
+          struct tm tm;
+          memset(&tm, 0, sizeof(tm));
+          gmtime_r(&now, &tm);
+          tm.tm_hour = (gm.Tb/4.0) - 3;
+          tm.tm_min = (gm.Tb % 4)*15;
+          tm.tm_sec = 0;
+
+          auto match = g_tles.getBestMatch(timegm(&tm), gm.getX(), gm.getY(), gm.getZ());
+          svstat.tleMatch = match;
+        }
       }
-      cout<<"GLONASS R"<<id.second<<" str "<<strno<<endl;
+      //      cout<<"GLONASS R"<<id.second<<" str "<<strno<<endl;
     }
     else {
       cout<<"Unknown type "<< (int)nmm.type()<<endl;

tle.cc

@@ -0,0 +1,105 @@
+#include "tle.hh"
+#include "SGP4.h"
+#include <iostream>
+#include <fstream>
+
+using namespace std;
+
+static void trim(std::string& str)
+{
+  auto pos = str.find_first_of("\r\n");
+  if(pos != string::npos)
+    str.resize(pos);
+  pos = str.find_last_not_of(" \t");
+  if(pos != string::npos)
+    str.resize(pos+1);
+}
+
+void TLERepo::parseFile(std::string_view fname)
+{
+  ifstream ifs(&fname[0]);
+  string name, line1, line2;
+  for(;;) {
+    if(!getline(ifs, name) || !getline(ifs, line1) || !getline(ifs, line2))
+      break;
+    trim(name);
+    trim(line1);
+    trim(line2);
+    Tle tle(line1, line2);
+    
+    cout<<"name: "<<name<<endl;
+    cout<<"line1: "<<line1<<endl;
+    cout<<"line2: "<<line2<<endl;
+    cout << tle.ToString() << endl;
+
+    auto sgp4 = std::make_unique<std::tuple<SGP4, Tle>>(SGP4(tle), tle);
+    d_sgp4s[name] = std::move(sgp4);
+  }
+}
+
+TLERepo::TLERepo()
+{}
+
+TLERepo::~TLERepo()
+{}
+
+
+static TLERepo::Match makeMatch(const DateTime& d, const SGP4& sgp4, const Tle& tle, string name, double distance)
+{
+  TLERepo::Match m;
+  m.name=name;
+  m.norad = tle.NoradNumber();
+  m.internat = tle.IntDesignator();
+  m.inclination = tle.Inclination(false);
+  m.ran = tle.RightAscendingNode(false);
+  m.e = tle.Eccentricity();
+  m.distance = distance;
+
+  auto eci = sgp4.FindPosition(d);
+
+  double theta = -eci.GetDateTime().ToGreenwichSiderealTime();
+  Vector rot = eci.Position();
+  m.eciX = rot.x;
+  m.eciY = rot.y;
+  m.eciZ = rot.z;
+  
+  rot.x = eci.Position().x * cos(theta) - eci.Position().y * sin(theta);
+  rot.y = eci.Position().x * sin(theta) + eci.Position().y * cos(theta);
+
+  m.ecefX = 1000.0 * rot.x;
+  m.ecefY = 1000.0 * rot.y;
+  m.ecefZ = 1000.0 * rot.z;
+  
+  return m;
+}
+
+TLERepo::Match TLERepo::getBestMatch(time_t now, double x, double y, double z, TLERepo::Match* secondbest)
+{
+  struct tm tm;
+  gmtime_r(&now, &tm);
+  DateTime d(1900 + tm.tm_year, tm.tm_mon+1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec);
+  
+  Vector sat(x/1000.0, y/1000.0, z/1000.0);
+
+  multimap<double, string> distances;
+  for(const auto& sgp4 : d_sgp4s) {
+    auto eci = get<0>(*sgp4.second).FindPosition(d);
+
+    double theta = -eci.GetDateTime().ToGreenwichSiderealTime();
+    Vector rot = eci.Position();
+    rot.x = eci.Position().x * cos(theta) - eci.Position().y * sin(theta);
+    rot.y = eci.Position().x * sin(theta) + eci.Position().y * cos(theta);
+    
+    distances.insert({1000.0*(rot - sat).Magnitude(),sgp4.first});
+  }
+  if(secondbest) {
+    auto iter = distances.begin();
+    if(iter != distances.end()) {
+      ++iter;
+      *secondbest = makeMatch(d, get<0>(*d_sgp4s[iter->second]), get<1>(*d_sgp4s[iter->second]), iter->second, iter->first);
+    }
+  }
+    
+  return makeMatch(d, get<0>(*d_sgp4s[distances.begin()->second]), get<1>(*d_sgp4s[distances.begin()->second]), distances.begin()->second, distances.begin()->first);
+
+}

tle.hh

@@ -0,0 +1,37 @@
+#pragma once
+#include <string>
+#include <map>
+#include <memory>
+class SGP4;
+class Tle;
+
+class TLERepo
+{
+public:
+  TLERepo();
+  ~TLERepo();
+  void parseFile(std::string_view fname);
+  struct Match
+  {
+    std::string name;
+    int norad;
+    std::string internat;
+    double inclination; // radians
+    double ran;         // radians
+    double e;       
+    double ecefX;  // m
+    double ecefY;  // m
+    double ecefZ;  // m
+
+    double eciX, eciY, eciZ; // m
+    double distance{-1}; // m
+  };
+  
+  Match getBestMatch(time_t, double x, double y, double z, Match* secondbest=0);
+
+  
+private:
+  std::map<std::string,
+           std::unique_ptr<std::tuple<SGP4, Tle>>
+          > d_sgp4s;
+};

ubxtool.cc

@@ -759,9 +759,11 @@ int main(int argc, char** argv)
           else if(id.first==6) {
             //            cerr<<"SFRBX from GLONASS "<<id.second<<" @ frequency "<<(int)payload[3]<<", msg of "<<(int)payload[4]<< " words"<<endl;
             auto gstr = getGlonassFromSFRBXMsg(payload);
+            /*
             static map<int, GlonassMessage> gms;
             GlonassMessage& gm = gms[id.second];
             int strno = gm.parse(gstr);
+            */
             if(id.second != 255) {
               NavMonMessage nmm;
               nmm.set_localutcseconds(g_gstutc.tv_sec);

update-tles

@@ -0,0 +1,5 @@
+#!/bin/sh
+for a in galileo gps-ops beidou glo-ops active
+do
+	wget --backups=1 https://www.celestrak.com/NORAD/elements/$a.txt
+done