update xsns_60_GPS.ino

lib/ArduinoNTPd/NTPServer.cpp

@@ -17,63 +17,81 @@
 
 bool NtpServer::beginListening()
 {
-    if (timeServerPort_.begin(NTP_PORT)){
+  if (timeServerPort_.begin(NTP_PORT)){
-      return true;
+    return true;
-    }
+  }
-    return false;
+  return false;
 }
 
 bool NtpServer::processOneRequest(uint32_t utc, uint32_t millisecs) 
 {
-    // We need the time we've received the packet in our response.
+  // millisecs is millis() at the time of the last iTOW reception, where iTOW%1000 == 0
-    uint32_t recvSecs =  utc + NTP_TIMESTAMP_DIFF;
+  uint32_t refMillis = millis()-millisecs;
-    double recvFractDouble = (double)millisecs/0.00023283064365386963; // millisec/((10^6)/(2^32))
+  if (refMillis>999){
-    uint32_t recvFract = (double)recvFractDouble; //TODO: really handle this!!!
+    utc++;
-    bool processed = false;
+    refMillis = refMillis%1000;
-    
+  }
-    int packetDataSize = timeServerPort_.parsePacket();
-    if (packetDataSize && packetDataSize >= NtpPacket::PACKET_SIZE)
-    {
-        // Received what is probably an NTP packet. Read it in and verify
-        // that it's legit.
-        NtpPacket packet;
-        timeServerPort_.read((char*)&packet, NtpPacket::PACKET_SIZE);
-        // TODO: verify packet.
 
-        // Populate response.
+  bool processed = false;
-        packet.swapEndian();        
+  
-        packet.leapIndicator(0);
+  int packetDataSize = timeServerPort_.parsePacket();
-        packet.versionNumber(4);
+  if (packetDataSize && packetDataSize >= NtpPacket::PACKET_SIZE)
-        packet.mode(4);
+  {
-        packet.stratum = 2; // I guess stratum 1 is too optimistic
+      // We need the time we've received the packet in our response.
-        packet.poll = 10; // 6-10 per RFC 5905.
+      uint32_t recvSecs =  utc + NTP_TIMESTAMP_DIFF;
-        packet.precision = -21; // ~0.5 microsecond precision.
+
-        packet.rootDelay = 0; //60 * (0xFFFF / 1000); // ~60 milliseconds, TBD
+      uint64_t recvFract64 = refMillis;
-        packet.rootDispersion = 0; //10 * (0xFFFF / 1000); // ~10 millisecond dispersion, TBD
+      recvFract64 <<= 32;
-        packet.referenceId[0] = 'G';
+      recvFract64 /= 1000;
-        packet.referenceId[1] = 'P';
+      uint32_t recvFract = recvFract64 & 0xffffffff;
-        packet.referenceId[2] = 'S';
+      // is equal to:
-        packet.referenceId[3] = 0;
+      // uint32_t recvFract = (double)(refMillis)/0.00000023283064365386963;
-        packet.referenceTimestampSeconds = utc;
+
-        packet.referenceTimestampFraction = recvFract;
+      // Received what is probably an NTP packet. Read it in and verify
-        packet.originTimestampSeconds = packet.transmitTimestampSeconds;
+      // that it's legit.
-        packet.originTimestampFraction = packet.transmitTimestampFraction;
+      NtpPacket packet;
-        packet.receiveTimestampSeconds = recvSecs;
+      timeServerPort_.read((char*)&packet, NtpPacket::PACKET_SIZE);
-        packet.receiveTimestampFraction = recvFract;
+      // TODO: verify packet.
-        
+
-        // ...and the transmit time.
+      // Populate response.
-        // timeSource_.now(&packet.transmitTimestampSeconds, &packet.transmitTimestampFraction);
+      packet.swapEndian();        
-        
+      packet.leapIndicator(0);
-        // Now transmit the response to the client.
+      packet.versionNumber(4);
-        packet.swapEndian();
+      packet.mode(4);
-        timeServerPort_.beginPacket(timeServerPort_.remoteIP(), timeServerPort_.remotePort());
+      packet.stratum = 1; // >1 will lead to misinterpretation of refId
-        for (int count = 0; count < NtpPacket::PACKET_SIZE; count++)
+      packet.poll = 10; // 6-10 per RFC 5905.
-        {
+      packet.precision = -21; // ~0.5 microsecond precision.
-            timeServerPort_.write(packet.packet()[count]);
+      packet.rootDelay = 100 * (0xFFFF / 1000); //~100 milliseconds
-        }
+      packet.rootDispersion = 50 * (0xFFFF / 1000);; //~50 millisecond dispersion
-        timeServerPort_.endPacket();
+      packet.referenceId[0] = 'G';
-        processed = true;
+      packet.referenceId[1] = 'P';
-    } 
+      packet.referenceId[2] = 'S';
-    
+      packet.referenceId[3] = 0;
-    return processed;
+      packet.referenceTimestampSeconds = recvSecs;
+      packet.referenceTimestampFraction = 0;  // the "click" of the GPS
+      packet.originTimestampSeconds = packet.transmitTimestampSeconds;
+      packet.originTimestampFraction = packet.transmitTimestampFraction;
+      packet.receiveTimestampSeconds = recvSecs;
+      packet.receiveTimestampFraction = recvFract;
+      
+      // ...and the transmit time.
+      // the latency has been between 135 and 175 microseconds in internal testing, so we harcode 150 
+      uint32_t transFract = recvFract+(150*(10^3)/(2^32)); // microsec/((10^3)/(2^32))
+      if (recvFract>transFract){
+        recvSecs++; //overflow
+      }
+      packet.transmitTimestampSeconds = recvSecs;
+      packet.transmitTimestampFraction = transFract; 
+      
+      // Now transmit the response to the client.
+      packet.swapEndian();
+
+      timeServerPort_.beginPacket(timeServerPort_.remoteIP(), timeServerPort_.remotePort());
+      timeServerPort_.write(packet.packet(), NtpPacket::PACKET_SIZE);
+      timeServerPort_.endPacket();
+
+      processed = true;
+  } 
+  
+  return processed;
 }

tasmota/xsns_60_GPS.ino

@@ -129,6 +129,7 @@ const char kUBXTypes[] PROGMEM = "UBX";
 
 #define UBX_SERIAL_BUFFER_SIZE 256
 #define UBX_TCP_PORT           1234
+#define NTP_MILLIS_OFFSET      50              // estimated latency in milliseconds 
 
 /********************************************************************************************\
 | *globals
@@ -251,7 +252,8 @@ struct UBX_t {
     uint32_t last_vAcc;
     uint8_t gpsFix;
     uint8_t non_empty_loops;   // in case of an unintended reset of the GPS, the serial interface will get flooded with NMEA
-    uint16_t log_interval;  // in tenth of seconds
+    uint16_t log_interval;     // in tenth of seconds
+    int32_t timeOffset;        // roughly computed offset millis() - iTOW
   } state;
 
   struct {
@@ -260,6 +262,7 @@ struct UBX_t {
     uint32_t send_when_new:1; // no teleinterval
     uint32_t send_UI_only:1;
     uint32_t runningNTP:1;
+    // uint32_t blockedNTP:1;
     uint32_t forceUTCupdate:1;
     uint32_t runningVPort:1;
     // TODO: more to come
@@ -322,6 +325,15 @@ void UBXinitCFG(void)
   DEBUG_SENSOR_LOG(PSTR("UBX: turn off NMEA"));
 }
 
+void UBXsendCFGLine(uint8_t _line)
+{
+  if (_line>sizeof(UBLOX_INIT)/16) return;
+  for (uint32_t i = 0; i < 16; i++) {
+    UBXSerial->write( pgm_read_byte(UBLOX_INIT+i+(_line*16)) );
+  }
+  DEBUG_SENSOR_LOG(PSTR("UBX: send line %u of UBLOX_INIT"), _line);
+}
+
 void UBXTriggerTele(void)
 {
   mqtt_data[0] = '\0';
@@ -583,6 +595,8 @@ void UBXSelectMode(uint16_t mode)
       break;
     case 10:
       UBX.mode.runningNTP = false;
+      UBXsendCFGLine(10); //NAV-POSLLH on
+      UBXsendCFGLine(11); //NAV-STATUS on
       break;
     case 11:
       UBX.mode.forceUTCupdate = true;
@@ -604,7 +618,6 @@ void UBXSelectMode(uint16_t mode)
       break;
     default:
       if (mode>1000 && mode <1066) {
-        // UBXSetRate(mode-1000); // min. 1001 = 0.001 Hz, but will be converted to 1/65535 anyway ~0.015 Hz, max. 2000 = 1.000 Hz
         UBXSetRate(mode-1000); // set interval between measurements in seconds from 1 to 65
       }
       break;
@@ -629,13 +642,16 @@ bool UBXHandlePOSLLH()
     UBX.rec_buffer.values.lon = UBX.Message.navPosllh.lon;
     DEBUG_SENSOR_LOG(PSTR("UBX: lat/lon: %i / %i"), UBX.rec_buffer.values.lat, UBX.rec_buffer.values.lon);
     DEBUG_SENSOR_LOG(PSTR("UBX: hAcc: %d"), UBX.Message.navPosllh.hAcc);
-    UBX.state.last_iTOW = UBX.Message.navPosllh.iTOW;
     UBX.state.last_alt = UBX.Message.navPosllh.alt;
     UBX.state.last_vAcc = UBX.Message.navPosllh.vAcc;
     UBX.state.last_hAcc = UBX.Message.navPosllh.hAcc;
     if (UBX.mode.send_when_new) {
       UBXTriggerTele();
     }
+    if (UBX.mode.runningNTP){ // after receiving pos-data at least once -> go to pure NTP-mode
+      UBXsendCFGLine(7); //NAV-POSLLH off
+      UBXsendCFGLine(8); //NAV-STATUS off
+    }
     return true; // new position
   } else {
     DEBUG_SENSOR_LOG(PSTR("UBX: no valid position data"));
@@ -657,9 +673,9 @@ void UBXHandleTIME()
 {
   DEBUG_SENSOR_LOG(PSTR("UBX: UTC-Time: %u-%u-%u %u:%u:%u"), UBX.Message.navTime.year, UBX.Message.navTime.month ,UBX.Message.navTime.day,UBX.Message.navTime.hour,UBX.Message.navTime.min,UBX.Message.navTime.sec);
   if (UBX.Message.navTime.valid.UTC == 1) {
+    UBX.state.timeOffset =  millis(); // iTOW%1000 should be 0 here, when NTP-server is enabled and in "pure mode"
     DEBUG_SENSOR_LOG(PSTR("UBX: UTC-Time is valid"));
-    if (Rtc.user_time_entry == false || UBX.mode.forceUTCupdate) {
+    if (Rtc.user_time_entry == false || UBX.mode.forceUTCupdate || UBX.mode.runningNTP) {
-      AddLog_P(LOG_LEVEL_INFO, PSTR("UBX: UTC-Time is valid, set system time"));
       TIME_T gpsTime;
       gpsTime.year = UBX.Message.navTime.year - 1970;
       gpsTime.month = UBX.Message.navTime.month;
@@ -667,7 +683,11 @@ void UBXHandleTIME()
       gpsTime.hour = UBX.Message.navTime.hour;
       gpsTime.minute = UBX.Message.navTime.min;
       gpsTime.second = UBX.Message.navTime.sec;
-      Rtc.utc_time = MakeTime(gpsTime);
+      UBX.rec_buffer.values.time = MakeTime(gpsTime);
+      if (UBX.mode.forceUTCupdate || Rtc.user_time_entry == false){
+        AddLog_P(LOG_LEVEL_INFO, PSTR("UBX: UTC-Time is valid, set system time"));
+        Rtc.utc_time = UBX.rec_buffer.values.time;
+      } 
       Rtc.user_time_entry = true;
     }
   }
@@ -705,7 +725,7 @@ void UBXLoop50msec(void)
   }
   // handle NTP-server
   if(UBX.mode.runningNTP){
-    timeServer.processOneRequest(Rtc.utc_time, UBX.state.last_iTOW%1000);
+    timeServer.processOneRequest(UBX.rec_buffer.values.time, UBX.state.timeOffset - NTP_MILLIS_OFFSET);
   }
 }