teach protobuf about GPS CNAV

berthubert · Feb 11, 2020 · 923c54f · 923c54f
1 parent 2973b66
commit 923c54f
Show file tree

Hide file tree

Showing 3 changed files with 260 additions and 0 deletions.
diff --git a/gpscnav.cc b/gpscnav.cc
@@ -0,0 +1,20 @@
+/*
+            cerr<<"Preamble "<<getbitu(&cnav[0], 0, 8)<<" sv "<<
+              getbitu(&cnav[0], 8, 6) << " type "<< type <<
+              " tow " << tow;
+
+            if(type == 10) {
+              wn = getbitu(&cnav[0], 38, 13);
+              cerr<<" t0p " << 300*getbitu(&cnav[0], 54,11)<< " t0e "<< 300*getbitu(&cnav[0], 70, 11);
+            }
+            else if(type == 11) {
+              cerr<<" t0e "<< 300* getbitu(&cnav[0], 38, 11);
+
+                            
+            }
+            else if(type == 32) {
+              cerr<< " delta-ut1 " << 1000.0*ldexp(getbits(&cnav[0], 215, 31), -24)<< "ms rate " << 1000.0*ldexp(getbits(&cnav[0], 246, 19), -25) << " te0p " <<
+                16*getbits(&cnav[0], 127, 16);
+                
+            }
+*/
diff --git a/gpscnav.hh b/gpscnav.hh
@@ -0,0 +1,227 @@
+#pragma once
+#include <bitset>
+#include <string>
+#include <map>
+#include "bits.hh"
+#include <iostream>
+#include <math.h>
+
+struct GPSCNavAlmanac
+{
+  int dataid{-1};
+  int sv;
+  uint32_t t0a{0}; 
+  uint32_t e{0}, sqrtA{0};
+  int32_t M0, Omega0, deltai, omega, omegadot;
+  int health;
+  int af0, af1;
+
+  double getMu() const
+  {
+    return 3.986005    * pow(10.0, 14.0);
+  } // m^3/s^2
+    // same for galileo & gps
+  double getOmegaE()    const { return 7.2921151467 * pow(10.0, -5.0);} // rad/s
+
+
+  double getE() const
+  {
+    return ldexp(e, -21);
+  }
+  double getT0e() const
+  {
+    return ldexp(t0a, 12);
+  }
+
+  double getI0() const
+  {
+    return M_PI*0.3 + ldexp(M_PI*deltai, -19);
+  }
+
+  double getOmegadot() const
+  {
+    return ldexp(M_PI * omegadot, -38);
+  }
+
+  double getSqrtA() const
+  {
+    return ldexp(sqrtA, -11);
+  }
+
+  double getOmega0() const
+  {
+    return ldexp(M_PI * Omega0, -23);
+  }
+  double getOmega() const
+  {
+    return ldexp(M_PI * omega, -23);
+  }
+
+  double getM0() const
+  {
+    return ldexp(M_PI * M0, -23);
+  }
+  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
+
+
+};
+
+
+struct GPSCNavState
+{
+    uint32_t t0e; 
+    uint32_t e, sqrtA;
+    int32_t m0, omega0, i0, omega, idot, omegadot, deltan;
+
+
+    int16_t cuc{0}, cus{0}, crc{0}, crs{0}, cic{0}, cis{0};
+    //        16 seconds
+    uint16_t t0c; 
+
+    //      2^-31     2^-43
+    int32_t af0   ,   af1;
+    //     ???
+    int8_t af2;
+    uint32_t wn{0}, tow{0};
+
+  double getMu() const
+  {
+    return 3.986005    * pow(10.0, 14.0);
+  } // m^3/s^2
+  // same for galileo & gps
+  double getOmegaE()    const { return 7.2921151467 * pow(10.0, -5.0);} // rad/s
+
+  uint32_t getT0e() const { return t0e; }
+  double getSqrtA() const { return ldexp(sqrtA,     -19);   }
+  double getE()     const { return ldexp(e,         -33);   }
+  double getCuc()   const { return ldexp(cuc,       -29);   } // radians
+  double getCus()   const { return ldexp(cus,       -29);   } // radians
+  double getCrc()   const { return ldexp(crc,        -5);   } // meters
+  double getCrs()   const { return ldexp(crs,        -5);   } // meters
+  double getM0()    const { return ldexp(m0 * M_PI, -31);   } // radians
+  double getDeltan()const { return ldexp(deltan *M_PI, -43); } //radians/s
+  double getI0()        const { return ldexp(i0 * M_PI,       -31);   } // radians
+  double getCic()       const { return ldexp(cic,             -29);   } // radians
+  double getCis()       const { return ldexp(cis,             -29);   } // radians
+  double getOmegadot()  const { return ldexp(omegadot * M_PI, -43);   } // radians/s
+  double getOmega0()    const { return ldexp(omega0 * M_PI,   -31);   } // radians
+  double getIdot()      const { return ldexp(idot * M_PI,     -43);   } // radians/s
+  double getOmega()     const { return ldexp(omega * M_PI,    -31);   } // radians
+
+
+  uint8_t gpshealth{0};
+  uint16_t ai0{0};
+  int16_t ai1{0}, ai2{0};
+
+  int BGDE1E5a{0}, BGDE1E5b{0};
+
+  bool disturb1{false}, disturb2{false}, disturb3{false}, disturb4{false}, disturb5{false};
+  //                     
+  //      2^-30   2^-50  1      8-bit week
+  int32_t a0{0}, a1{0}, t0t{0}, wn0t{0};  
+  int32_t a0g{0}, a1g{0}, t0g{0}, wn0g{0};
+  int8_t dtLS{0}, dtLSF{0};
+  uint16_t wnLSF{0};
+  uint8_t dn; // leap second day number
+
+
+  int gpsiod{-1};
+
+  int teop;
+  int deltaUT1;
+  int deltaUT1dot;
+
+  // milliseconds
+  std::pair<double, double> getUT1OffsetMS(int tow)
+  {
+    int delta = ephAge(tow, 16* teop);
+    std::cout<<" tow "<<tow<<" teop "<< 16*teop <<" delta "<<delta<<" " << 1000.0*ldexp(deltaUT1, -24)<<"ms ";
+    double cur = ldexp(deltaUT1 + ldexp(delta * deltaUT1dot / 86400.0, -1), -24);
+    double trend = ldexp(deltaUT1dot, -25);
+    return {1000.0* cur, 1000.0*trend};
+  }
+
+};
+
+template<typename T>
+std::pair<double, double> getGPSCNavAtomicOffset(int tow, const T& eph)
+{
+  int delta = ephAge(tow, getT0c(eph));
+  double cur = eph.af0  + ldexp(delta*eph.af1, -12) + ldexp(delta*delta*eph.af2, -24);
+  double trend = ldexp(eph.af1, -12) + ldexp(2*delta*eph.af2, -24);
+
+  // now in units of 2^-31 seconds, which are 0.5 nanoseconds each
+  double factor = ldexp(1000000000, -31);
+  return {factor * cur, factor * trend};
+}
+
+template<typename T>
+std::pair<double, double> getGPSCNavUTCOffset(int tow, int wn, const T& eph) 
+{
+  // 2^-30  2^-50   3600
+  // a0     a1      t0t 
+
+  int dw = (int)(uint8_t)wn - (int)(uint8_t) eph.wn0t;
+  int delta = dw*7*86400  + tow - eph.t0t; // this is pre-scaled for GPS..
+
+  double cur = eph.a0  + ldexp(1.0*delta*eph.a1, -20);
+  double trend = ldexp(eph.a1, -20);
+
+  // now in units of 2^-30 seconds, which are ~1.1 nanoseconds each
+
+  double factor = ldexp(1000000000, -30);
+  return {factor * cur, factor * trend};
+}
+
+
+
+
+
+template<typename T>
+int parseGPSCNavMessage(std::basic_string_view<uint8_t> msg, T& out)
+{
+  using namespace std;
+  int type = getbitu(&msg[0], 14, 6);
+  out.tow = 6 * getbitu(&msg[0], 20, 17) - 12;
+
+  if(type == 10) {
+    out.wn = getbitu(&msg[0], 38, 13);
+    //    out.ura = getbitu(&msg[0], 65, 5);
+    out.t0e = getbitu(&msg[0], 70, 11); // XXX scale?
+
+    //    int32_t deltaSqrtA = getbits(&msg[0], 81, 26);
+    //    int32_t SqrtADot = getbits(&msg[0], 107, 25);
+    out.deltan = getbitu(&msg[0], 132, 17); // XXX scale??
+    //    int deltandot = getbitu(&msg[0], 149, 23); // XXX scale??
+    //    int64_t M0n = getbitu(&msg[0], 172, 33); // XXX scale??
+
+    out.e = getbitu(&msg[0], 205, 33); // XXX scale??
+    out.omega = getbitu(&msg[0], 238, 33); // XXX scale?
+  }
+  else if(type == 11) {
+    out.t0e = getbitu(&msg[0], 38, 11); // XXX scale?
+    out.omega0 = getbitu(&msg[0], 49, 33); // XXX scale?
+    out.i0 = getbitu(&msg[0], 82, 33); // XXX scale?
+    //    int64_t deltaOmegaDot = getbitu(&msg[0], 115, 17); // XXX scale?
+    out.idot = getbitu(&msg[0], 132, 15); // XXX scale?
+    out.cis = getbits(&msg[0], 147, 16);// XXX scale?
+    out.cic = getbits(&msg[0], 163, 16);// XXX scale?
+    out.crs = getbits(&msg[0], 179, 24);// XXX scale?
+    out.crc = getbits(&msg[0], 203, 24);// XXX scale?
+    out.cus = getbits(&msg[0], 227, 21);// XXX scale?
+    out.cuc = getbits(&msg[0], 248, 21); // XXX scale?
+  }
+  else if(type == 32) {
+    out.deltaUT1 = getbits(&msg[0], 215, 31);
+    out.deltaUT1dot = getbits(&msg[0], 246, 19);
+    out.teop = getbitu(&msg[0], 127, 16);
+  }
+  return type;
+}
diff --git a/navmon.proto b/navmon.proto
@@ -15,6 +15,7 @@ message NavMonMessage {
     ObserverDetailsType = 11;
     UbloxJammingStatsType = 12;
     SBASMessageType = 13;
+    GPSCnavType = 14;
   }
 
   required uint64 sourceID = 1;
@@ -160,6 +161,17 @@ message NavMonMessage {
   }
 
 
+  message GPSCnav {
+	required uint32 gnssWN =1;
+  	required uint32 gnssTOW =2; // INTEGERS!
+
+	required uint32 gnssID =3;
+	required uint32 gnssSV =4;
+	required bytes contents =5;
+        required uint32 sigid = 6;
+  }
+
+
   optional GalileoInav gi=5;
   optional ReceptionData rd=6;  
   optional RFData rfd=7;
@@ -173,4 +185,5 @@ message NavMonMessage {
   optional ObserverDetails od = 15;
   optional UbloxJammingStats ujs = 16;
   optional SBASMessage sbm = 17;
+  optional GPSCnav gpsc=18; 
 }