feat(pps): preserve loop dynamics across nav periods

This commit is contained in:
2026-07-29 00:45:16 +03:00
parent a6bb080270
commit 347e1c671d
19 changed files with 889 additions and 111 deletions
@@ -55,6 +55,7 @@
#endif #endif
#define GPS_NAV_BITS_PER_WEEK 30240000u #define GPS_NAV_BITS_PER_WEEK 30240000u
#define GPS_NAV_SOLVE_PERIOD_MS (GPS_NAV_SOLVE_TIMER_TICKS * GPS_NAV_SOLVE_TIMER_TICK_MS) #define GPS_NAV_SOLVE_PERIOD_MS (GPS_NAV_SOLVE_TIMER_TICKS * GPS_NAV_SOLVE_TIMER_TICK_MS)
#define GPS_NAV_BITS_PER_SUBFRAME 300u
/* Keep integrity statistics active, but do not let leave-one-out FDE /* Keep integrity statistics active, but do not let leave-one-out FDE
* replace the full-WLS position until the real measurement-noise model * replace the full-WLS position until the real measurement-noise model
* is calibrated. The live stationary test produced a 22.7% exclusion * is calibrated. The live stationary test produced a 22.7% exclusion
@@ -83,6 +84,8 @@
#error "GPS_NAV_SOLVE_TIMER_TICKS must be non-zero" #error "GPS_NAV_SOLVE_TIMER_TICKS must be non-zero"
#elif ((GPS_NAV_BITS_PER_WEEK % GPS_NAV_SOLVE_TIMER_TICKS) != 0u) #elif ((GPS_NAV_BITS_PER_WEEK % GPS_NAV_SOLVE_TIMER_TICKS) != 0u)
#error "GPS_NAV_SOLVE_TIMER_TICKS must divide 30240000 GPS navigation bits per week" #error "GPS_NAV_SOLVE_TIMER_TICKS must divide 30240000 GPS navigation bits per week"
#elif ((GPS_NAV_BITS_PER_SUBFRAME % GPS_NAV_SOLVE_TIMER_TICKS) != 0u)
#error "GPS_NAV_SOLVE_TIMER_TICKS must divide the 300-bit LNAV subframe"
#endif #endif
_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u), _Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
"GPS_NAV_ENABLE_FDE must be zero or one"); "GPS_NAV_ENABLE_FDE must be zero or one");
@@ -2,6 +2,9 @@
#define GPS_PPS_QUALITY_H #define GPS_PPS_QUALITY_H
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h>
#define GPS_PPS_BASELINE_UPDATE_PERIOD_SEC 0.200
typedef enum { typedef enum {
GPS_PPS_PROFILE_HOLDOVER = 0, GPS_PPS_PROFILE_HOLDOVER = 0,
@@ -29,6 +32,8 @@ typedef struct {
gps_pps_profile_t profile; gps_pps_profile_t profile;
gps_pps_profile_t candidate; gps_pps_profile_t candidate;
unsigned int candidate_updates; unsigned int candidate_updates;
unsigned int upgrade_updates;
unsigned int degrade_updates;
} gps_pps_quality_selector_t; } gps_pps_quality_selector_t;
typedef struct { typedef struct {
@@ -43,13 +48,17 @@ typedef struct {
} gps_pps_loop_profile_t; } gps_pps_loop_profile_t;
void gps_pps_quality_selector_init(gps_pps_quality_selector_t* selector); void gps_pps_quality_selector_init(gps_pps_quality_selector_t* selector);
void gps_pps_quality_selector_init_period(
gps_pps_quality_selector_t* selector,
double update_period_sec);
/* /*
* Select a bounded loop profile from timing geometry, code/carrier quality * Select a bounded loop profile from timing geometry, code/carrier quality
* and integrity. Upgrades require sustained evidence and degradations require * and integrity. Upgrades and degradations require sustained evidence; their
* three consecutive observations. A single rejected WLS epoch is skipped * update counts are scaled from the proven 200 ms timing when period-aware
* without resetting the loop; sustained loss enters holdover, while the * initialization is used. A single rejected WLS epoch is skipped without
* independent reference watchdog remains the final stale-data guard. * resetting the loop; sustained loss enters holdover, while the independent
* reference watchdog remains the final stale-data guard.
*/ */
gps_pps_profile_t gps_pps_quality_selector_update( gps_pps_profile_t gps_pps_quality_selector_update(
gps_pps_quality_selector_t* selector, gps_pps_quality_selector_t* selector,
@@ -57,6 +66,22 @@ gps_pps_profile_t gps_pps_quality_selector_update(
bool* profile_changed_out); bool* profile_changed_out);
gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile); gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile);
gps_pps_loop_profile_t gps_pps_loop_profile_for_period(
gps_pps_profile_t profile,
double update_period_sec);
/*
* Map the proven 200 ms discrete poles to another update period. The pole
* exponentiation preserves the current loop's continuous-time settling and
* damping while returning exactly the legacy coefficients at 200 ms.
*/
bool gps_pps_loop_coefficients(
gps_pps_loop_profile_t parameters,
double update_period_sec,
double clock_frequency_hz,
int64_t* kp_q46_out,
int64_t* ki_q46_out,
uint32_t* max_slew_q32_out);
const char* gps_pps_profile_name(gps_pps_profile_t profile); const char* gps_pps_profile_name(gps_pps_profile_t profile);
#endif #endif
@@ -58,7 +58,9 @@ bool gps_pps_reference_bias_drift(
* linear oscillator drift. The Doppler WLS clock-drift state predicts the * linear oscillator drift. The Doppler WLS clock-drift state predicts the
* bias at the new capture epoch; only the residual bias noise is low-passed. * bias at the new capture epoch; only the residual bias noise is low-passed.
* If Doppler is temporarily unavailable, a bounded, slower finite-difference * If Doppler is temporarily unavailable, a bounded, slower finite-difference
* drift estimate keeps the filter usable in the robust profile. * drift estimate keeps the filter usable in the robust profile. All three
* baseline per-update gains are converted from the actual elapsed time, so
* changing the NAV solve period does not change their real-time poles.
* *
* COARSE is returned during initial settling, a reference-datum change, or * COARSE is returned during initial settling, a reference-datum change, or
* after a gap too long for bounded Doppler prediction. The independent * after a gap too long for bounded Doppler prediction. The independent
+3 -1
View File
@@ -1844,7 +1844,9 @@ void gps_startup_initialization(unsigned int nsp_tick)
GPS_NAV_ENABLE_FDE); GPS_NAV_ENABLE_FDE);
gps_init_acq_tables(); gps_init_acq_tables();
gps_pps_init(nsp_tick); gps_pps_init(nsp_tick);
gps_pps_quality_selector_init(&gps_pps_quality_selector); gps_pps_quality_selector_init_period(
&gps_pps_quality_selector,
(double)nsp_tick * (double)GPS_NAV_SOLVE_TIMER_TICK_MS / 1000.0);
gps_pps_applied_profile = GPS_PPS_PROFILE_HOLDOVER; gps_pps_applied_profile = GPS_PPS_PROFILE_HOLDOVER;
gps_pps_status_log_decimator = 0u; gps_pps_status_log_decimator = 0u;
gps_pps_reference_svid_valid = false; gps_pps_reference_svid_valid = false;
+49 -19
View File
@@ -65,7 +65,8 @@ enum {
}; };
#define GPS_PPS_VERSION_ADAPTIVE_LOOP 0x00020200u #define GPS_PPS_VERSION_ADAPTIVE_LOOP 0x00020200u
#define GPS_PPS_REFERENCE_TIMEOUT_TICKS (2u * GPS_PPS_CLOCK_HZ) #define GPS_PPS_MIN_REFERENCE_TIMEOUT_TICKS (2u * GPS_PPS_CLOCK_HZ)
#define GPS_PPS_REFERENCE_TIMEOUT_PERIODS 3u
static volatile gps_pps_regs_t* const gps_pps = static volatile gps_pps_regs_t* const gps_pps =
(volatile gps_pps_regs_t*)BSP_GPS_PPS_BASE; (volatile gps_pps_regs_t*)BSP_GPS_PPS_BASE;
@@ -77,6 +78,7 @@ static bool gps_pps_force_coarse;
static bool gps_pps_reference_committed; static bool gps_pps_reference_committed;
static bool gps_pps_holdover; static bool gps_pps_holdover;
static uint64_t gps_pps_last_reference_counter; static uint64_t gps_pps_last_reference_counter;
static uint64_t gps_pps_reference_timeout_ticks;
static gps_pps_time_filter_t gps_pps_time_filter; static gps_pps_time_filter_t gps_pps_time_filter;
static uint64_t gps_pps_read_u64( static uint64_t gps_pps_read_u64(
@@ -94,19 +96,30 @@ static void gps_pps_write_q46(volatile uint32_t* lo, volatile uint32_t* hi, int6
*hi = (uint32_t)(encoded >> 32); *hi = (uint32_t)(encoded >> 32);
} }
static void gps_pps_write_loop_profile(gps_pps_profile_t profile) static double gps_pps_update_period_sec(void)
{ {
const double pi = 3.14159265358979323846; return (gps_pps_nav_solve_tick != 0u) ?
gps_pps_loop_profile_t parameters = gps_pps_loop_profile(profile); ((double)gps_pps_nav_solve_tick * 0.020) :
const double update_period_sec = (gps_pps_nav_solve_tick != 0u) ? GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
((double)gps_pps_nav_solve_tick * 0.020) : 0.200; }
const double wn = 2.0 * pi * parameters.natural_frequency_hz;
const double q46 = 70368744177664.0; static bool gps_pps_write_loop_profile(gps_pps_profile_t profile)
int64_t kp_q46 = (int64_t)llround( {
(2.0 * parameters.damping * wn / GPS_PPS_CLOCK_HZ) * q46); const double update_period_sec = gps_pps_update_period_sec();
int64_t ki_q46 = (int64_t)llround((wn * wn * update_period_sec / GPS_PPS_CLOCK_HZ) * q46); gps_pps_loop_profile_t parameters =
uint32_t max_slew_q32 = (uint32_t)llround( gps_pps_loop_profile_for_period(profile, update_period_sec);
parameters.max_slew_ppm * 1.0e-6 * 4294967296.0); int64_t kp_q46 = 0;
int64_t ki_q46 = 0;
uint32_t max_slew_q32 = 0u;
if(!gps_pps_loop_coefficients(
parameters, update_period_sec, (double)GPS_PPS_CLOCK_HZ,
&kp_q46, &ki_q46, &max_slew_q32))
{
GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_NAV,
"PPS loop coefficient generation failed profile=%s period_ms=%.1lf\r\n",
gps_pps_profile_name(profile), update_period_sec * 1000.0);
return false;
}
gps_pps_write_q46(&gps_pps->kp_q46_lo, &gps_pps->kp_q46_hi, kp_q46); gps_pps_write_q46(&gps_pps->kp_q46_lo, &gps_pps->kp_q46_hi, kp_q46);
gps_pps_write_q46(&gps_pps->ki_q46_lo, &gps_pps->ki_q46_hi, ki_q46); gps_pps_write_q46(&gps_pps->ki_q46_lo, &gps_pps->ki_q46_hi, ki_q46);
@@ -118,14 +131,25 @@ static void gps_pps_write_loop_profile(gps_pps_profile_t profile)
((parameters.lock_release_ticks & 0xffu) << 16) | ((parameters.lock_release_ticks & 0xffu) << 16) |
((parameters.lock_acquire_updates & 0xffu) << 24); ((parameters.lock_acquire_updates & 0xffu) << 24);
} }
return true;
} }
void gps_pps_init(unsigned int nav_solve_tick) void gps_pps_init(unsigned int nav_solve_tick)
{ {
gps_pps_nav_solve_tick = nav_solve_tick; gps_pps_nav_solve_tick = nav_solve_tick;
uint64_t period_ticks =
((uint64_t)GPS_PPS_CLOCK_HZ * nav_solve_tick * 20u) / 1000u;
gps_pps_reference_timeout_ticks =
GPS_PPS_REFERENCE_TIMEOUT_PERIODS * period_ticks;
if(gps_pps_reference_timeout_ticks <
(uint64_t)GPS_PPS_MIN_REFERENCE_TIMEOUT_TICKS)
{
gps_pps_reference_timeout_ticks =
GPS_PPS_MIN_REFERENCE_TIMEOUT_TICKS;
}
gps_pps->control = 0u; gps_pps->control = 0u;
gps_pps_version = gps_pps->version; gps_pps_version = gps_pps->version;
gps_pps_write_loop_profile(GPS_PPS_PROFILE_ROBUST); (void)gps_pps_write_loop_profile(GPS_PPS_PROFILE_ROBUST);
gps_pps_active_loop_profile = GPS_PPS_PROFILE_ROBUST; gps_pps_active_loop_profile = GPS_PPS_PROFILE_ROBUST;
/* A GDB CPU reset does not reset the tracking-clock PPS state. Continue /* A GDB CPU reset does not reset the tracking-clock PPS state. Continue
* its mailbox sequence and force the first new solution to re-anchor the * its mailbox sequence and force the first new solution to re-anchor the
@@ -149,12 +173,14 @@ void gps_pps_init(unsigned int nav_solve_tick)
GPS_PPS_CONTROL_CLEAR_STATUS; GPS_PPS_CONTROL_CLEAR_STATUS;
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV, GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
"PPS init rtl=0x%08x adaptive=%u loop_config=0x%08x " "PPS init rtl=0x%08x adaptive=%u loop_config=0x%08x "
"clock_hz=%u update_ticks=%u\r\n", "clock_hz=%u update_ticks=%u reference_timeout_ms=%lu\r\n",
gps_pps_version, gps_pps_version,
gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ? 1u : 0u, gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ? 1u : 0u,
gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ? gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ?
gps_pps->loop_config : 0u, gps_pps->loop_config : 0u,
GPS_PPS_CLOCK_HZ, gps_pps_nav_solve_tick); GPS_PPS_CLOCK_HZ, gps_pps_nav_solve_tick,
(unsigned long)((gps_pps_reference_timeout_ticks * 1000u) /
GPS_PPS_CLOCK_HZ));
} }
void gps_pps_service(void) void gps_pps_service(void)
@@ -170,7 +196,7 @@ void gps_pps_service(void)
uint32_t now_hi = gps_pps->raw_counter_hi; uint32_t now_hi = gps_pps->raw_counter_hi;
uint64_t now = ((uint64_t)now_hi << 32) | now_lo; uint64_t now = ((uint64_t)now_hi << 32) | now_lo;
if((now - gps_pps_last_reference_counter) > if((now - gps_pps_last_reference_counter) >
(uint64_t)GPS_PPS_REFERENCE_TIMEOUT_TICKS) gps_pps_reference_timeout_ticks)
{ {
gps_pps_set_holdover(true); gps_pps_set_holdover(true);
} }
@@ -229,7 +255,10 @@ bool gps_pps_set_loop_profile(gps_pps_profile_t profile)
{ {
return false; return false;
} }
gps_pps_write_loop_profile(profile); if(!gps_pps_write_loop_profile(profile))
{
return false;
}
gps_pps_active_loop_profile = profile; gps_pps_active_loop_profile = profile;
return true; return true;
} }
@@ -277,7 +306,8 @@ gps_pps_time_filter_result_t gps_pps_filter_clock_bias(
double* innovation_m_out) double* innovation_m_out)
{ {
gps_pps_loop_profile_t parameters = gps_pps_loop_profile_t parameters =
gps_pps_loop_profile(gps_pps_active_loop_profile); gps_pps_loop_profile_for_period(
gps_pps_active_loop_profile, gps_pps_update_period_sec());
return gps_pps_time_filter_update( return gps_pps_time_filter_update(
&gps_pps_time_filter, epoch_counter, (double)GPS_PPS_CLOCK_HZ, &gps_pps_time_filter, epoch_counter, (double)GPS_PPS_CLOCK_HZ,
measured_clock_bias_m, clock_drift_valid, measured_clock_bias_m, clock_drift_valid,
@@ -7,6 +7,33 @@
#define GPS_PPS_PROFILE_UPGRADE_UPDATES 8u #define GPS_PPS_PROFILE_UPGRADE_UPDATES 8u
#define GPS_PPS_PROFILE_DEGRADE_UPDATES 3u #define GPS_PPS_PROFILE_DEGRADE_UPDATES 3u
#define GPS_PPS_MIN_HYSTERESIS_UPDATES 2u
#define GPS_PPS_MIN_LOCK_UPDATES 2u
#define GPS_PPS_MIN_SETTLE_UPDATES 3u
#define GPS_PPS_MAX_CONFIG_UPDATES 255u
static unsigned int gps_pps_period_updates(
unsigned int baseline_updates,
double update_period_sec,
unsigned int minimum_updates)
{
if(!isfinite(update_period_sec) || (update_period_sec <= 0.0))
{
update_period_sec = GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
}
double duration_sec =
(double)baseline_updates * GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
double required = ceil(duration_sec / update_period_sec - 1.0e-12);
if(required < (double)minimum_updates)
{
return minimum_updates;
}
if(required > (double)GPS_PPS_MAX_CONFIG_UPDATES)
{
return GPS_PPS_MAX_CONFIG_UPDATES;
}
return (unsigned int)required;
}
static bool gps_pps_metrics_finite(const gps_pps_quality_metrics_t* metrics) static bool gps_pps_metrics_finite(const gps_pps_quality_metrics_t* metrics)
{ {
@@ -67,12 +94,26 @@ static gps_pps_profile_t gps_pps_classify_quality(
} }
void gps_pps_quality_selector_init(gps_pps_quality_selector_t* selector) void gps_pps_quality_selector_init(gps_pps_quality_selector_t* selector)
{
gps_pps_quality_selector_init_period(
selector, GPS_PPS_BASELINE_UPDATE_PERIOD_SEC);
}
void gps_pps_quality_selector_init_period(
gps_pps_quality_selector_t* selector,
double update_period_sec)
{ {
if(selector != NULL) if(selector != NULL)
{ {
memset(selector, 0, sizeof(*selector)); memset(selector, 0, sizeof(*selector));
selector->profile = GPS_PPS_PROFILE_HOLDOVER; selector->profile = GPS_PPS_PROFILE_HOLDOVER;
selector->candidate = GPS_PPS_PROFILE_HOLDOVER; selector->candidate = GPS_PPS_PROFILE_HOLDOVER;
selector->upgrade_updates = gps_pps_period_updates(
GPS_PPS_PROFILE_UPGRADE_UPDATES, update_period_sec,
GPS_PPS_MIN_HYSTERESIS_UPDATES);
selector->degrade_updates = gps_pps_period_updates(
GPS_PPS_PROFILE_DEGRADE_UPDATES, update_period_sec,
GPS_PPS_MIN_HYSTERESIS_UPDATES);
} }
} }
@@ -111,8 +152,8 @@ gps_pps_profile_t gps_pps_quality_selector_update(
unsigned int required_updates = unsigned int required_updates =
(target > selector->profile) ? (target > selector->profile) ?
GPS_PPS_PROFILE_UPGRADE_UPDATES : selector->upgrade_updates :
GPS_PPS_PROFILE_DEGRADE_UPDATES; selector->degrade_updates;
if(selector->candidate_updates >= required_updates) if(selector->candidate_updates >= required_updates)
{ {
selector->profile = target; selector->profile = target;
@@ -127,10 +168,19 @@ gps_pps_profile_t gps_pps_quality_selector_update(
gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile) gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
{ {
return gps_pps_loop_profile_for_period(
profile, GPS_PPS_BASELINE_UPDATE_PERIOD_SEC);
}
gps_pps_loop_profile_t gps_pps_loop_profile_for_period(
gps_pps_profile_t profile,
double update_period_sec)
{
gps_pps_loop_profile_t parameters;
switch(profile) switch(profile)
{ {
case GPS_PPS_PROFILE_ACQUIRE: case GPS_PPS_PROFILE_ACQUIRE:
return (gps_pps_loop_profile_t){ parameters = (gps_pps_loop_profile_t){
.natural_frequency_hz = 0.120, .natural_frequency_hz = 0.120,
.damping = 0.9, .damping = 0.9,
.max_slew_ppm = 20.0, .max_slew_ppm = 20.0,
@@ -140,8 +190,9 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
.software_bias_gain = 1.0 / 16.0, .software_bias_gain = 1.0 / 16.0,
.software_settle_updates = 12u .software_settle_updates = 12u
}; };
break;
case GPS_PPS_PROFILE_PRECISION: case GPS_PPS_PROFILE_PRECISION:
return (gps_pps_loop_profile_t){ parameters = (gps_pps_loop_profile_t){
.natural_frequency_hz = 0.035, .natural_frequency_hz = 0.035,
.damping = 0.9, .damping = 0.9,
.max_slew_ppm = 20.0, .max_slew_ppm = 20.0,
@@ -151,8 +202,9 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
.software_bias_gain = 1.0 / 32.0, .software_bias_gain = 1.0 / 32.0,
.software_settle_updates = 12u .software_settle_updates = 12u
}; };
break;
case GPS_PPS_PROFILE_ROBUST: case GPS_PPS_PROFILE_ROBUST:
return (gps_pps_loop_profile_t){ parameters = (gps_pps_loop_profile_t){
.natural_frequency_hz = 0.012, .natural_frequency_hz = 0.012,
.damping = 1.0, .damping = 1.0,
.max_slew_ppm = 20.0, .max_slew_ppm = 20.0,
@@ -162,9 +214,10 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
.software_bias_gain = 1.0 / 32.0, .software_bias_gain = 1.0 / 32.0,
.software_settle_updates = 16u .software_settle_updates = 16u
}; };
break;
case GPS_PPS_PROFILE_HOLDOVER: case GPS_PPS_PROFILE_HOLDOVER:
default: default:
return (gps_pps_loop_profile_t){ parameters = (gps_pps_loop_profile_t){
.natural_frequency_hz = 0.0, .natural_frequency_hz = 0.0,
.damping = 1.0, .damping = 1.0,
.max_slew_ppm = 20.0, .max_slew_ppm = 20.0,
@@ -174,7 +227,106 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
.software_bias_gain = 1.0 / 32.0, .software_bias_gain = 1.0 / 32.0,
.software_settle_updates = 16u .software_settle_updates = 16u
}; };
break;
} }
parameters.lock_acquire_updates = gps_pps_period_updates(
parameters.lock_acquire_updates, update_period_sec,
GPS_PPS_MIN_LOCK_UPDATES);
parameters.software_settle_updates = gps_pps_period_updates(
parameters.software_settle_updates, update_period_sec,
GPS_PPS_MIN_SETTLE_UPDATES);
return parameters;
}
bool gps_pps_loop_coefficients(
gps_pps_loop_profile_t parameters,
double update_period_sec,
double clock_frequency_hz,
int64_t* kp_q46_out,
int64_t* ki_q46_out,
uint32_t* max_slew_q32_out)
{
if((kp_q46_out == NULL) || (ki_q46_out == NULL) ||
(max_slew_q32_out == NULL) ||
!isfinite(update_period_sec) || (update_period_sec <= 0.0) ||
!isfinite(clock_frequency_hz) || (clock_frequency_hz <= 0.0) ||
!isfinite(parameters.natural_frequency_hz) ||
(parameters.natural_frequency_hz < 0.0) ||
!isfinite(parameters.damping) || (parameters.damping < 0.0) ||
!isfinite(parameters.max_slew_ppm) ||
(parameters.max_slew_ppm < 0.0))
{
return false;
}
const double pi = 3.14159265358979323846;
const double q46 = 70368744177664.0;
const double q32 = 4294967296.0;
const double baseline_sec = GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
const double wn = 2.0 * pi * parameters.natural_frequency_hz;
/* Use the actual deployed Q2.46 baseline, not its ideal double value, so
* the mapped poles are those of the proven fixed-point implementation. */
const double baseline_kp =
(double)llround(
(2.0 * parameters.damping * wn / clock_frequency_hz) * q46) /
q46;
const double baseline_ki =
(double)llround(
(wn * wn * baseline_sec / clock_frequency_hz) * q46) /
q46;
const double baseline_ticks = clock_frequency_hz * baseline_sec;
const double baseline_sum =
2.0 - baseline_ticks * (baseline_kp + baseline_ki);
const double baseline_product =
1.0 - baseline_ticks * baseline_kp;
const double discriminant =
baseline_sum * baseline_sum - 4.0 * baseline_product;
const double period_ratio = update_period_sec / baseline_sec;
double target_sum = 0.0;
double target_product = 0.0;
if(discriminant >= 0.0)
{
double root = sqrt(discriminant);
double pole_a = 0.5 * (baseline_sum + root);
double pole_b = 0.5 * (baseline_sum - root);
if((pole_a < 0.0) || (pole_b < 0.0))
{
return false;
}
double target_a = pow(pole_a, period_ratio);
double target_b = pow(pole_b, period_ratio);
target_sum = target_a + target_b;
target_product = target_a * target_b;
}
else
{
if(baseline_product <= 0.0)
{
return false;
}
double radius = sqrt(baseline_product);
double cosine = baseline_sum / (2.0 * radius);
cosine = fmax(-1.0, fmin(1.0, cosine));
double angle = acos(cosine);
double target_radius = pow(radius, period_ratio);
target_sum =
2.0 * target_radius * cos(angle * period_ratio);
target_product = target_radius * target_radius;
}
double target_ticks = clock_frequency_hz * update_period_sec;
double kp = (1.0 - target_product) / target_ticks;
double ki = (1.0 - target_sum + target_product) / target_ticks;
if(!isfinite(kp) || !isfinite(ki) || (kp < 0.0) || (ki < 0.0))
{
return false;
}
*kp_q46_out = (int64_t)llround(kp * q46);
*ki_q46_out = (int64_t)llround(ki * q46);
*max_slew_q32_out = (uint32_t)llround(
parameters.max_slew_ppm * 1.0e-6 * q32);
return true;
} }
const char* gps_pps_profile_name(gps_pps_profile_t profile) const char* gps_pps_profile_name(gps_pps_profile_t profile)
@@ -20,6 +20,7 @@
#define GPS_PPS_FILTER_INNOVATION_CLAMP_M 300.0 #define GPS_PPS_FILTER_INNOVATION_CLAMP_M 300.0
#define GPS_PPS_FILTER_HARD_OUTLIER_M 1500.0 #define GPS_PPS_FILTER_HARD_OUTLIER_M 1500.0
#define GPS_PPS_SPEED_OF_LIGHT_MPS 299792458.0 #define GPS_PPS_SPEED_OF_LIGHT_MPS 299792458.0
#define GPS_PPS_FILTER_BASELINE_PERIOD_SEC 0.200
static double clamp_double(double value, double limit) static double clamp_double(double value, double limit)
{ {
@@ -34,6 +35,21 @@ static double clamp_double(double value, double limit)
return value; return value;
} }
/* Preserve the baseline discrete pole when the navigation interval changes.
* Applying a baseline gain alpha N times is equivalent to one elapsed-time
* update with alpha_T = 1 - (1 - alpha)^N. This also handles an occasional
* missed solve without slowing the clock-bias or drift estimator. */
static double gps_pps_filter_elapsed_gain(
double baseline_gain, double elapsed_sec)
{
double periods = elapsed_sec / GPS_PPS_FILTER_BASELINE_PERIOD_SEC;
if(fabs(periods - 1.0) <= 1.0e-12)
{
return baseline_gain;
}
return -expm1(periods * log1p(-baseline_gain));
}
void gps_pps_time_filter_init(gps_pps_time_filter_t* filter) void gps_pps_time_filter_init(gps_pps_time_filter_t* filter)
{ {
if(filter != NULL) if(filter != NULL)
@@ -269,14 +285,20 @@ gps_pps_time_filter_result_t gps_pps_time_filter_update(
return GPS_PPS_TIME_FILTER_REJECTED; return GPS_PPS_TIME_FILTER_REJECTED;
} }
double effective_bias_gain =
gps_pps_filter_elapsed_gain(bias_gain, elapsed_sec);
double effective_drift_gain = gps_pps_filter_elapsed_gain(
GPS_PPS_FILTER_DRIFT_GAIN, elapsed_sec);
double effective_fallback_drift_gain = gps_pps_filter_elapsed_gain(
GPS_PPS_FILTER_FALLBACK_DRIFT_GAIN, elapsed_sec);
filter->clock_bias_m = filter->clock_bias_m =
predicted_clock_bias_m + predicted_clock_bias_m +
bias_gain * effective_bias_gain *
clamp_double(innovation_m, GPS_PPS_FILTER_INNOVATION_CLAMP_M); clamp_double(innovation_m, GPS_PPS_FILTER_INNOVATION_CLAMP_M);
if(clock_drift_valid) if(clock_drift_valid)
{ {
filter->clock_drift_mps += filter->clock_drift_mps +=
GPS_PPS_FILTER_DRIFT_GAIN * effective_drift_gain *
(measured_clock_drift_mps - filter->clock_drift_mps); (measured_clock_drift_mps - filter->clock_drift_mps);
} }
else else
@@ -291,7 +313,7 @@ gps_pps_time_filter_result_t gps_pps_time_filter_update(
else else
{ {
filter->clock_drift_mps += filter->clock_drift_mps +=
GPS_PPS_FILTER_FALLBACK_DRIFT_GAIN * effective_fallback_drift_gain *
(fallback_drift_mps - filter->clock_drift_mps); (fallback_drift_mps - filter->clock_drift_mps);
} }
} }
@@ -91,14 +91,11 @@ static void profile_coefficients(
int64_t* ki_q46, int64_t* ki_q46,
int64_t* max_slew_q32) int64_t* max_slew_q32)
{ {
const double pi = 3.14159265358979323846; uint32_t max_slew = 0u;
double wn = 2.0 * pi * profile.natural_frequency_hz; assert(gps_pps_loop_coefficients(
*kp_q46 = (int64_t)llround( profile, UPDATE_PERIOD_SEC, TRACKING_HZ,
(2.0 * profile.damping * wn / TRACKING_HZ) * Q46_SCALE); kp_q46, ki_q46, &max_slew));
*ki_q46 = (int64_t)llround( *max_slew_q32 = max_slew;
(wn * wn * UPDATE_PERIOD_SEC / TRACKING_HZ) * Q46_SCALE);
*max_slew_q32 = (int64_t)llround(
profile.max_slew_ppm * 1.0e-6 * Q32_SCALE);
} }
static void synthetic_rtl_coarse(synthetic_rtl_t* rtl, int64_t reference_q32) static void synthetic_rtl_coarse(synthetic_rtl_t* rtl, int64_t reference_q32)
@@ -107,6 +107,69 @@ static void test_profile_parameters_are_ordered(void)
assert(robust.lock_release_ticks >= precision.lock_release_ticks); assert(robust.lock_release_ticks >= precision.lock_release_ticks);
} }
static void test_period_scaled_hysteresis_and_settling(void)
{
gps_pps_quality_selector_t selector;
gps_pps_quality_selector_init_period(&selector, 0.5);
assert(selector.upgrade_updates == 4u);
assert(selector.degrade_updates == 2u);
gps_pps_loop_profile_t precision =
gps_pps_loop_profile_for_period(GPS_PPS_PROFILE_PRECISION, 0.5);
gps_pps_loop_profile_t robust =
gps_pps_loop_profile_for_period(GPS_PPS_PROFILE_ROBUST, 0.5);
assert(precision.lock_acquire_updates == 2u);
assert(precision.software_settle_updates == 5u);
assert(robust.lock_acquire_updates == 4u);
assert(robust.software_settle_updates == 7u);
gps_pps_quality_selector_init_period(&selector, 1.0);
assert(selector.upgrade_updates == 2u);
assert(selector.degrade_updates == 2u);
precision = gps_pps_loop_profile_for_period(
GPS_PPS_PROFILE_PRECISION, 1.0);
robust = gps_pps_loop_profile_for_period(
GPS_PPS_PROFILE_ROBUST, 1.0);
assert(precision.lock_acquire_updates == 2u);
assert(precision.software_settle_updates == 3u);
assert(robust.lock_acquire_updates == 2u);
assert(robust.software_settle_updates == 4u);
}
static void assert_coefficients(
gps_pps_profile_t profile,
double period_sec,
int64_t expected_kp,
int64_t expected_ki)
{
int64_t kp = 0;
int64_t ki = 0;
uint32_t max_slew = 0u;
gps_pps_loop_profile_t parameters =
gps_pps_loop_profile_for_period(profile, period_sec);
assert(gps_pps_loop_coefficients(
parameters, period_sec, 125000000.0,
&kp, &ki, &max_slew));
assert(kp == expected_kp);
assert(ki == expected_ki);
assert(max_slew == 85899u);
}
static void test_period_scaled_discrete_coefficients(void)
{
/* The production 200 ms coefficients remain bit-for-bit unchanged. */
assert_coefficients(GPS_PPS_PROFILE_ACQUIRE, 0.2, 764018, 64006);
assert_coefficients(GPS_PPS_PROFILE_PRECISION, 0.2, 222838, 5445);
assert_coefficients(GPS_PPS_PROFILE_ROBUST, 0.2, 84891, 640);
/* Other cadences exponentiate those proven discrete poles in time. */
assert_coefficients(GPS_PPS_PROFILE_ACQUIRE, 0.5, 615780, 127492);
assert_coefficients(GPS_PPS_PROFILE_PRECISION, 0.5, 209783, 12804);
assert_coefficients(GPS_PPS_PROFILE_ROBUST, 0.5, 82980, 1564);
assert_coefficients(GPS_PPS_PROFILE_ACQUIRE, 1.0, 447388, 178038);
assert_coefficients(GPS_PPS_PROFILE_PRECISION, 1.0, 190239, 23149);
assert_coefficients(GPS_PPS_PROFILE_ROBUST, 1.0, 79923, 3011);
}
static void test_quality_boundaries_and_recovery(void) static void test_quality_boundaries_and_recovery(void)
{ {
gps_pps_quality_selector_t selector; gps_pps_quality_selector_t selector;
@@ -167,6 +230,8 @@ int main(void)
test_hysteretic_profile_transitions(); test_hysteretic_profile_transitions();
test_alternating_quality_does_not_upgrade(); test_alternating_quality_does_not_upgrade();
test_profile_parameters_are_ordered(); test_profile_parameters_are_ordered();
test_period_scaled_hysteresis_and_settling();
test_period_scaled_discrete_coefficients();
test_quality_boundaries_and_recovery(); test_quality_boundaries_and_recovery();
puts("gps_pps_quality tests passed"); puts("gps_pps_quality tests passed");
return 0; return 0;
@@ -6,6 +6,30 @@
#define COUNTER_HZ 125000000.0 #define COUNTER_HZ 125000000.0
static void test_elapsed_time_gain_preserves_filter_poles(void)
{
gps_pps_time_filter_t filter;
gps_pps_time_filter_init(&filter);
double filtered = 0.0;
double innovation = 0.0;
assert(gps_pps_time_filter_update(
&filter, 0u, COUNTER_HZ, 0.0, true, 0.0, 1.0,
1.0 / 32.0, 1u, &filtered, &innovation) ==
GPS_PPS_TIME_FILTER_COARSE);
assert(gps_pps_time_filter_update(
&filter, (uint64_t)COUNTER_HZ, COUNTER_HZ, 100.0, true, 100.0,
1.0, 1.0 / 32.0, 1u, &filtered, &innovation) ==
GPS_PPS_TIME_FILTER_TRACKING);
/* One 1 s update has exactly the pole of five baseline 200 ms updates. */
double expected_bias_gain = 1.0 - pow(1.0 - 1.0 / 32.0, 5.0);
double expected_drift_gain = 1.0 - pow(1.0 - 1.0 / 4.0, 5.0);
assert(fabs(filtered - 100.0 * expected_bias_gain) < 1.0e-12);
assert(fabs(filter.clock_drift_mps - 100.0 * expected_drift_gain) <
1.0e-12);
}
static void test_drift_prediction_rejects_bias_noise(void) static void test_drift_prediction_rejects_bias_noise(void)
{ {
static const double noise_m[] = { static const double noise_m[] = {
@@ -298,6 +322,7 @@ static void test_reference_selection_is_stable(void)
int main(void) int main(void)
{ {
test_elapsed_time_gain_preserves_filter_poles();
test_drift_prediction_rejects_bias_noise(); test_drift_prediction_rejects_bias_noise();
test_quality_gap_and_outlier_handling(); test_quality_gap_and_outlier_handling();
test_bias_ramp_without_doppler_uses_bounded_fallback(); test_bias_ramp_without_doppler_uses_bounded_fallback();
@@ -80,6 +80,10 @@ that the repository did not capture:
```bash ```bash
python3 -m pps_end_to_end_validation.stability_analysis python3 -m pps_end_to_end_validation.stability_analysis
python3 -m pps_end_to_end_validation.stability_analysis \
--update-period-ms 500
python3 -m pps_end_to_end_validation.stability_analysis \
--update-period-ms 1000
python3 -m unittest \ python3 -m unittest \
pps_end_to_end_validation/test_stability_analysis.py pps_end_to_end_validation/test_stability_analysis.py
``` ```
@@ -89,7 +93,7 @@ Generated results are in `output/stability/`:
- `stability_report.md`: real-data inventory, discrete poles, absolute phase - `stability_report.md`: real-data inventory, discrete poles, absolute phase
wander, period jitter, dynamic parameter sweep, and recommendations; wander, period jitter, dynamic parameter sweep, and recommendations;
- `parameter_sweep.csv`: machine-readable results for every scenario/tuning; - `parameter_sweep.csv`: machine-readable results for every scenario/tuning;
- `synthetic_telemetry.csv`: 5 Hz ABS_SAMPLE, REM_CODE_PHASE, corrected 125 MHz - `synthetic_telemetry.csv`: solve-cadence ABS_SAMPLE, REM_CODE_PHASE, corrected 125 MHz
capture, HDOP/VDOP/TDOP/GDOP, C/N0, pseudorange noise, clock sigma/drift, capture, HDOP/VDOP/TDOP/GDOP, C/N0, pseudorange noise, clock sigma/drift,
oscillator and timing-error inputs. oscillator and timing-error inputs.
@@ -102,8 +106,9 @@ temperature, and trusted-edge timing remain unavailable. The stability CSV
labels replacements as synthetic; it must not be read as board telemetry. labels replacements as synthetic; it must not be read as board telemetry.
The current acquire, precision, and robust fixed-point poles are inside the The current acquire, precision, and robust fixed-point poles are inside the
unit circle at the 200 ms update rate, and the stress analysis observes no unit circle at the 200, 500, and 1000 ms supported update rates, and the stress
unlock. It also exposes an important metrics issue: cycle-to-cycle PPS period analysis observes no unlock. It also exposes an important metrics issue:
cycle-to-cycle PPS period
jitter can remain below 20 ns while absolute phase moves slowly through tens jitter can remain below 20 ns while absolute phase moves slowly through tens
of nanoseconds. A physical 20 ns maximum therefore requires a common-clock of nanoseconds. A physical 20 ns maximum therefore requires a common-clock
time-interval-counter capture and cable/output-delay calibration; a p95 period time-interval-counter capture and cable/output-delay calibration; a p95 period
@@ -112,8 +117,28 @@ jitter assertion or synthetic run cannot certify it.
The sweep includes the current coefficients, quieter/faster precision loops, The sweep includes the current coefficients, quieter/faster precision loops,
a robust-noise guard, a larger Doppler-drift gain, and a quality-scheduled a robust-noise guard, a larger Doppler-drift gain, and a quality-scheduled
candidate. The generated report contains the current numerical comparison. candidate. The generated report contains the current numerical comparison.
The stability analysis does not alter loop constants. Production firmware now Production firmware maps the proven 200 ms discrete poles to the selected NAV
uses the current-RTL-compatible 180 ms epoch correction described below. period and leaves the 200 ms Q2.46 constants bit-identical. The model and
firmware also scale quality/lock/settling timers in wall-clock time, convert
software bias/drift gains from actual elapsed time, and extend the stale-data
watchdog to `max(2 s, 3 * solve period)`. This preserves deterministic settling
and damping; it cannot preserve measurement-noise statistics when fewer WLS
samples are available. Production firmware uses the current-RTL-compatible
180 ms epoch correction independently of solve cadence.
A deterministic four-seed, 180 s smoke sweep with the current profiles gave:
| solve period | real-geometry absolute RMS | worst period step over current scenarios | unlocks |
|---:|---:|---:|---:|
| 200 ms | 11.17 ns | 16 ns | 0 |
| 500 ms | 15.96 ns | 16 ns | 0 |
| 1000 ms | 25.09 ns | 24 ns | 0 |
Thus 1000 ms is linearly stable but is not recommended when the stated
synthetic maximum-period-jitter target is 20 ns. Use 200 ms by default, or
500 ms when CPU load requires it. A 1000 ms solve needs hardware validation
and likely oscillator-temperature/frequency prediction; pole remapping alone
cannot recreate the missing measurements.
## Live board telemetry replay ## Live board telemetry replay
@@ -146,9 +171,13 @@ omits latitude/longitude.
For an already-running board, replay imports the first logged locked For an already-running board, replay imports the first logged locked
filter/PI snapshot. Starting the model from reset would erase the observed filter/PI snapshot. Starting the model from reset would erase the observed
integrator and create a false acquisition transient. When status is decimated, integrator and create a false acquisition transient. When status is decimated,
commit sequence numbers expand the trace back to the nominal 200 ms cadence; commit sequence numbers expand the trace back to the inferred solve cadence;
intermediate values are explicitly marked `interpolated=true`. intermediate values are explicitly marked `interpolated=true`.
The replay infers cadence from UART host time divided by commit-sequence
advance, snapped to the 20 ms navigation-bit grid. Use
`--update-period-ms` only when host timestamps are unreliable.
The report separates three quantities that should not be conflated: The report separates three quantities that should not be conflated:
- direct `hw_phase` statistics from the board; - direct `hw_phase` statistics from the board;
@@ -187,7 +216,9 @@ correction, consistently in both satellite-state and PPS calculations. It is
containing the current eighth bit, firmware must return to 160 ms in the same containing the current eighth bit, firmware must return to 160 ms in the same
coordinated release. coordinated release.
The validation intentionally does not modify production RTL or firmware. The validation intentionally does not modify production RTL. Firmware changes
for period-aware control are exercised by the same coefficient and timing
tests; the 180 ms absolute epoch convention remains unchanged.
## Other regression coverage ## Other regression coverage
@@ -201,16 +232,20 @@ The unit tests verify:
- 180 ms firmware compatibility with current RTL; - 180 ms firmware compatibility with current RTL;
- invariance to synthetic CPU/APB stalls when capture timestamps are used; - invariance to synthetic CPU/APB stalls when capture timestamps are used;
- DUT-versus-trusted output-delay sign; and - DUT-versus-trusted output-delay sign; and
- rejection behavior for the firmware-permitted `NAV_SOLVE_TIMER=40` case. - rejection behavior explaining why firmware now compile-rejects
`NAV_SOLVE_TIMER=40`; and
- zero NAV-solve rejection and unchanged absolute epoch at N=10/25/50
(200/500/1000 ms).
The `N=40` case divides the GPS week but not a 300-bit subframe. When firmware The `N=40` case divides the GPS week but not a 300-bit subframe. When the model
refreshes the HOW/sample anchor after six seconds, the timer phase is half a refreshes the HOW/sample anchor after six seconds, the timer phase is half a
solve period away from that anchor and the current `N * 20 ms` snap rejects solve period away from that anchor and the current `N * 20 ms` snap rejects
otherwise valid solves. Production `N=10` divides 300 and is unaffected. otherwise valid solves. Firmware therefore requires N to divide both the GPS
week and the 300-bit subframe. Production-safe examples are N=10, 25, and 50.
If general timer periods are required, reconstruct elapsed time on the If arbitrary timer periods are required later, reconstruct elapsed time on the
fundamental 20 ms navigation-bit grid. A narrower compatibility restriction is fundamental 20 ms navigation-bit grid instead of snapping through a refreshed
to accept only periods for which `300 % N == 0`. HOW anchor.
## Hardware confirmation ## Hardware confirmation
@@ -10,7 +10,7 @@ PPS edge. This module therefore reports two separate results:
* a telemetry-conditioned replay of the bit-accurate fixed-point loop model, * a telemetry-conditioned replay of the bit-accurate fixed-point loop model,
initialized from the captured locked PI/filter state. initialized from the captured locked PI/filter state.
When ``GPS_PPS_STATUS_LOG_INTERVAL`` is greater than one, missing 200 ms solve When ``GPS_PPS_STATUS_LOG_INTERVAL`` is greater than one, missing solve
epochs are linearly interpolated. The replay reconstructs a self-consistent epochs are linearly interpolated. The replay reconstructs a self-consistent
reference trajectory from the logged filtered bias, PI residual and rate word. reference trajectory from the logged filtered bias, PI residual and rate word.
That preserves measured low-frequency drift but is a lower bound for That preserves measured low-frequency drift but is a lower bound for
@@ -43,6 +43,7 @@ from pps_python.adaptive_pps import (
UPDATE_PERIOD_S = 0.2 UPDATE_PERIOD_S = 0.2
NAV_BIT_PERIOD_S = 0.020
PPS_RE = re.compile( PPS_RE = re.compile(
@@ -286,13 +287,15 @@ def _profile(sample: FirmwarePpsSample) -> Profile:
return Profile.HOLDOVER return Profile.HOLDOVER
def _pi_rate_prediction_ppm(sample: FirmwarePpsSample) -> float: def _pi_rate_prediction_ppm(
sample: FirmwarePpsSample, update_period_s: float
) -> float:
"""Recreate RTL ``integrator + Kp * phase`` from diagnostic fields.""" """Recreate RTL ``integrator + Kp * phase`` from diagnostic fields."""
profile = _profile(sample) profile = _profile(sample)
if profile == Profile.HOLDOVER or sample.mode != "tracking": if profile == Profile.HOLDOVER or sample.mode != "tracking":
return math.nan return math.nan
kp_q46, _, max_slew_q32 = PROFILES[profile].fixed_coefficients( kp_q46, _, max_slew_q32 = PROFILES[profile].fixed_coefficients(
UPDATE_PERIOD_S update_period_s
) )
phase_q32 = round( phase_q32 = round(
sample.hardware_phase_ns * 1.0e-9 * TRACKING_HZ * Q32 sample.hardware_phase_ns * 1.0e-9 * TRACKING_HZ * Q32
@@ -331,7 +334,54 @@ def _parse_int(group: dict[str, str], key: str) -> int:
return int(group[key]) return int(group[key])
def load_firmware_telemetry(path: Path) -> list[FirmwarePpsSample]: def _infer_update_period(
pending: list[dict[str, object]],
override_s: float | None,
) -> float:
if override_s is not None:
if not math.isfinite(override_s) or override_s <= 0.0:
raise ValueError("update period override must be positive")
return override_s
ratios = []
for first, second in zip(pending, pending[1:]):
sequence_step = (
int(second["sequence"]) - int(first["sequence"])
) & 0xFFFF_FFFF
host_step = (
float(second["host_monotonic_s"])
- float(first["host_monotonic_s"])
)
if 0 < sequence_step < 10_000 and host_step > 0.0:
ratios.append(host_step / sequence_step)
if not ratios:
return UPDATE_PERIOD_S
inferred_s = statistics.median(ratios)
snapped_s = max(
NAV_BIT_PERIOD_S,
round(inferred_s / NAV_BIT_PERIOD_S) * NAV_BIT_PERIOD_S,
)
return (
snapped_s
if abs(snapped_s - inferred_s) <= max(0.005, 0.1 * snapped_s)
else inferred_s
)
def _sample_update_period(samples: list[FirmwarePpsSample]) -> float:
ratios = []
for first, second in zip(samples, samples[1:]):
sequence_step = (
second.sequence - first.sequence
) & 0xFFFF_FFFF
elapsed_s = second.time_s - first.time_s
if 0 < sequence_step < 10_000 and elapsed_s > 0.0:
ratios.append(elapsed_s / sequence_step)
return statistics.median(ratios) if ratios else UPDATE_PERIOD_S
def load_firmware_telemetry(
path: Path, *, update_period_s: float | None = None
) -> list[FirmwarePpsSample]:
"""Load host-timestamped UART CSV produced by ``serial_capture.py``.""" """Load host-timestamped UART CSV produced by ``serial_capture.py``."""
pending: list[dict[str, object]] = [] pending: list[dict[str, object]] = []
latest_location: GpsContext | None = None latest_location: GpsContext | None = None
@@ -428,12 +478,13 @@ def load_firmware_telemetry(path: Path) -> list[FirmwarePpsSample]:
if not pending: if not pending:
return [] return []
update_period_s = _infer_update_period(pending, update_period_s)
first_sequence = int(pending[0]["sequence"]) first_sequence = int(pending[0]["sequence"])
samples: list[FirmwarePpsSample] = [] samples: list[FirmwarePpsSample] = []
for fields in pending: for fields in pending:
sequence = int(fields["sequence"]) sequence = int(fields["sequence"])
sequence_delta = (sequence - first_sequence) & 0xFFFF_FFFF sequence_delta = (sequence - first_sequence) & 0xFFFF_FFFF
fields["time_s"] = sequence_delta * UPDATE_PERIOD_S fields["time_s"] = sequence_delta * update_period_s
samples.append(FirmwarePpsSample(**fields)) samples.append(FirmwarePpsSample(**fields))
return samples return samples
@@ -484,11 +535,12 @@ def observed_metrics(samples: list[FirmwarePpsSample]) -> ObservedMetrics:
centered_active = [ centered_active = [
value - active_center for value in inferred_active_phase value - active_center for value in inferred_active_phase
] ]
update_period_s = _sample_update_period(samples)
pi_rate_errors_ppb = [ pi_rate_errors_ppb = [
(prediction - sample.rate_command_ppm) * 1.0e3 (prediction - sample.rate_command_ppm) * 1.0e3
for sample in samples for sample in samples
if math.isfinite( if math.isfinite(
prediction := _pi_rate_prediction_ppm(sample) prediction := _pi_rate_prediction_ppm(sample, update_period_s)
) )
] ]
return ObservedMetrics( return ObservedMetrics(
@@ -592,7 +644,7 @@ def _interpolated_sample(
float(getattr(first, name)), float(getattr(second, name)), fraction float(getattr(first, name)), float(getattr(second, name)), fraction
) )
fields["sequence"] = (first.sequence + step) & 0xFFFF_FFFF fields["sequence"] = (first.sequence + step) & 0xFFFF_FFFF
fields["time_s"] = first.time_s + step * UPDATE_PERIOD_S fields["time_s"] = _lerp(first.time_s, second.time_s, fraction)
fields["pulse_count"] = round( fields["pulse_count"] = round(
_lerp(first.pulse_count, second.pulse_count, fraction) _lerp(first.pulse_count, second.pulse_count, fraction)
) )
@@ -637,6 +689,7 @@ def replay_firmware_telemetry(
if len(samples) < 2: if len(samples) < 2:
raise ValueError("at least two firmware PPS samples are required") raise ValueError("at least two firmware PPS samples are required")
expanded = expand_samples(samples) expanded = expand_samples(samples)
update_period_s = _sample_update_period(samples)
first = expanded[0][0] first = expanded[0][0]
first_profile = _profile(first) first_profile = _profile(first)
if not first.locked or first_profile == Profile.HOLDOVER: if not first.locked or first_profile == Profile.HOLDOVER:
@@ -667,7 +720,7 @@ def replay_firmware_telemetry(
for sample, timing_s in zip(expanded_samples, reference_timing_s) for sample, timing_s in zip(expanded_samples, reference_timing_s)
] ]
model = FixedPointAdaptivePps( model = FixedPointAdaptivePps(
update_period_s=UPDATE_PERIOD_S, update_period_s=update_period_s,
oscillator_ppm=oscillator_ppm, oscillator_ppm=oscillator_ppm,
receiver_clock_bias_m=true_bias_m[0], receiver_clock_bias_m=true_bias_m[0],
receiver_clock_drift_mps=0.0, receiver_clock_drift_mps=0.0,
@@ -736,7 +789,7 @@ def replay_firmware_telemetry(
model.trace.frequency_command_ppm[-1], model.trace.frequency_command_ppm[-1],
) )
) )
trace = model.finish(expanded[-1][0].time_s + UPDATE_PERIOD_S) trace = model.finish(expanded[-1][0].time_s + update_period_s)
edge_errors_ns = [ edge_errors_ns = [
error * 1.0e9 error * 1.0e9
@@ -874,6 +927,7 @@ def write_report(
replay: ReplayMetrics, replay: ReplayMetrics,
sensitivity: list[ReplayMetrics], sensitivity: list[ReplayMetrics],
) -> None: ) -> None:
update_period_s = _sample_update_period(samples)
profiles = sorted({sample.profile for sample in samples}) profiles = sorted({sample.profile for sample in samples})
references = sorted({sample.reference_svid for sample in samples}) references = sorted({sample.reference_svid for sample in samples})
lines = [ lines = [
@@ -886,7 +940,8 @@ def write_report(
f"- Samples/duration: {observed.samples} / {observed.duration_s:.1f} s.", f"- Samples/duration: {observed.samples} / {observed.duration_s:.1f} s.",
f"- Status interval: {observed.status_interval_median_s:.3f} s; " f"- Status interval: {observed.status_interval_median_s:.3f} s; "
f"median sequence step {observed.sequence_step_median:.0f} " f"median sequence step {observed.sequence_step_median:.0f} "
f"({observed.sequence_step_median * UPDATE_PERIOD_S:.1f} s of 200 ms solves).", f"({observed.sequence_step_median * update_period_s:.1f} s of "
f"{update_period_s * 1000.0:.0f} ms solves).",
f"- Profiles: {', '.join(profiles)}; locked fraction " f"- Profiles: {', '.join(profiles)}; locked fraction "
f"{observed.locked_fraction:.3f}; transitions " f"{observed.locked_fraction:.3f}; transitions "
f"{observed.profile_transitions}; reference SVIDs {references}.", f"{observed.profile_transitions}; reference SVIDs {references}.",
@@ -998,7 +1053,8 @@ def write_report(
"", "",
"- Missing status epochs are linearly interpolated when sequence step " "- Missing status epochs are linearly interpolated when sequence step "
"is greater than one. This preserves low-frequency wander but " "is greater than one. This preserves low-frequency wander but "
"understates unlogged 200 ms noise; it is not an exact firmware replay.", f"understates unlogged {update_period_s * 1000.0:.0f} ms noise; it is "
"not an exact firmware replay.",
"- The inferred oscillator and active phase assume one constant crystal " "- The inferred oscillator and active phase assume one constant crystal "
"frequency chosen to make net phase slope zero over this capture. " "frequency chosen to make net phase slope zero over this capture. "
"Temperature telemetry was unavailable, so slow crystal motion is not " "Temperature telemetry was unavailable, so slow crystal motion is not "
@@ -1007,7 +1063,7 @@ def write_report(
"capture counter, chi-square ratio, pseudorange residual vector, board " "capture counter, chi-square ratio, pseudorange residual vector, board "
"temperature, or a trusted PPS edge timestamp. The bit/subframe part " "temperature, or a trusted PPS edge timestamp. The bit/subframe part "
"of the end-to-end model remains synthetic for those fields.", "of the end-to-end model remains synthetic for those fields.",
"- Set `GPS_PPS_STATUS_LOG_INTERVAL=1` for exact 200 ms input cadence. " "- Set `GPS_PPS_STATUS_LOG_INTERVAL=1` for exact solve-cadence input. "
"For a max-20 ns pin claim, capture DUT and trusted PPS with a " "For a max-20 ns pin claim, capture DUT and trusted PPS with a "
"common-clock TIC/scope after cable/output-delay calibration.", "common-clock TIC/scope after cable/output-delay calibration.",
"", "",
@@ -1025,13 +1081,25 @@ def parse_args() -> argparse.Namespace:
default=Path(__file__).resolve().parent / "output/hardware_replay", default=Path(__file__).resolve().parent / "output/hardware_replay",
) )
parser.add_argument("--warmup", type=float, default=20.0) parser.add_argument("--warmup", type=float, default=20.0)
parser.add_argument(
"--update-period-ms",
type=float,
help="override NAV solve period; default infers it from host/sequence",
)
parser.add_argument("--no-write", action="store_true") parser.add_argument("--no-write", action="store_true")
return parser.parse_args() return parser.parse_args()
def main() -> int: def main() -> int:
args = parse_args() args = parse_args()
samples = load_firmware_telemetry(args.input) samples = load_firmware_telemetry(
args.input,
update_period_s=(
args.update_period_ms / 1000.0
if args.update_period_ms is not None
else None
),
)
if len(samples) < 2: if len(samples) < 2:
raise SystemExit("input does not contain at least two firmware PPS records") raise SystemExit("input does not contain at least two firmware PPS records")
observed = observed_metrics(samples) observed = observed_metrics(samples)
@@ -38,6 +38,7 @@ RF_HZ = 16_367_981.0
CODE_FREQ_HZ = 1_023_000.0 CODE_FREQ_HZ = 1_023_000.0
SPEED_OF_LIGHT_MPS = 299_792_458.0 SPEED_OF_LIGHT_MPS = 299_792_458.0
UPDATE_PERIOD_S = 0.2 UPDATE_PERIOD_S = 0.2
NAV_BIT_PERIOD_S = 0.020
BOARD_CLOCK_SIGMA_M = 15.87 BOARD_CLOCK_SIGMA_M = 15.87
BOARD_TDOP = 1.50 BOARD_TDOP = 1.50
BOARD_GDOP = 2.92 BOARD_GDOP = 2.92
@@ -297,14 +298,15 @@ def synthesize_epochs(
*, *,
duration_s: float, duration_s: float,
seed: int, seed: int,
update_period_s: float = UPDATE_PERIOD_S,
) -> list[SyntheticEpoch]: ) -> list[SyntheticEpoch]:
"""Combine real GDOP/satellite count with documented board noise.""" """Combine real GDOP/satellite count with documented board noise."""
rng = random.Random(seed) rng = random.Random(seed)
result: list[SyntheticEpoch] = [] result: list[SyntheticEpoch] = []
count = round(duration_s / UPDATE_PERIOD_S) count = round(duration_s / update_period_s)
rf_initial = (1 << 32) - round(10.25 * RF_HZ) - 1000 rf_initial = (1 << 32) - round(10.25 * RF_HZ) - 1000
for update in range(count): for update in range(count):
time_s = update * UPDATE_PERIOD_S time_s = update * update_period_s
gdop = telemetry.gdop_at(time_s) gdop = telemetry.gdop_at(time_s)
satellites = max(4, min(8, telemetry.tracked_at(time_s))) satellites = max(4, min(8, telemetry.tracked_at(time_s)))
tdop = gdop * (BOARD_TDOP / BOARD_GDOP) tdop = gdop * (BOARD_TDOP / BOARD_GDOP)
@@ -432,8 +434,14 @@ def synthesize_epochs(
return result return result
def _run_loop(epochs: list[SyntheticEpoch], tuning: Tuning): def _run_loop(
epochs: list[SyntheticEpoch],
tuning: Tuning,
*,
update_period_s: float = UPDATE_PERIOD_S,
):
model = FixedPointAdaptivePps( model = FixedPointAdaptivePps(
update_period_s=update_period_s,
oscillator_ppm=BOARD_OSCILLATOR_PPM, oscillator_ppm=BOARD_OSCILLATOR_PPM,
profiles=tuning.profiles, profiles=tuning.profiles,
acquire_profile=tuning.acquire, acquire_profile=tuning.acquire,
@@ -451,7 +459,7 @@ def _run_loop(epochs: list[SyntheticEpoch], tuning: Tuning):
else tuning.drift_gain else tuning.drift_gain
) )
model.update(epoch.observation) model.update(epoch.observation)
finish_s = epochs[-1].time_s + UPDATE_PERIOD_S if epochs else 0.0 finish_s = epochs[-1].time_s + update_period_s if epochs else 0.0
return model.finish(finish_s) return model.finish(finish_s)
@@ -515,12 +523,15 @@ def aggregate(
) )
def discrete_poles(profile: LoopProfile) -> tuple[complex, complex]: def discrete_poles(
profile: LoopProfile,
update_period_s: float = UPDATE_PERIOD_S,
) -> tuple[complex, complex]:
"""Poles of the exact sample/commit ordering, before slew saturation.""" """Poles of the exact sample/commit ordering, before slew saturation."""
kp_q46, ki_q46, _ = profile.fixed_coefficients(UPDATE_PERIOD_S) kp_q46, ki_q46, _ = profile.fixed_coefficients(update_period_s)
kp = kp_q46 / (1 << 46) kp = kp_q46 / (1 << 46)
ki = ki_q46 / (1 << 46) ki = ki_q46 / (1 << 46)
ticks = UPDATE_PERIOD_S * TRACKING_HZ ticks = update_period_s * TRACKING_HZ
a = 1.0 - ticks * (kp + ki) a = 1.0 - ticks * (kp + ki)
b = ticks b = ticks
c = -ki c = -ki
@@ -573,6 +584,7 @@ def write_report(
telemetry: RealTelemetry, telemetry: RealTelemetry,
results: list[Aggregate], results: list[Aggregate],
epoch_result, epoch_result,
update_period_s: float = UPDATE_PERIOD_S,
) -> None: ) -> None:
gdop = telemetry.gdop gdop = telemetry.gdop
tracked = telemetry.tracked tracked = telemetry.tracked
@@ -657,7 +669,7 @@ def write_report(
"## Linear stability", "## Linear stability",
"", "",
"The fixed-point coefficients give the following discrete poles " "The fixed-point coefficients give the following discrete poles "
"at the 200 ms navigation update rate:", f"at the {update_period_s * 1000.0:.0f} ms navigation update rate:",
"", "",
"| profile | natural frequency | damping | poles | max magnitude |", "| profile | natural frequency | damping | poles | max magnitude |",
"|---|---:|---:|---|---:|", "|---|---:|---:|---|---:|",
@@ -671,7 +683,7 @@ def write_report(
) )
] ]
for name, profile in profile_rows: for name, profile in profile_rows:
poles = discrete_poles(profile) poles = discrete_poles(profile, update_period_s)
pole_text = ", ".join( pole_text = ", ".join(
f"{pole.real:.6f}{pole.imag:+.6f}j" for pole in poles f"{pole.real:.6f}{pole.imag:+.6f}j" for pole in poles
) )
@@ -687,9 +699,11 @@ def write_report(
"integrator, and the RTL snapshots Kp/Ki/max-slew/lock limits " "integrator, and the RTL snapshots Kp/Ki/max-slew/lock limits "
"together with the next commit, so the current acquire/precision/" "together with the next commit, so the current acquire/precision/"
"robust switching is structurally sound. Stable individual modes " "robust switching is structurally sound. Stable individual modes "
"do not prove arbitrary-switching stability, but the asymmetric " "do not prove arbitrary-switching stability, but the baseline "
"8-update upgrade / 3-update downgrade hysteresis and the stress " "8-update upgrade / 3-update downgrade hysteresis and the stress "
"runs show no coefficient chatter or unlock.", "runs show no coefficient chatter or unlock. Hysteresis and "
"settling counts are scaled to preserve their wall-clock duration "
"with at least two/three independent updates.",
"", "",
"## Dynamic parameter sweep", "## Dynamic parameter sweep",
"", "",
@@ -784,6 +798,12 @@ def parse_args() -> argparse.Namespace:
parser = argparse.ArgumentParser(description=__doc__) parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--duration", type=float, default=300.0) parser.add_argument("--duration", type=float, default=300.0)
parser.add_argument("--seeds", type=int, default=24) parser.add_argument("--seeds", type=int, default=24)
parser.add_argument(
"--update-period-ms",
type=float,
default=UPDATE_PERIOD_S * 1000.0,
help="NAV solve period (production-safe examples: 200, 500, 1000)",
)
parser.add_argument( parser.add_argument(
"--output-dir", "--output-dir",
type=Path, type=Path,
@@ -797,6 +817,19 @@ def main() -> int:
args = parse_args() args = parse_args()
if args.duration < 120.0 or args.seeds < 1: if args.duration < 120.0 or args.seeds < 1:
raise SystemExit("duration must be >= 120 s and seeds must be positive") raise SystemExit("duration must be >= 120 s and seeds must be positive")
update_period_s = args.update_period_ms / 1000.0
nav_ticks = round(update_period_s / NAV_BIT_PERIOD_S)
if (
not math.isfinite(update_period_s)
or update_period_s <= 0.0
or abs(update_period_s - nav_ticks * NAV_BIT_PERIOD_S) > 1.0e-12
or nav_ticks <= 0
or 300 % nav_ticks != 0
):
raise SystemExit(
"update period must be N*20 ms with positive N dividing "
"the 300-bit subframe"
)
software_dir = Path(__file__).resolve().parents[1] software_dir = Path(__file__).resolve().parents[1]
telemetry = load_real_telemetry(software_dir) telemetry = load_real_telemetry(software_dir)
if not telemetry.gdop or not telemetry.tracked: if not telemetry.gdop or not telemetry.tracked:
@@ -818,6 +851,7 @@ def main() -> int:
scenario, scenario,
duration_s=args.duration, duration_s=args.duration,
seed=seed, seed=seed,
update_period_s=update_period_s,
) )
for seed in range(1, args.seeds + 1) for seed in range(1, args.seeds + 1)
] ]
@@ -826,7 +860,12 @@ def main() -> int:
for tuning in tuning_matrix(): for tuning in tuning_matrix():
runs = [ runs = [
measure_trace( measure_trace(
_run_loop(epochs, tuning), after_s=analysis_start _run_loop(
epochs,
tuning,
update_period_s=update_period_s,
),
after_s=analysis_start,
) )
for epochs in seed_epochs for epochs in seed_epochs
] ]
@@ -858,6 +897,7 @@ def main() -> int:
telemetry, telemetry,
results, results,
epoch_result, epoch_result,
update_period_s,
) )
print(f"wrote {args.output_dir}") print(f"wrote {args.output_dir}")
return 0 return 0
@@ -23,11 +23,16 @@ from pps_end_to_end_validation.hardware_replay import (
) )
def _status_line(index: int) -> str: def _status_line(
index: int,
*,
update_period_s: float = UPDATE_PERIOD_S,
log_interval_updates: int = 10,
) -> str:
phase_ns = 140.0 + 15.0 * math.sin(index * 0.7) phase_ns = 140.0 + 15.0 * math.sin(index * 0.7)
integrator_ppm = -1.560 integrator_ppm = -1.560
kp_q46, _, _ = PROFILES[Profile.ROBUST].fixed_coefficients( kp_q46, _, _ = PROFILES[Profile.ROBUST].fixed_coefficients(
UPDATE_PERIOD_S update_period_s
) )
phase_q32 = round(phase_ns * 1.0e-9 * TRACKING_HZ * Q32) phase_q32 = round(phase_ns * 1.0e-9 * TRACKING_HZ * Q32)
proportional_q32 = (phase_q32 * kp_q46) >> 46 proportional_q32 = (phase_q32 * kp_q46) >> 46
@@ -46,11 +51,17 @@ def _status_line(index: int) -> str:
"pps_v=0.100/0.080/0x00 ref_svid=23 capture_corr=32.00ns " "pps_v=0.100/0.080/0x00 ref_svid=23 capture_corr=32.00ns "
f"hw_phase={phase_ns:.2f}ns rate_cmd={rate_ppm:.3f}ppm " f"hw_phase={phase_ns:.2f}ns rate_cmd={rate_ppm:.3f}ppm "
f"integ={integrator_ppm:.3f}ppm pulse={100 + 2 * index} " f"integ={integrator_ppm:.3f}ppm pulse={100 + 2 * index} "
f"seq={1000 + 10 * index} status=0x80f" f"seq={1000 + log_interval_updates * index} status=0x80f"
) )
def _write_capture(path: Path, count: int = 16) -> None: def _write_capture(
path: Path,
count: int = 16,
*,
update_period_s: float = UPDATE_PERIOD_S,
log_interval_updates: int = 10,
) -> None:
location = ( location = (
"[LOCATION] source=gps lat=0 lon=0 height=0 " "[LOCATION] source=gps lat=0 lon=0 height=0 "
"gdop=2.82 pdop=2.54 hdop=1.81 vdop=1.78 tdop=1.22 " "gdop=2.82 pdop=2.54 hdop=1.81 vdop=1.78 tdop=1.22 "
@@ -61,9 +72,17 @@ def _write_capture(path: Path, count: int = 16) -> None:
writer = csv.writer(output, lineterminator="\n") writer = csv.writer(output, lineterminator="\n")
writer.writerow(("host_monotonic_s", "host_unix_s", "uart_line")) writer.writerow(("host_monotonic_s", "host_unix_s", "uart_line"))
for index in range(count): for index in range(count):
host_s = 2.0 * index host_s = update_period_s * log_interval_updates * index
writer.writerow((host_s - 0.01, 1000.0 + host_s, location)) writer.writerow((host_s - 0.01, 1000.0 + host_s, location))
writer.writerow((host_s, 1000.01 + host_s, _status_line(index))) writer.writerow((
host_s,
1000.01 + host_s,
_status_line(
index,
update_period_s=update_period_s,
log_interval_updates=log_interval_updates,
),
))
class HardwareReplayTests(unittest.TestCase): class HardwareReplayTests(unittest.TestCase):
@@ -99,6 +118,21 @@ class HardwareReplayTests(unittest.TestCase):
self.assertTrue(math.isfinite(replay.modeled_edge_centered_rms_ns)) self.assertTrue(math.isfinite(replay.modeled_edge_centered_rms_ns))
self.assertGreater(replay.modeled_edge_centered_rms_ns, 0.0) self.assertGreater(replay.modeled_edge_centered_rms_ns, 0.0)
def test_one_second_cadence_is_inferred_from_uart_sequence(self):
_write_capture(
self.capture,
update_period_s=1.0,
log_interval_updates=2,
)
samples = load_firmware_telemetry(self.capture)
self.assertEqual(samples[-1].time_s, 30.0)
self.assertEqual(len(expand_samples(samples)), 31)
observed = observed_metrics(samples)
self.assertLess(observed.pi_rate_reproduction_rms_ppb, 0.6)
_, replay = replay_firmware_telemetry(samples, warmup_s=5.0)
self.assertEqual(replay.expanded_updates, 31)
self.assertEqual(replay.unlocks, 0)
def test_tuning_sensitivity_keeps_candidates_separate(self): def test_tuning_sensitivity_keeps_candidates_separate(self):
samples = load_firmware_telemetry(self.capture) samples = load_firmware_telemetry(self.capture)
results = replay_tuning_sensitivity(samples, warmup_s=5.0) results = replay_tuning_sensitivity(samples, warmup_s=5.0)
@@ -81,11 +81,10 @@ class PpsEndToEndTests(unittest.TestCase):
self.assertGreater(result.nav_solve_interrupts, 20) self.assertGreater(result.nav_solve_interrupts, 20)
self.assertEqual(result.firmware_rejected_nav_solves, 0) self.assertEqual(result.firmware_rejected_nav_solves, 0)
def test_permitted_40_bit_period_rejects_after_how_anchor_moves(self): def test_non_subframe_divisor_explains_40_bit_build_restriction(self):
# Firmware's compile-time rule accepts 40 because it divides the GPS # N=40 divides the GPS week but not a 300-bit subframe. Refreshing the
# week. It does not divide a 300-bit subframe, so refreshing the HOW # HOW sample anchor changes timer phase by half a solve period, so the
# sample anchor changes timer phase by half a solve period. The # model rejects solves. Firmware now prevents this configuration.
# current N*20 ms snap then rejects otherwise valid capture events.
self.assertEqual(30_240_000 % 40, 0) self.assertEqual(30_240_000 % 40, 0)
self.assertNotEqual(300 % 40, 0) self.assertNotEqual(300 % 40, 0)
result = run_end_to_end( result = run_end_to_end(
@@ -93,6 +92,23 @@ class PpsEndToEndTests(unittest.TestCase):
) )
self.assertGreater(result.firmware_rejected_nav_solves, 0) self.assertGreater(result.firmware_rejected_nav_solves, 0)
def test_supported_200_500_1000ms_periods_keep_absolute_epoch(self):
for period_bits in (10, 25, 50):
result = run_end_to_end(
EndToEndConfig(
nav_solve_period_bits=period_bits,
subframes=4,
)
)
self.assertEqual(result.firmware_rejected_nav_solves, 0)
self.assertAlmostEqual(
result.expected_semantic_offset_ms, 0.0, places=9
)
self.assertLess(
abs(result.pps.edges[0].dut_minus_trusted_s),
100.0e-9,
)
def test_current_rtl_and_160ms_correction_make_pps_20ms_late(self): def test_current_rtl_and_160ms_correction_make_pps_20ms_late(self):
result = run_end_to_end(EndToEndConfig(postpone_tow_s=0.160)) result = run_end_to_end(EndToEndConfig(postpone_tow_s=0.160))
offset_s = result.pps.edges[0].dut_minus_trusted_s offset_s = result.pps.edges[0].dut_minus_trusted_s
@@ -40,8 +40,23 @@ class StabilityAnalysisTests(unittest.TestCase):
PROFILES[Profile.PRECISION], PROFILES[Profile.PRECISION],
PROFILES[Profile.ROBUST], PROFILES[Profile.ROBUST],
): ):
poles = discrete_poles(profile) baseline_poles = discrete_poles(profile)
self.assertTrue(all(abs(pole) < 1.0 for pole in poles)) self.assertTrue(all(abs(pole) < 1.0 for pole in baseline_poles))
for period_s in (0.5, 1.0):
mapped_poles = discrete_poles(profile, period_s)
expected_poles = tuple(
pole ** (period_s / 0.2) for pole in baseline_poles
)
self.assertTrue(all(abs(pole) < 1.0 for pole in mapped_poles))
self.assertLess(
max(
abs(actual - expected)
for actual, expected in zip(
mapped_poles, expected_poles
)
),
1.0e-4,
)
def test_synthetic_telemetry_exercises_wrap_and_physical_fields(self): def test_synthetic_telemetry_exercises_wrap_and_physical_fields(self):
epochs = synthesize_epochs( epochs = synthesize_epochs(
@@ -79,6 +94,25 @@ class StabilityAnalysisTests(unittest.TestCase):
metrics.period_max_ns, metrics.period_max_ns,
) )
def test_current_loop_remains_stable_at_supported_slower_cadences(self):
for period_s in (0.5, 1.0):
epochs = synthesize_epochs(
self.telemetry,
scenario_matrix()[0],
duration_s=180.0,
seed=4,
update_period_s=period_s,
)
trace = _run_loop(
epochs,
tuning_matrix()[0],
update_period_s=period_s,
)
metrics = measure_trace(trace, after_s=60.0)
self.assertLess(metrics.first_lock_s, 15.0)
self.assertEqual(metrics.unlocks, 0)
self.assertLessEqual(metrics.period_max_ns, 16.0 + 1.0e-9)
if __name__ == "__main__": if __name__ == "__main__":
unittest.main() unittest.main()
+61 -2
View File
@@ -9,7 +9,8 @@ davranışlarını taklit eder:
- RF `ABS_SAMPLE` sayacı: `16.367981 MHz`, 32 bit ve wrap eden sayaç. - RF `ABS_SAMPLE` sayacı: `16.367981 MHz`, 32 bit ve wrap eden sayaç.
- Tracking/PPS sayacı: `125 MHz` free-running sayaç. - Tracking/PPS sayacı: `125 MHz` free-running sayaç.
- Navigation solve periyodu: `NAV_SOLVE_TIMER=10`, yani `200 ms`. - Varsayılan navigation solve periyodu: `NAV_SOLVE_TIMER=10`, yani `200 ms`;
period-aware loop doğrulaması ayrıca 500 ve 1000 ms'yi kapsar.
- Ortak navigation bit transmit epoch'u ve uyduların farklı propagation - Ortak navigation bit transmit epoch'u ve uyduların farklı propagation
delay'lerle art arda tracking olayı üretmesi. delay'lerle art arda tracking olayı üretmesi.
- Firmware'deki corrected sample denklemi: - Firmware'deki corrected sample denklemi:
@@ -239,7 +240,7 @@ PI, raw 125 MHz sayacı değiştirmez. Ayrı GPS time accumulator'ın rate word'
ayarlar: ayarlar:
```text ```text
integrator += Ki * phase_error * update_period integrator += Ki_discrete * phase_error
rate_command = integrator + Kp * phase_error rate_command = integrator + Kp * phase_error
``` ```
@@ -267,6 +268,64 @@ olması gerekir. Üretim profilleri tek seyrek WLS sıçramasında fiziksel puls
kaçırmamak için lock'u ancak `128 tick = 1.024 us` üstünde bırakır; watchdog, kaçırmamak için lock'u ancak `128 tick = 1.024 us` üstünde bırakır; watchdog,
quality holdover ve slew clamp daha büyük arızaları ayrıca sınırlar. quality holdover ve slew clamp daha büyük arızaları ayrıca sınırlar.
### NAV solve periyodu değiştiğinde loop dinamiği
`Ki` zaten ayrık-zaman katsayısıdır; aynı sayıyı 200, 500 ve 1000 ms'de
kullanmak aynı loop değildir. Basitçe `Ki`'yi periyotla çarpmak da uzun
periyotlarda mevcut RTL'nin gerçek kutuplarını yeterince doğru korumaz.
Firmware ve Python model şu uyumlu yöntemi kullanır:
1. Kartta doğrulanmış 200 ms Q2.46 katsayılarından RTL'nin iki ayrık kutbu
çıkarılır.
2. Yeni `T` periyodu için her kutup `T / 0.2` kuvvetine yükseltilir.
3. Hedef kutuplardan, RTL'nin önce-integral-sonra-proportional commit sırasına
uygun yeni `Kp` ve `Ki` geri hesaplanır.
Bu yöntem 200 ms'de eski katsayıları bit düzeyinde aynen üretir; 500 ve
1000 ms'de aynı gerçek-zaman damping ve settling davranışını korur:
| NAV periyodu | profil | Kp Q2.46 | Ki Q2.46 |
|---:|---|---:|---:|
| 200 ms | acquire / precision / robust | 764018 / 222838 / 84891 | 64006 / 5445 / 640 |
| 500 ms | acquire / precision / robust | 615780 / 209783 / 82980 | 127492 / 12804 / 1564 |
| 1000 ms | acquire / precision / robust | 447388 / 190239 / 79923 | 178038 / 23149 / 3011 |
Toplam döngü yalnız RTL PI'den oluşmadığı için yazılım clock-bias, Doppler
drift ve fallback drift kazançları da gerçek geçen süreyle
`alpha_T = 1 - (1 - alpha_200ms)^(T/0.2)` biçiminde uygulanır. Quality/profile
histerezisi, lock ve filter-settle sayaçları saniye cinsinden ölçeklenir;
bağımsız örnek güvenliği için sırasıyla en az 2/2/3 update korunur. Reference
watchdog `max(2 s, 3*T)` olur.
Desteklenen build değeri `GPS_NAV_SOLVE_TIMER_TICKS=N` için N, 300 bitlik
LNAV subframe'i bölmelidir. Doğrulanan örnekler N=10, 25 ve 50'dir. N=40,
HOW anchor yenilendiğinde timer fazını kaydırdığı için compile-time reddedilir.
`POSTPONE_TOW=180 ms` NAV solve periyodundan bağımsızdır ve değişmez.
Örnek firmware build'leri:
```bash
make -C gps_system RISCV_TOOLCHAIN=/tools/toolchains/rvtc-medany \
GPS_NAV_SOLVE_TIMER_TICKS=10 all # 200 ms, varsayılan
make -C gps_system RISCV_TOOLCHAIN=/tools/toolchains/rvtc-medany \
GPS_NAV_SOLVE_TIMER_TICKS=25 all # 500 ms
make -C gps_system RISCV_TOOLCHAIN=/tools/toolchains/rvtc-medany \
GPS_NAV_SOLVE_TIMER_TICKS=50 all # 1000 ms
```
Linux stability sweep'i aynı periyotları doğrudan kabul eder:
```bash
python3 -m pps_end_to_end_validation.stability_analysis \
--update-period-ms 500
```
Kutup eşlemesi deterministik loop dinamiğini korur; daha seyrek WLS ölçümü
aynı gürültü performansını garanti etmez. Örnek sayısı azalınca aliasing,
outlier yakalama gecikmesi ve mutlak faz wander'ı değişebilir. Bu nedenle her
kart/periyot kombinasyonu trusted PPS ile ayrıca ölçülmelidir.
## Adaptif PPS senaryo matrisi ## Adaptif PPS senaryo matrisi
Tam fixed-point/holdover matrisi: Tam fixed-point/holdover matrisi:
+129 -16
View File
@@ -8,7 +8,7 @@ rounding, lock acquisition/release and holdover/re-acquisition.
from __future__ import annotations from __future__ import annotations
from dataclasses import dataclass from dataclasses import dataclass, replace
from enum import IntEnum from enum import IntEnum
import math import math
from typing import Iterable, Mapping from typing import Iterable, Mapping
@@ -16,7 +16,9 @@ from typing import Iterable, Mapping
TRACKING_HZ = 125_000_000 TRACKING_HZ = 125_000_000
SPEED_OF_LIGHT_MPS = 299_792_458.0 SPEED_OF_LIGHT_MPS = 299_792_458.0
REFERENCE_WATCHDOG_SEC = 2.0 BASELINE_UPDATE_PERIOD_S = 0.2
MIN_REFERENCE_WATCHDOG_SEC = 2.0
REFERENCE_WATCHDOG_PERIODS = 3.0
MAX_PREDICTION_GAP_SEC = 30.0 MAX_PREDICTION_GAP_SEC = 30.0
Q32 = 1 << 32 Q32 = 1 << 32
Q46 = 1 << 46 Q46 = 1 << 46
@@ -24,6 +26,27 @@ MAX_SIGNED_64 = (1 << 63) - 1
MIN_SIGNED_64 = -(1 << 63) MIN_SIGNED_64 = -(1 << 63)
def _period_updates(
baseline_updates: int,
update_period_s: float,
minimum_updates: int,
) -> int:
if not math.isfinite(update_period_s) or update_period_s <= 0.0:
raise ValueError("update_period_s must be finite and positive")
duration_s = baseline_updates * BASELINE_UPDATE_PERIOD_S
return min(
255,
max(minimum_updates, math.ceil(duration_s / update_period_s - 1e-12)),
)
def _elapsed_gain(baseline_gain: float, elapsed_s: float) -> float:
periods = elapsed_s / BASELINE_UPDATE_PERIOD_S
if abs(periods - 1.0) <= 1e-12:
return baseline_gain
return -math.expm1(periods * math.log1p(-baseline_gain))
class Profile(IntEnum): class Profile(IntEnum):
HOLDOVER = 0 HOLDOVER = 0
ROBUST = 1 ROBUST = 1
@@ -58,14 +81,74 @@ class LoopProfile:
software_settle_updates: int software_settle_updates: int
def fixed_coefficients(self, update_period_s: float) -> tuple[int, int, int]: def fixed_coefficients(self, update_period_s: float) -> tuple[int, int, int]:
"""Map the proven 200 ms discrete poles to ``update_period_s``.
The 200 ms result is exactly the legacy production coefficient set.
At another NAV cadence each baseline pole is raised to T/0.2 before
recovering Kp/Ki for the RTL's integrate-then-proportional ordering.
"""
if not math.isfinite(update_period_s) or update_period_s <= 0.0:
raise ValueError("update_period_s must be finite and positive")
omega = 2.0 * math.pi * self.natural_frequency_hz omega = 2.0 * math.pi * self.natural_frequency_hz
kp_q46 = round((2.0 * self.damping * omega / TRACKING_HZ) * Q46) baseline_kp = math.floor(
ki_q46 = round( (2.0 * self.damping * omega / TRACKING_HZ) * Q46 + 0.5
(omega * omega * update_period_s / TRACKING_HZ) * Q46 ) / Q46
baseline_ki = math.floor(
(
omega * omega * BASELINE_UPDATE_PERIOD_S / TRACKING_HZ
) * Q46 + 0.5
) / Q46
baseline_ticks = TRACKING_HZ * BASELINE_UPDATE_PERIOD_S
baseline_sum = 2.0 - baseline_ticks * (
baseline_kp + baseline_ki
)
baseline_product = 1.0 - baseline_ticks * baseline_kp
discriminant = baseline_sum * baseline_sum - 4.0 * baseline_product
period_ratio = update_period_s / BASELINE_UPDATE_PERIOD_S
if discriminant >= 0.0:
root = math.sqrt(discriminant)
pole_a = 0.5 * (baseline_sum + root)
pole_b = 0.5 * (baseline_sum - root)
if pole_a < 0.0 or pole_b < 0.0:
raise ValueError("baseline loop has an unmappable negative pole")
target_a = pole_a**period_ratio
target_b = pole_b**period_ratio
target_sum = target_a + target_b
target_product = target_a * target_b
else:
if baseline_product <= 0.0:
raise ValueError("baseline loop has invalid complex poles")
radius = math.sqrt(baseline_product)
cosine = max(-1.0, min(1.0, baseline_sum / (2.0 * radius)))
angle = math.acos(cosine)
target_radius = radius**period_ratio
target_sum = 2.0 * target_radius * math.cos(
angle * period_ratio
)
target_product = target_radius * target_radius
target_ticks = TRACKING_HZ * update_period_s
kp = (1.0 - target_product) / target_ticks
ki = (1.0 - target_sum + target_product) / target_ticks
if not math.isfinite(kp) or not math.isfinite(ki) or kp < 0 or ki < 0:
raise ValueError("mapped loop coefficients are invalid")
kp_q46 = math.floor(kp * Q46 + 0.5)
ki_q46 = math.floor(ki * Q46 + 0.5)
max_slew_q32 = math.floor(
self.max_slew_ppm * 1.0e-6 * Q32 + 0.5
) )
max_slew_q32 = round(self.max_slew_ppm * 1.0e-6 * Q32)
return kp_q46, ki_q46, max_slew_q32 return kp_q46, ki_q46, max_slew_q32
def for_update_period(self, update_period_s: float) -> LoopProfile:
return replace(
self,
lock_acquire_updates=_period_updates(
self.lock_acquire_updates, update_period_s, 2
),
software_settle_updates=_period_updates(
self.software_settle_updates, update_period_s, 3
),
)
PROFILES = { PROFILES = {
Profile.HOLDOVER: LoopProfile( Profile.HOLDOVER: LoopProfile(
@@ -149,10 +232,12 @@ def classify_quality(metrics: QualityMetrics) -> Profile:
class QualitySelector: class QualitySelector:
"""Three-profile selector with asymmetric transition hysteresis.""" """Three-profile selector with asymmetric transition hysteresis."""
def __init__(self) -> None: def __init__(self, update_period_s: float = BASELINE_UPDATE_PERIOD_S) -> None:
self.profile = Profile.HOLDOVER self.profile = Profile.HOLDOVER
self.candidate = Profile.HOLDOVER self.candidate = Profile.HOLDOVER
self.candidate_updates = 0 self.candidate_updates = 0
self.upgrade_updates = _period_updates(8, update_period_s, 2)
self.degrade_updates = _period_updates(3, update_period_s, 2)
def update(self, metrics: QualityMetrics) -> tuple[Profile, bool]: def update(self, metrics: QualityMetrics) -> tuple[Profile, bool]:
target = classify_quality(metrics) target = classify_quality(metrics)
@@ -165,7 +250,11 @@ class QualitySelector:
self.candidate_updates = 1 self.candidate_updates = 1
else: else:
self.candidate_updates += 1 self.candidate_updates += 1
required = 8 if target > self.profile else 3 required = (
self.upgrade_updates
if target > self.profile
else self.degrade_updates
)
if self.candidate_updates >= required: if self.candidate_updates >= required:
self.profile = target self.profile = target
self.candidate_updates = 0 self.candidate_updates = 0
@@ -339,19 +428,28 @@ class FixedPointAdaptivePps:
software_drift_gain: float = 1.0 / 4.0, software_drift_gain: float = 1.0 / 4.0,
software_fallback_drift_gain: float = 1.0 / 16.0, software_fallback_drift_gain: float = 1.0 / 16.0,
) -> None: ) -> None:
if not math.isfinite(update_period_s) or update_period_s <= 0.0:
raise ValueError("update_period_s must be finite and positive")
self.update_period_s = update_period_s self.update_period_s = update_period_s
self.oscillator_fraction = oscillator_ppm * 1.0e-6 self.oscillator_fraction = oscillator_ppm * 1.0e-6
self.adaptive = adaptive self.adaptive = adaptive
self.software_filter = software_filter self.software_filter = software_filter
self.receiver_clock_bias_m = receiver_clock_bias_m self.receiver_clock_bias_m = receiver_clock_bias_m
self.receiver_clock_drift_mps = receiver_clock_drift_mps self.receiver_clock_drift_mps = receiver_clock_drift_mps
self.profiles = dict(PROFILES if profiles is None else profiles) baseline_profiles = dict(PROFILES if profiles is None else profiles)
self.profiles = {
profile: parameters.for_update_period(update_period_s)
for profile, parameters in baseline_profiles.items()
}
missing_profiles = set(Profile) - self.profiles.keys() missing_profiles = set(Profile) - self.profiles.keys()
if missing_profiles: if missing_profiles:
raise ValueError(f"missing loop profiles: {sorted(missing_profiles)}") raise ValueError(f"missing loop profiles: {sorted(missing_profiles)}")
self.acquire_profile = ( baseline_acquire_profile = (
ACQUIRE_PROFILE if acquire_profile is None else acquire_profile ACQUIRE_PROFILE if acquire_profile is None else acquire_profile
) )
self.acquire_profile = baseline_acquire_profile.for_update_period(
update_period_s
)
if not 0.0 < software_drift_gain <= 1.0: if not 0.0 < software_drift_gain <= 1.0:
raise ValueError("software_drift_gain must be in (0, 1]") raise ValueError("software_drift_gain must be in (0, 1]")
if not 0.0 < software_fallback_drift_gain <= 1.0: if not 0.0 < software_fallback_drift_gain <= 1.0:
@@ -360,7 +458,11 @@ class FixedPointAdaptivePps:
) )
self.software_drift_gain = software_drift_gain self.software_drift_gain = software_drift_gain
self.software_fallback_drift_gain = software_fallback_drift_gain self.software_fallback_drift_gain = software_fallback_drift_gain
self.selector = QualitySelector() self.selector = QualitySelector(update_period_s)
self.reference_watchdog_sec = max(
MIN_REFERENCE_WATCHDOG_SEC,
REFERENCE_WATCHDOG_PERIODS * update_period_s,
)
self.profile = Profile.HOLDOVER if adaptive else Profile.PRECISION self.profile = Profile.HOLDOVER if adaptive else Profile.PRECISION
self.parameters = self.profiles[self.profile] self.parameters = self.profiles[self.profile]
self.kp_q46, self.ki_q46, self.max_slew_q32 = ( self.kp_q46, self.ki_q46, self.max_slew_q32 = (
@@ -652,12 +754,21 @@ class FixedPointAdaptivePps:
return False, False return False, False
innovation_m = max(-300.0, min(300.0, innovation_m)) innovation_m = max(-300.0, min(300.0, innovation_m))
effective_bias_gain = _elapsed_gain(
self.parameters.software_bias_gain, interval_s
)
effective_drift_gain = _elapsed_gain(
self.software_drift_gain, interval_s
)
effective_fallback_drift_gain = _elapsed_gain(
self.software_fallback_drift_gain, interval_s
)
self.software_bias_m = ( self.software_bias_m = (
predicted_bias_m predicted_bias_m
+ self.parameters.software_bias_gain * innovation_m + effective_bias_gain * innovation_m
) )
if observation.clock_drift_valid: if observation.clock_drift_valid:
self.software_clock_drift_mps += self.software_drift_gain * ( self.software_clock_drift_mps += effective_drift_gain * (
measured_drift_mps - self.software_clock_drift_mps measured_drift_mps - self.software_clock_drift_mps
) )
else: else:
@@ -665,7 +776,7 @@ class FixedPointAdaptivePps:
self.software_clock_drift_mps = fallback_drift_mps self.software_clock_drift_mps = fallback_drift_mps
else: else:
self.software_clock_drift_mps += ( self.software_clock_drift_mps += (
self.software_fallback_drift_gain * effective_fallback_drift_gain *
(fallback_drift_mps - self.software_clock_drift_mps) (fallback_drift_mps - self.software_clock_drift_mps)
) )
self.software_previous_measurement_m = measured_bias_m self.software_previous_measurement_m = measured_bias_m
@@ -731,7 +842,7 @@ class FixedPointAdaptivePps:
if ( if (
self.last_observation_time_s is None self.last_observation_time_s is None
or observation.time_s - self.last_observation_time_s or observation.time_s - self.last_observation_time_s
> REFERENCE_WATCHDOG_SEC > self.reference_watchdog_sec
): ):
self.reference_watchdog_holdover = True self.reference_watchdog_holdover = True
self._record(observation, 0, 0) self._record(observation, 0, 0)
@@ -747,7 +858,7 @@ class FixedPointAdaptivePps:
if ( if (
self.last_observation_time_s is None self.last_observation_time_s is None
or observation.time_s - self.last_observation_time_s or observation.time_s - self.last_observation_time_s
> REFERENCE_WATCHDOG_SEC > self.reference_watchdog_sec
): ):
self.reference_watchdog_holdover = True self.reference_watchdog_holdover = True
self._record(observation, 0, 0) self._record(observation, 0, 0)
@@ -817,6 +928,7 @@ def run_observations(
observations: Iterable[Observation], observations: Iterable[Observation],
*, *,
finish_time_s: float, finish_time_s: float,
update_period_s: float = BASELINE_UPDATE_PERIOD_S,
oscillator_ppm: float = 1.65, oscillator_ppm: float = 1.65,
adaptive: bool = True, adaptive: bool = True,
software_filter: bool = True, software_filter: bool = True,
@@ -828,6 +940,7 @@ def run_observations(
software_fallback_drift_gain: float = 1.0 / 16.0, software_fallback_drift_gain: float = 1.0 / 16.0,
) -> AdaptiveTrace: ) -> AdaptiveTrace:
model = FixedPointAdaptivePps( model = FixedPointAdaptivePps(
update_period_s=update_period_s,
oscillator_ppm=oscillator_ppm, oscillator_ppm=oscillator_ppm,
adaptive=adaptive, adaptive=adaptive,
software_filter=software_filter, software_filter=software_filter,
@@ -6,6 +6,7 @@ import unittest
from pps_python.adaptive_pps import ( from pps_python.adaptive_pps import (
ACQUIRE_PROFILE, ACQUIRE_PROFILE,
FixedPointAdaptivePps,
Observation, Observation,
Profile, Profile,
PROFILES, PROFILES,
@@ -59,6 +60,61 @@ def noisy_observations(
class AdaptivePpsTests(unittest.TestCase): class AdaptivePpsTests(unittest.TestCase):
def test_period_aware_coefficients_match_firmware(self):
expected = {
0.2: {
"acquire": (764018, 64006),
Profile.PRECISION: (222838, 5445),
Profile.ROBUST: (84891, 640),
},
0.5: {
"acquire": (615780, 127492),
Profile.PRECISION: (209783, 12804),
Profile.ROBUST: (82980, 1564),
},
1.0: {
"acquire": (447388, 178038),
Profile.PRECISION: (190239, 23149),
Profile.ROBUST: (79923, 3011),
},
}
for period_s, coefficients in expected.items():
self.assertEqual(
ACQUIRE_PROFILE.fixed_coefficients(period_s)[:2],
coefficients["acquire"],
)
for profile in (Profile.PRECISION, Profile.ROBUST):
self.assertEqual(
PROFILES[profile].fixed_coefficients(period_s)[:2],
coefficients[profile],
)
def test_wall_clock_hysteresis_and_settling_scale_with_period(self):
model_500ms = FixedPointAdaptivePps(update_period_s=0.5)
self.assertEqual(model_500ms.selector.upgrade_updates, 4)
self.assertEqual(model_500ms.selector.degrade_updates, 2)
self.assertEqual(
model_500ms.profiles[Profile.PRECISION].software_settle_updates,
5,
)
self.assertEqual(
model_500ms.profiles[Profile.ROBUST].lock_acquire_updates,
4,
)
model_1s = FixedPointAdaptivePps(update_period_s=1.0)
self.assertEqual(model_1s.selector.upgrade_updates, 2)
self.assertEqual(model_1s.selector.degrade_updates, 2)
self.assertEqual(
model_1s.profiles[Profile.PRECISION].software_settle_updates,
3,
)
self.assertEqual(
model_1s.profiles[Profile.ROBUST].lock_acquire_updates,
2,
)
self.assertEqual(model_1s.reference_watchdog_sec, 3.0)
def test_board_calibrated_doppler_predictor_reduces_pps_error(self): def test_board_calibrated_doppler_predictor_reduces_pps_error(self):
"""Genesys2 WLS statistics: predictor-on must beat fallback-only.""" """Genesys2 WLS statistics: predictor-on must beat fallback-only."""
quality_metrics = QualityMetrics( quality_metrics = QualityMetrics(