folkert/KEK
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

microseconds resolution

Browse source
This commit is contained in:
folkert van heusden 2024-05-07 14:22:40 +02:00
parent 5f8b2028a5
commit 8c8084ec5b
Signed by untrusted user who does not match committer: folkert
GPG key ID: 6B6455EDFEED3BD1
1 changed files with 37 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

61
ESP32/FvHNTP/FvHNTP.cpp
View file

@ -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<uint64_t> ntp::get_unix_epoch_ms()
std::optional<uint64_t> ntp::get_unix_epoch_us()
{
std::unique_lock<std::mutex> 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<std::mutex> 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;
}
}
}