diff --git a/ESP32/FvHNTP/FvHNTP.cpp b/ESP32/FvHNTP/FvHNTP.cpp index a8e4e1d..2982149 100644 --- a/ESP32/FvHNTP/FvHNTP.cpp +++ b/ESP32/FvHNTP/FvHNTP.cpp @@ -32,6 +32,25 @@ struct sntp_datagram uint32_t transmit_timestamp_fraq; }; +uint64_t get_us_from_ntp(const uint32_t high, const uint32_t low) +{ + return uint64_t(ntohl(high)) * 1000000ll + ntohl(low) / 4295; +} + +uint64_t micros64() +{ + static uint32_t low32 = 0, high32 = 0; + + uint32_t new_low32 = micros(); + + if (new_low32 < low32) + high32++; + + low32 = new_low32; + + return (uint64_t(high32) << 32) | low32; +} + ntp::ntp(const std::string & server): server(server) { } @@ -51,21 +70,16 @@ void ntp::begin() th = new std::thread(std::ref(*this)); } -std::optional ntp::get_unix_epoch_ms() +std::optional ntp::get_unix_epoch_us() { std::unique_lock lck(lock); if (ntp_at_ts == 0) return { }; - auto now = millis(); + auto now = micros64(); - return ntp_at_ts + now - millis_at_ts - NTP_EPOCH * 1000; -} - -uint64_t get_ms_from_ntp(const uint32_t high, const uint32_t low) -{ - return uint64_t(ntohl(high)) * 1000ll + ntohl(low) / (4295 * 1000); + return ntp_at_ts + now - micros_at_ts - NTP_EPOCH * 1000000l; } void ntp::operator()() @@ -87,11 +101,11 @@ void ntp::operator()() packet_out.stratum = 14; packet_out.poll = 2; - auto now = get_unix_epoch_ms(); + auto now = get_unix_epoch_us(); if (now.has_value()) { - uint64_t sec = now.value() / 1000; - uint64_t usec = (now.value() % 1000) * 1000; + uint64_t sec = now.value() / 1000000l; + uint64_t usec = now.value() % 1000000l; packet_out.originate_timestamp_secs = htonl(sec + NTP_EPOCH); // T1 packet_out.originate_timestamp_fraq = htonl(usec * 4295); @@ -103,37 +117,36 @@ void ntp::operator()() // TODO verify source address if (recvfrom(fd, &packet_in, sizeof(packet_in), 0, nullptr, nullptr) == sizeof(packet_in)) { // TODO verify version etc - uint32_t now = millis(); - auto t_t4 = get_unix_epoch_ms(); + uint64_t now = micros64(); + auto t_t4 = get_unix_epoch_us(); std::unique_lock lck(lock); s = 60; if (t_t4.has_value()) { - int64_t t1 = get_ms_from_ntp(packet_out.originate_timestamp_secs, packet_out.originate_timestamp_fraq); - int64_t t2 = get_ms_from_ntp(packet_in.receive_timestamp_seqs, packet_in.receive_timestamp_fraq ); - int64_t t3 = get_ms_from_ntp(packet_in.transmit_timestamp_secs, packet_in.transmit_timestamp_fraq ); - int64_t t4 = t_t4.value() + NTP_EPOCH * 1000; + int64_t t1 = get_us_from_ntp(packet_out.originate_timestamp_secs, packet_out.originate_timestamp_fraq); + int64_t t2 = get_us_from_ntp(packet_in.receive_timestamp_seqs, packet_in.receive_timestamp_fraq ); + int64_t t3 = get_us_from_ntp(packet_in.transmit_timestamp_secs, packet_in.transmit_timestamp_fraq ); + int64_t t4 = t_t4.value() + NTP_EPOCH * 1000000l; auto offset = ((t2 - t1) + (t3 - t4)) / 2; - // TODO handle millis wrap if (offset > 0) { - if (offset < millis_at_ts) - millis_at_ts -= offset; + if (offset < micros_at_ts) + micros_at_ts -= offset; else { - millis_at_ts = 0; + micros_at_ts = 0; s = 4; } } else { - millis_at_ts -= offset; + micros_at_ts -= offset; } } else { - ntp_at_ts = get_ms_from_ntp(packet_in.transmit_timestamp_secs, packet_in.transmit_timestamp_fraq); - millis_at_ts = now; + ntp_at_ts = get_us_from_ntp(packet_in.transmit_timestamp_secs, packet_in.transmit_timestamp_fraq); + micros_at_ts = now; } } }