update xsns_60_GPS.ino

2026-03-19 13:48:42 +01:00 · 2020-04-06 19:24:11 +02:00
parent 17c605ac6a
commit cefc278bdf
2 changed files with 99 additions and 61 deletions
--- a/lib/ArduinoNTPd/NTPServer.cpp
+++ b/lib/ArduinoNTPd/NTPServer.cpp
@@ -17,63 +17,81 @@

 bool NtpServer::beginListening()
 {
-    if (timeServerPort_.begin(NTP_PORT)){
-      return true;
-    }
-    return false;
+  if (timeServerPort_.begin(NTP_PORT)){
+    return true;
+  }
+  return false;
 }

 bool NtpServer::processOneRequest(uint32_t utc, uint32_t millisecs) 
 {
-    // We need the time we've received the packet in our response.
-    uint32_t recvSecs =  utc + NTP_TIMESTAMP_DIFF;
-    double recvFractDouble = (double)millisecs/0.00023283064365386963; // millisec/((10^6)/(2^32))
-    uint32_t recvFract = (double)recvFractDouble; //TODO: really handle this!!!
-    bool processed = false;
-    
-    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.
+  // millisecs is millis() at the time of the last iTOW reception, where iTOW%1000 == 0
+  uint32_t refMillis = millis()-millisecs;
+  if (refMillis>999){
+    utc++;
+    refMillis = refMillis%1000;
+  }

-        // Populate response.
-        packet.swapEndian();        
-        packet.leapIndicator(0);
-        packet.versionNumber(4);
-        packet.mode(4);
-        packet.stratum = 2; // I guess stratum 1 is too optimistic
-        packet.poll = 10; // 6-10 per RFC 5905.
-        packet.precision = -21; // ~0.5 microsecond precision.
-        packet.rootDelay = 0; //60 * (0xFFFF / 1000); // ~60 milliseconds, TBD
-        packet.rootDispersion = 0; //10 * (0xFFFF / 1000); // ~10 millisecond dispersion, TBD
-        packet.referenceId[0] = 'G';
-        packet.referenceId[1] = 'P';
-        packet.referenceId[2] = 'S';
-        packet.referenceId[3] = 0;
-        packet.referenceTimestampSeconds = utc;
-        packet.referenceTimestampFraction = recvFract;
-        packet.originTimestampSeconds = packet.transmitTimestampSeconds;
-        packet.originTimestampFraction = packet.transmitTimestampFraction;
-        packet.receiveTimestampSeconds = recvSecs;
-        packet.receiveTimestampFraction = recvFract;
-        
-        // ...and the transmit time.
-        // timeSource_.now(&packet.transmitTimestampSeconds, &packet.transmitTimestampFraction);
-        
-        // Now transmit the response to the client.
-        packet.swapEndian();
-        timeServerPort_.beginPacket(timeServerPort_.remoteIP(), timeServerPort_.remotePort());
-        for (int count = 0; count < NtpPacket::PACKET_SIZE; count++)
-        {
-            timeServerPort_.write(packet.packet()[count]);
-        }
-        timeServerPort_.endPacket();
-        processed = true;
-    } 
-    
-    return processed;
+  bool processed = false;
+  
+  int packetDataSize = timeServerPort_.parsePacket();
+  if (packetDataSize && packetDataSize >= NtpPacket::PACKET_SIZE)
+  {
+      // We need the time we've received the packet in our response.
+      uint32_t recvSecs =  utc + NTP_TIMESTAMP_DIFF;
+
+      uint64_t recvFract64 = refMillis;
+      recvFract64 <<= 32;
+      recvFract64 /= 1000;
+      uint32_t recvFract = recvFract64 & 0xffffffff;
+      // is equal to:
+      // uint32_t recvFract = (double)(refMillis)/0.00000023283064365386963;
+
+      // 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.
+      packet.swapEndian();        
+      packet.leapIndicator(0);
+      packet.versionNumber(4);
+      packet.mode(4);
+      packet.stratum = 1; // >1 will lead to misinterpretation of refId
+      packet.poll = 10; // 6-10 per RFC 5905.
+      packet.precision = -21; // ~0.5 microsecond precision.
+      packet.rootDelay = 100 * (0xFFFF / 1000); //~100 milliseconds
+      packet.rootDispersion = 50 * (0xFFFF / 1000);; //~50 millisecond dispersion
+      packet.referenceId[0] = 'G';
+      packet.referenceId[1] = 'P';
+      packet.referenceId[2] = 'S';
+      packet.referenceId[3] = 0;
+      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;
 }