From 31360724bd8e1f5f776b5bebae89b176b89c103c Mon Sep 17 00:00:00 2001
From: bert hubert <bert.hubert@netherlabs.nl>
Date: Thu, 9 Jul 2020 17:18:35 +0200
Subject: [PATCH] first part of the timegal timegps etc stuff

---
 navdump.cc   |   7 +++
 navmon.proto |  26 +++++++++-
 navparse.cc  |  41 +++++++++++++++-
 ubxtool.cc   | 134 ++++++++++++++++++++++++++++++++++++++++++++++++---
 4 files changed, 197 insertions(+), 11 deletions(-)

diff --git a/navdump.cc b/navdump.cc
index 9f8fa4b..b456746 100644
--- a/navdump.cc
+++ b/navdump.cc
@@ -1171,6 +1171,13 @@ try
       cout<<" SAR RLM type "<< nmm.sr().type() <<" from gal sv ";
       cout<< nmm.sr().gnsssv() << " beacon "<<hexstring <<" code "<<(int)nmm.sr().code()<<" params "<< makeHexDump(nmm.sr().params()) <<endl;
     }
+    else if(nmm.type() == NavMonMessage::TimeOffsetType) {
+      cout<<" got a time-offset message with "<< nmm.to().offsets().size()<<" offsets"<<endl;
+      for(const auto& o : nmm.to().offsets()) {
+        cout << o.gnssid()<<" " << o.offsetns() << " ";
+      }
+            
+    }
     else {
       etstamp();
       cout<<"Unknown type "<< (int)nmm.type()<<endl;
diff --git a/navmon.proto b/navmon.proto
index 0f0586e..93f4fca 100644
--- a/navmon.proto
+++ b/navmon.proto
@@ -17,6 +17,7 @@ message NavMonMessage {
     SBASMessageType = 13;
     GPSCnavType = 14;
     RTCMMessageType = 15;
+    TimeOffsetType = 16;
   }
 
   required uint64 sourceID = 1;
@@ -172,10 +173,32 @@ message NavMonMessage {
         required uint32 sigid = 6;
   }
 
+
   message RTCMMessage {
 	required bytes contents =5;
   }
 
+  message GNSSOffset
+  {
+        required uint32 gnssid = 1;
+
+        required int32 offsetNS = 2;
+        required uint32 tAcc = 3;
+        required bool valid = 4;
+        optional int32 leapS = 5;
+        required uint32 tow = 6;
+        optional uint32 wn = 7;
+        optional uint32 nT = 8;
+        optional uint32 n4 = 9;
+
+  }
+  
+
+  message TimeOffsetMessage
+  {
+       required uint32 itow = 1;
+       repeated GNSSOffset offsets = 2;
+  }
 
 
   optional GalileoInav gi=5;
@@ -192,5 +215,6 @@ message NavMonMessage {
   optional UbloxJammingStats ujs = 16;
   optional SBASMessage sbm = 17;
   optional GPSCnav gpsc = 18;
-  optional RTCMMessage rm = 19;   
+  optional RTCMMessage rm = 19;
+  optional TimeOffsetMessage to = 20;
 }
diff --git a/navparse.cc b/navparse.cc
index b8551fc..fc304df 100644
--- a/navparse.cc
+++ b/navparse.cc
@@ -2797,14 +2797,51 @@ try
       for(int n = 0; n < 15; ++n)                                   
         hexstring+=fmt::sprintf("%x", (int)getbitu((unsigned char*)hexid.c_str(), 4 + 4*n, 4));
 
-
-      
       idb.addValue(id, "galsar", {{"mtype", (int) nmm.sr().type()},
             {"midentifier", hexstring},
               {"mcode", nmm.sr().code()},
                 {"mparams", makeHexDump(nmm.sr().params())}}, satUTCTime(id), nmm.sourceid());
 
     }
+    else if(nmm.type() == NavMonMessage::TimeOffsetType) {
+      struct gnsstimeoffset
+      {
+        double offset;
+        double accuracy;
+      } gps, gal, glo, bds;
+      for(const auto& o : nmm.to().offsets()) {
+        if(o.gnssid() == 0) {
+          gps.offset = o.offsetns();
+          gps.accuracy = o.tacc();
+        } else if(o.gnssid() == 2) {
+          gal.offset = o.offsetns();
+          gal.accuracy = o.tacc();
+        } else if(o.gnssid() == 3) {
+          bds.offset = o.offsetns();
+          bds.accuracy = o.tacc();
+        } else if(o.gnssid() == 6) {
+          glo.offset = o.offsetns();
+          glo.accuracy = o.tacc();
+        }
+      }
+      idb.addValueObserver(nmm.sourceid(), "timeoffset",
+      {{"itow", nmm.to().itow()},
+                           {"gps-offset", gps.offset},
+                           {"gal-offset", gal.offset},
+                           {"glo-offset", glo.offset},
+                           {"bds-offset", bds.offset},
+                           {"gps-tacc", gps.accuracy},
+                           {"gal-tacc", gal.accuracy},
+                           {"glo-tacc", glo.accuracy},
+                             {"bds-tacc", bds.accuracy},
+                               {"gal-gps-offset", gal.offset - gps.offset},
+                                 {"gal-bds-offset", gal.offset - bds.offset},
+                                   {"gal-glo-offset", gal.offset - glo.offset},
+                                     {"gps-bds-offset", gps.offset - bds.offset},
+                                       {"gps-glo-offset", gps.offset - glo.offset},
+                                         {"bds-glo-offset", bds.offset - glo.offset}},
+                           nmm.localutcseconds());
+    }
     else {
       cout<<"Unknown type "<< (int)nmm.type()<<endl;
     }
diff --git a/ubxtool.cc b/ubxtool.cc
index bea8abb..77476d2 100644
--- a/ubxtool.cc
+++ b/ubxtool.cc
@@ -446,6 +446,53 @@ static string format_serial(basic_string<uint8_t> payload)
                             payload[8]);
 }
 
+// these are four structs to capture Ublox F9P time offset stats
+namespace {
+struct TIMEGAL
+{
+  uint32_t itow;
+  uint32_t galTow;
+  int32_t fGalTow;
+  int16_t galWno;
+  int8_t leapS;
+  uint8_t valid;
+  uint32_t tAcc;          
+} __attribute__((packed));
+
+struct TIMEBDS
+{
+  uint32_t itow;
+  uint32_t sow;
+  int32_t fSow;
+  int16_t week;
+  int8_t leapS;
+  uint8_t valid;
+  uint32_t tAcc;          
+} __attribute__((packed));
+
+struct TIMEGLO
+{
+  uint32_t itow;
+  uint32_t tod;
+  int32_t fTod;
+  uint16_t nT;
+  uint8_t n4;
+  uint8_t valid;
+  uint32_t tAcc;          
+} __attribute__((packed));
+
+struct TIMEGPS
+{
+  uint32_t itow;
+  int32_t ftow;
+  int16_t week;
+  int8_t leapS;
+  uint8_t valid;
+  uint32_t tAcc;          
+} __attribute__((packed));
+
+}
+
 // ubxtool device srcid
 int main(int argc, char** argv)
 {
@@ -672,9 +719,9 @@ int main(int argc, char** argv)
         }
       }
       else { // UBX-CFG-VALSET
-
+        //                                 vers  ram   res   res    
         msg = buildUbxMessage(0x06, 0x8a, {0x00, 0x01, 0x00, 0x00,
-              0x1f,0x00,0x31,0x10, doGPS,
+              0x1f,0x00,0x31,0x10, doGPS,   // 
               0x01,0x00,0x31,0x10, doGPS,
               0x03,0x00,0x31,0x10, doGPS,
 
@@ -899,7 +946,20 @@ int main(int argc, char** argv)
       if (doDEBUG) { cerr<<humanTimeNow()<<" Enabling UBX-NAV-CLOCK"<<endl; } // clock details
       enableUBXMessageOnPort(fd, 0x01, 0x22, ubxport, 16); // UBX-NAV-CLOCK
 
-      
+      if(version9) {
+        if (doDEBUG) { cerr<<humanTimeNow()<<" Enabling UBX-NAV-CLOCK"<<endl; } // GPS time solution
+        enableUBXMessageOnPort(fd, 0x01, 0x20, ubxport, 16); // UBX-NAV-TIMEGPS
+        
+        if (doDEBUG) { cerr<<humanTimeNow()<<" Enabling UBX-NAV-CLOCK"<<endl; } // GLONASS time solution
+        enableUBXMessageOnPort(fd, 0x01, 0x23, ubxport, 16); // UBX-NAV-TIMEGLO
+	
+        if (doDEBUG) { cerr<<humanTimeNow()<<" Enabling UBX-NAV-CLOCK"<<endl; } // Beidou time solution
+        enableUBXMessageOnPort(fd, 0x01, 0x24, ubxport, 16); // UBX-NAV-TIMEBDS
+	
+        if (doDEBUG) { cerr<<humanTimeNow()<<" Enabling UBX-NAV-CLOCK"<<endl; } // Galileo time solution
+        enableUBXMessageOnPort(fd, 0x01, 0x25, ubxport, 16); // UBX-NAV-TIMEGAL
+      }
+            
       if(!version9 && !m8t && !fuzzPositionMeters) {
         if (doDEBUG) { cerr<<humanTimeNow()<<" Enabling debugging data"<<endl; } // RF doppler
         enableUBXMessageOnPort(fd, 0x03, 0x10, ubxport, 4);
@@ -948,12 +1008,73 @@ int main(int argc, char** argv)
   ns.launch();
   
   cerr<<humanTimeNow()<<" Entering main loop"<<endl;
+
+  struct TIMEXState
+  {
+    TIMEGAL gal;
+    TIMEGPS gps;
+    TIMEGLO glo;
+    TIMEBDS bds;
+    void transmitIfComplete(NMMSender& ns)
+    {
+      if(!(gal.itow && gal.itow == bds.itow && bds.itow == glo.itow && glo.itow == gps.itow))
+        return;
+      
+      NavMonMessage nmm;
+      nmm.set_sourceid(g_srcid);
+      nmm.set_localutcseconds(g_gnssutc.tv_sec);
+      nmm.set_localutcnanoseconds(g_gnssutc.tv_nsec);
+      
+      nmm.set_type(NavMonMessage::TimeOffsetType);
+      nmm.mutable_to()->set_itow(gps.itow);
+
+      auto no = nmm.mutable_to()->add_offsets();
+      no->set_gnssid(0);
+      no->set_offsetns(gps.ftow);
+      no->set_tacc(gps.tAcc);
+      no->set_tow(gps.itow); // this is for consistency
+      no->set_leaps(gps.leapS);
+      no->set_wn(gps.week);
+      no->set_valid(gps.valid);
+
+      no = nmm.mutable_to()->add_offsets();
+      no->set_gnssid(2);
+      no->set_offsetns(gal.fGalTow);
+      no->set_tacc(gal.tAcc);
+      no->set_leaps(gal.leapS);
+      no->set_wn(gal.galWno);
+      no->set_valid(gal.valid);
+      no->set_tow(gal.galTow);
+      
+      no = nmm.mutable_to()->add_offsets();
+      no->set_gnssid(3);
+      no->set_offsetns(bds.fSow);
+      no->set_tacc(bds.tAcc);
+      no->set_leaps(bds.leapS);
+      no->set_wn(bds.week);
+      no->set_valid(bds.valid);
+      no->set_tow(bds.sow);
+
+      no = nmm.mutable_to()->add_offsets();
+      no->set_gnssid(6);
+      no->set_offsetns(glo.fTod);
+      no->set_tacc(glo.tAcc);
+      no->set_nt(glo.nT);
+      no->set_n4(glo.n4);
+      no->set_valid(glo.valid);
+      no->set_tow(glo.tod);
+
+      ns.emitNMM(nmm);
+      gal.itow = 0;
+    }
+  } tstate;
+  
   for(;;) {
     try {
       auto [msg, timestamp] = getUBXMessage(fd, nullptr);
       (void)timestamp;
       auto payload = msg.getPayload();
-
+      
       if(msg.getClass() == 0x01 && msg.getType() == 0x07) {  // UBX-NAV-PVT
         struct PVT
         {
@@ -1042,10 +1163,7 @@ int main(int argc, char** argv)
         if(doDEBUG)
           cerr<<humanTimeNow()<<" Serial number from stream "<< serialno <<endl;
       }
-
-      
-      
-      if(msg.getClass() == 0x02 && msg.getType() == 0x15) {  // RAWX, the doppler stuff
+      else       if(msg.getClass() == 0x02 && msg.getType() == 0x15) {  // RAWX, the doppler stuff
         //        if (doDEBUG) { cerr<<humanTimeNow()<<" Got "<<(int)payload[11] <<" measurements "<<endl; }
         double rcvTow;
         memcpy(&rcvTow, &payload[0], 8);