bsncubed/aes67-ESP32-P4
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Step 3.3a: PTP servo, clock locks to the GM

Browse Source 
- ptp_hw: ptp_hw_adj_freq() (ETH_MAC_ESP_CMD_ADJ_PTP_TIME, absolute ppb
  vs nominal, clamped +-500 ppm, retried while the addend update is
  busy) and ptp_hw_step() (read + write time).
- ptp_clock: first Sync after a GM is selected seeds the integral with
  the measured rate and steps the clock; then linuxptp-style PI (kp/ki
  from the Sync interval: 0.7/0.3 at 1 s). Re-step above 1 ms. Locked
  after 8 Syncs with |offset| < 1 us, unlocked after 3 above; GM change
  or loss unlocks and keeps the frequency (holdover).
- Verified vs ptp4l: stepped to GM time, locked after ~19 s; locked
  offset within +-510 ns (mostly < 300 ns), correction +39.9 ppm
  (crystal -39.8 ppm), path delay ~10.2 us.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
This commit is contained in:
Ben Nicholson
2026-09-25 06:15:26 +10:00
parent e2f4d81326
commit 685060e12c
3 changed files with 119 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
components/aes67_ptp/ptp_clock.c
+83 -3
View File
@@ -1,5 +1,7 @@
#include "ptp_clock.h"
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "aes67_cfg.h"
@@ -16,6 +18,11 @@
#define PTP_GENERAL_PORT 320
#define HDR_LEN 34
#define FLAG_TWO_STEP 0x0200
#define STEP_NS 1000000 // re-step instead of slewing above 1 ms
#define LOCK_NS 1000 // |offset| below this counts as good
#define LOCK_GOOD 8 // consecutive good Syncs to lock
#define LOCK_BAD 3 // consecutive bad Syncs to unlock
#define MAX_DRIFT_PPB 500000.0
static const char *TAG = "ptp";
static const uint8_t PTP_MCAST_MAC[6] = { 0x01, 0x00, 0x5e, 0x00, 0x01, 0x81 };
@@ -53,8 +60,18 @@ static struct {
// while the local clock is not yet syntonised.
int64_t raw, prev_raw, prev_t2;
double rate;
int8_t log_sync; // GM's Sync interval (from Sync logMessageInterval)
// Servo (PI, linuxptp-style gains)
bool stepped; // clock set to GM time since this GM was selected
double drift_ppb; // integral term
double freq_ppb; // correction currently applied (positive = faster)
int64_t offset_ns;
int good, bad;
bool locked;
} s;
static void set_locked(bool locked);
/* ----- helpers ----- */
static uint16_t rd16(const uint8_t *p) { return (p[0] << 8) | p[1]; }
@@ -146,11 +163,71 @@ static void on_announce(const uint8_t *b, int len, uint32_t src_ip)
s.gm = m;
s.sync_pending = s.dreq_pending = false;
s.delay_ns = s.prev_t2 = 0;
s.stepped = false;
set_locked(false);
s.log_dreq = 0;
s.next_dreq_us = 0;
}
}
static void set_locked(bool locked)
{
if (locked != s.locked) {
s.locked = locked;
if (locked) {
ESP_LOGI(TAG, "locked: offset %+lld ns, frequency %+.3f ppm, path delay %lld ns",
s.offset_ns, s.freq_ppb / 1000, s.delay_ns);
} else {
ESP_LOGW(TAG, "unlocked");
}
}
if (!locked) {
s.good = 0;
}
}
// offset = local - GM (ns)
static void servo(int64_t offset)
{
s.offset_ns = offset;
if (!s.stepped || llabs(offset) > STEP_NS) {
if (!s.stepped) {
// Seed the integral with the measured rate error (measured with the current correction applied).
s.drift_ppb += s.rate * 1e9;
}
s.freq_ppb = -s.drift_ppb;
ptp_hw_adj_freq(s.freq_ppb);
esp_err_t err = ptp_hw_step(offset);
ESP_LOGI(TAG, "clock stepped by %+lld ns (%s), frequency %+.3f ppm", -offset, esp_err_to_name(err),
s.freq_ppb / 1000);
s.stepped = true;
s.prev_t2 = 0; // rate across the step is meaningless
s.bad = 0;
set_locked(false);
return;
}
// linuxptp PI gains for hardware timestamps, scaled by the Sync interval
double iv = ldexp(1.0, s.log_sync);
double kp = fmin(0.7 * pow(iv, -0.3), 0.7 / iv);
double ki = fmin(0.3 * pow(iv, 0.4), 0.3 / iv);
double ppb = kp * offset + s.drift_ppb;
s.drift_ppb = fmax(-MAX_DRIFT_PPB, fmin(MAX_DRIFT_PPB, s.drift_ppb + ki * offset));
s.freq_ppb = -ppb;
ptp_hw_adj_freq(s.freq_ppb);
if (llabs(offset) < LOCK_NS) {
s.bad = 0;
if (++s.good >= LOCK_GOOD) {
set_locked(true);
}
} else {
s.good = 0;
if (s.locked && ++s.bad >= LOCK_BAD) {
set_locked(false);
}
}
}
static void sync_complete(void)
{
s.sync_pending = false;
@@ -164,9 +241,9 @@ static void sync_complete(void)
return;
}
// offset = t2 - t1 - corrections - mean path delay
int64_t offset = s.raw - s.delay_ns;
ESP_LOGI(TAG, "seq %u: offset %+lld ns, path delay %lld ns, rate %+.3f ppm", s.sync_seq, offset, s.delay_ns,
s.rate * 1e6);
servo(s.raw - s.delay_ns);
ESP_LOGI(TAG, "seq %u: offset %+lld ns, freq %+.3f ppm, path delay %lld ns%s", s.sync_seq, s.offset_ns,
s.freq_ppb / 1000, s.delay_ns, s.locked ? ", locked" : "");
}
static void on_sync(const uint8_t *b, int len)
@@ -181,6 +258,7 @@ static void on_sync(const uint8_t *b, int len)
return;
}
s.sync_seq = seq;
s.log_sync = (int8_t)b[33];
s.t2 = mac_ns(&t2);
s.sync_corr = rd_corr_ns(b + 8);
if (rd16(b + 6) & FLAG_TWO_STEP) {
@@ -309,6 +387,8 @@ static void ptp_task(void *arg)
ESP_LOGW(TAG, "TimeTransmitter lost (no Announce for %lld ms), listening", window / 1000);
memset(&s.gm, 0, sizeof(s.gm));
s.delay_ns = s.prev_t2 = 0;
s.stepped = false;
set_locked(false); // frequency correction stays (holdover)
} else if (now >= s.next_dreq_us && s.prev_t2) {
send_delay_req();
// Delay_Req interval from the GM's Delay_Resp; randomised 0.5..1.5x