From 347e1c671d1b6f0dad8d22994b0bfddd104a6520 Mon Sep 17 00:00:00 2001 From: onur Date: Wed, 29 Jul 2026 00:45:16 +0300 Subject: [PATCH] feat(pps): preserve loop dynamics across nav periods --- .../gps_system/header/gps/gps_ip_defines.h | 3 + .../gps_system/header/gps/gps_pps_quality.h | 33 +++- .../header/gps/gps_pps_time_filter.h | 4 +- soc/software/gps_system/src/gps/gps_ip.c | 4 +- soc/software/gps_system/src/gps/gps_pps.c | 68 ++++++-- .../gps_system/src/gps/gps_pps_quality.c | 164 +++++++++++++++++- .../gps_system/src/gps/gps_pps_time_filter.c | 28 ++- .../tests/test_gps_pps_adaptive_loop.c | 13 +- .../gps_system/tests/test_gps_pps_quality.c | 65 +++++++ .../tests/test_gps_pps_time_filter.c | 25 +++ .../pps_end_to_end_validation/README.md | 61 +++++-- .../hardware_replay.py | 94 ++++++++-- .../stability_analysis.py | 64 +++++-- .../test_hardware_replay.py | 46 ++++- .../test_pps_end_to_end.py | 26 ++- .../test_stability_analysis.py | 38 +++- soc/software/pps_python/README.md | 63 ++++++- soc/software/pps_python/adaptive_pps.py | 145 ++++++++++++++-- soc/software/pps_python/test_pps_adaptive.py | 56 ++++++ 19 files changed, 889 insertions(+), 111 deletions(-) diff --git a/soc/software/gps_system/header/gps/gps_ip_defines.h b/soc/software/gps_system/header/gps/gps_ip_defines.h index 79735d8..e1ceb9c 100644 --- a/soc/software/gps_system/header/gps/gps_ip_defines.h +++ b/soc/software/gps_system/header/gps/gps_ip_defines.h @@ -55,6 +55,7 @@ #endif #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_BITS_PER_SUBFRAME 300u /* Keep integrity statistics active, but do not let leave-one-out FDE * replace the full-WLS position until the real measurement-noise model * 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" #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" + #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 _Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u), "GPS_NAV_ENABLE_FDE must be zero or one"); diff --git a/soc/software/gps_system/header/gps/gps_pps_quality.h b/soc/software/gps_system/header/gps/gps_pps_quality.h index a6e95bf..4d55651 100644 --- a/soc/software/gps_system/header/gps/gps_pps_quality.h +++ b/soc/software/gps_system/header/gps/gps_pps_quality.h @@ -2,6 +2,9 @@ #define GPS_PPS_QUALITY_H #include +#include + +#define GPS_PPS_BASELINE_UPDATE_PERIOD_SEC 0.200 typedef enum { GPS_PPS_PROFILE_HOLDOVER = 0, @@ -29,6 +32,8 @@ typedef struct { gps_pps_profile_t profile; gps_pps_profile_t candidate; unsigned int candidate_updates; + unsigned int upgrade_updates; + unsigned int degrade_updates; } gps_pps_quality_selector_t; typedef struct { @@ -43,13 +48,17 @@ typedef struct { } gps_pps_loop_profile_t; 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 - * and integrity. Upgrades require sustained evidence and degradations require - * three consecutive observations. A single rejected WLS epoch is skipped - * without resetting the loop; sustained loss enters holdover, while the - * independent reference watchdog remains the final stale-data guard. + * and integrity. Upgrades and degradations require sustained evidence; their + * update counts are scaled from the proven 200 ms timing when period-aware + * initialization is used. A single rejected WLS epoch is skipped without + * 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_quality_selector_t* selector, @@ -57,6 +66,22 @@ gps_pps_profile_t gps_pps_quality_selector_update( 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_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); #endif diff --git a/soc/software/gps_system/header/gps/gps_pps_time_filter.h b/soc/software/gps_system/header/gps/gps_pps_time_filter.h index 96c8eb5..606467e 100644 --- a/soc/software/gps_system/header/gps/gps_pps_time_filter.h +++ b/soc/software/gps_system/header/gps/gps_pps_time_filter.h @@ -58,7 +58,9 @@ bool gps_pps_reference_bias_drift( * 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. * 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 * after a gap too long for bounded Doppler prediction. The independent diff --git a/soc/software/gps_system/src/gps/gps_ip.c b/soc/software/gps_system/src/gps/gps_ip.c index 745bfb9..9c44734 100644 --- a/soc/software/gps_system/src/gps/gps_ip.c +++ b/soc/software/gps_system/src/gps/gps_ip.c @@ -1844,7 +1844,9 @@ void gps_startup_initialization(unsigned int nsp_tick) GPS_NAV_ENABLE_FDE); gps_init_acq_tables(); 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_status_log_decimator = 0u; gps_pps_reference_svid_valid = false; diff --git a/soc/software/gps_system/src/gps/gps_pps.c b/soc/software/gps_system/src/gps/gps_pps.c index cb2fff9..c64fbb7 100644 --- a/soc/software/gps_system/src/gps/gps_pps.c +++ b/soc/software/gps_system/src/gps/gps_pps.c @@ -65,7 +65,8 @@ enum { }; #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 = (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_holdover; 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 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); } -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; - gps_pps_loop_profile_t parameters = gps_pps_loop_profile(profile); - const double update_period_sec = (gps_pps_nav_solve_tick != 0u) ? - ((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; - int64_t kp_q46 = (int64_t)llround( - (2.0 * parameters.damping * wn / GPS_PPS_CLOCK_HZ) * q46); - int64_t ki_q46 = (int64_t)llround((wn * wn * update_period_sec / GPS_PPS_CLOCK_HZ) * q46); - uint32_t max_slew_q32 = (uint32_t)llround( - parameters.max_slew_ppm * 1.0e-6 * 4294967296.0); + return (gps_pps_nav_solve_tick != 0u) ? + ((double)gps_pps_nav_solve_tick * 0.020) : + GPS_PPS_BASELINE_UPDATE_PERIOD_SEC; +} + +static bool gps_pps_write_loop_profile(gps_pps_profile_t profile) +{ + const double update_period_sec = gps_pps_update_period_sec(); + gps_pps_loop_profile_t parameters = + gps_pps_loop_profile_for_period(profile, update_period_sec); + 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->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_acquire_updates & 0xffu) << 24); } + return true; } void gps_pps_init(unsigned int 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_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; /* 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 @@ -149,12 +173,14 @@ void gps_pps_init(unsigned int nav_solve_tick) GPS_PPS_CONTROL_CLEAR_STATUS; GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV, "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_ADAPTIVE_LOOP ? 1u : 0u, gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ? 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) @@ -170,7 +196,7 @@ void gps_pps_service(void) uint32_t now_hi = gps_pps->raw_counter_hi; uint64_t now = ((uint64_t)now_hi << 32) | now_lo; if((now - gps_pps_last_reference_counter) > - (uint64_t)GPS_PPS_REFERENCE_TIMEOUT_TICKS) + gps_pps_reference_timeout_ticks) { gps_pps_set_holdover(true); } @@ -229,7 +255,10 @@ bool gps_pps_set_loop_profile(gps_pps_profile_t profile) { return false; } - gps_pps_write_loop_profile(profile); + if(!gps_pps_write_loop_profile(profile)) + { + return false; + } gps_pps_active_loop_profile = profile; return true; } @@ -277,7 +306,8 @@ gps_pps_time_filter_result_t gps_pps_filter_clock_bias( double* innovation_m_out) { 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( &gps_pps_time_filter, epoch_counter, (double)GPS_PPS_CLOCK_HZ, measured_clock_bias_m, clock_drift_valid, diff --git a/soc/software/gps_system/src/gps/gps_pps_quality.c b/soc/software/gps_system/src/gps/gps_pps_quality.c index a942f7f..ef6e9db 100644 --- a/soc/software/gps_system/src/gps/gps_pps_quality.c +++ b/soc/software/gps_system/src/gps/gps_pps_quality.c @@ -7,6 +7,33 @@ #define GPS_PPS_PROFILE_UPGRADE_UPDATES 8u #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) { @@ -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) +{ + 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) { memset(selector, 0, sizeof(*selector)); selector->profile = 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 = (target > selector->profile) ? - GPS_PPS_PROFILE_UPGRADE_UPDATES : - GPS_PPS_PROFILE_DEGRADE_UPDATES; + selector->upgrade_updates : + selector->degrade_updates; if(selector->candidate_updates >= required_updates) { 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) { + 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) { case GPS_PPS_PROFILE_ACQUIRE: - return (gps_pps_loop_profile_t){ + parameters = (gps_pps_loop_profile_t){ .natural_frequency_hz = 0.120, .damping = 0.9, .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_settle_updates = 12u }; + break; case GPS_PPS_PROFILE_PRECISION: - return (gps_pps_loop_profile_t){ + parameters = (gps_pps_loop_profile_t){ .natural_frequency_hz = 0.035, .damping = 0.9, .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_settle_updates = 12u }; + break; case GPS_PPS_PROFILE_ROBUST: - return (gps_pps_loop_profile_t){ + parameters = (gps_pps_loop_profile_t){ .natural_frequency_hz = 0.012, .damping = 1.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_settle_updates = 16u }; + break; case GPS_PPS_PROFILE_HOLDOVER: default: - return (gps_pps_loop_profile_t){ + parameters = (gps_pps_loop_profile_t){ .natural_frequency_hz = 0.0, .damping = 1.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_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) diff --git a/soc/software/gps_system/src/gps/gps_pps_time_filter.c b/soc/software/gps_system/src/gps/gps_pps_time_filter.c index 0333ad0..e1ead4f 100644 --- a/soc/software/gps_system/src/gps/gps_pps_time_filter.c +++ b/soc/software/gps_system/src/gps/gps_pps_time_filter.c @@ -20,6 +20,7 @@ #define GPS_PPS_FILTER_INNOVATION_CLAMP_M 300.0 #define GPS_PPS_FILTER_HARD_OUTLIER_M 1500.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) { @@ -34,6 +35,21 @@ static double clamp_double(double value, double limit) 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) { if(filter != NULL) @@ -269,14 +285,20 @@ gps_pps_time_filter_result_t gps_pps_time_filter_update( 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 = predicted_clock_bias_m + - bias_gain * + effective_bias_gain * clamp_double(innovation_m, GPS_PPS_FILTER_INNOVATION_CLAMP_M); if(clock_drift_valid) { filter->clock_drift_mps += - GPS_PPS_FILTER_DRIFT_GAIN * + effective_drift_gain * (measured_clock_drift_mps - filter->clock_drift_mps); } else @@ -291,7 +313,7 @@ gps_pps_time_filter_result_t gps_pps_time_filter_update( else { filter->clock_drift_mps += - GPS_PPS_FILTER_FALLBACK_DRIFT_GAIN * + effective_fallback_drift_gain * (fallback_drift_mps - filter->clock_drift_mps); } } diff --git a/soc/software/gps_system/tests/test_gps_pps_adaptive_loop.c b/soc/software/gps_system/tests/test_gps_pps_adaptive_loop.c index 2dfda0e..eb91b0e 100644 --- a/soc/software/gps_system/tests/test_gps_pps_adaptive_loop.c +++ b/soc/software/gps_system/tests/test_gps_pps_adaptive_loop.c @@ -91,14 +91,11 @@ static void profile_coefficients( int64_t* ki_q46, int64_t* max_slew_q32) { - const double pi = 3.14159265358979323846; - double wn = 2.0 * pi * profile.natural_frequency_hz; - *kp_q46 = (int64_t)llround( - (2.0 * profile.damping * wn / TRACKING_HZ) * Q46_SCALE); - *ki_q46 = (int64_t)llround( - (wn * wn * UPDATE_PERIOD_SEC / TRACKING_HZ) * Q46_SCALE); - *max_slew_q32 = (int64_t)llround( - profile.max_slew_ppm * 1.0e-6 * Q32_SCALE); + uint32_t max_slew = 0u; + assert(gps_pps_loop_coefficients( + profile, UPDATE_PERIOD_SEC, TRACKING_HZ, + kp_q46, ki_q46, &max_slew)); + *max_slew_q32 = max_slew; } static void synthetic_rtl_coarse(synthetic_rtl_t* rtl, int64_t reference_q32) diff --git a/soc/software/gps_system/tests/test_gps_pps_quality.c b/soc/software/gps_system/tests/test_gps_pps_quality.c index 8a5314a..59e8937 100644 --- a/soc/software/gps_system/tests/test_gps_pps_quality.c +++ b/soc/software/gps_system/tests/test_gps_pps_quality.c @@ -107,6 +107,69 @@ static void test_profile_parameters_are_ordered(void) 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) { gps_pps_quality_selector_t selector; @@ -167,6 +230,8 @@ int main(void) test_hysteretic_profile_transitions(); test_alternating_quality_does_not_upgrade(); test_profile_parameters_are_ordered(); + test_period_scaled_hysteresis_and_settling(); + test_period_scaled_discrete_coefficients(); test_quality_boundaries_and_recovery(); puts("gps_pps_quality tests passed"); return 0; diff --git a/soc/software/gps_system/tests/test_gps_pps_time_filter.c b/soc/software/gps_system/tests/test_gps_pps_time_filter.c index 1e65fcd..aa6963c 100644 --- a/soc/software/gps_system/tests/test_gps_pps_time_filter.c +++ b/soc/software/gps_system/tests/test_gps_pps_time_filter.c @@ -6,6 +6,30 @@ #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 const double noise_m[] = { @@ -298,6 +322,7 @@ static void test_reference_selection_is_stable(void) int main(void) { + test_elapsed_time_gain_preserves_filter_poles(); test_drift_prediction_rejects_bias_noise(); test_quality_gap_and_outlier_handling(); test_bias_ramp_without_doppler_uses_bounded_fallback(); diff --git a/soc/software/pps_end_to_end_validation/README.md b/soc/software/pps_end_to_end_validation/README.md index 638b686..5daa7a5 100644 --- a/soc/software/pps_end_to_end_validation/README.md +++ b/soc/software/pps_end_to_end_validation/README.md @@ -80,6 +80,10 @@ that the repository did not capture: ```bash 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 \ 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 wander, period jitter, dynamic parameter sweep, and recommendations; - `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, 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. 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 -unlock. It also exposes an important metrics issue: cycle-to-cycle PPS period +unit circle at the 200, 500, and 1000 ms supported update rates, and the stress +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 of nanoseconds. A physical 20 ns maximum therefore requires a common-clock 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, a robust-noise guard, a larger Doppler-drift gain, and a quality-scheduled candidate. The generated report contains the current numerical comparison. -The stability analysis does not alter loop constants. Production firmware now -uses the current-RTL-compatible 180 ms epoch correction described below. +Production firmware maps the proven 200 ms discrete poles to the selected NAV +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 @@ -146,9 +171,13 @@ omits latitude/longitude. For an already-running board, replay imports the first logged locked filter/PI snapshot. Starting the model from reset would erase the observed 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`. +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: - 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 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 @@ -201,16 +232,20 @@ The unit tests verify: - 180 ms firmware compatibility with current RTL; - invariance to synthetic CPU/APB stalls when capture timestamps are used; - 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 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 -fundamental 20 ms navigation-bit grid. A narrower compatibility restriction is -to accept only periods for which `300 % N == 0`. +If arbitrary timer periods are required later, reconstruct elapsed time on the +fundamental 20 ms navigation-bit grid instead of snapping through a refreshed +HOW anchor. ## Hardware confirmation diff --git a/soc/software/pps_end_to_end_validation/hardware_replay.py b/soc/software/pps_end_to_end_validation/hardware_replay.py index 1e6048e..69ab25d 100644 --- a/soc/software/pps_end_to_end_validation/hardware_replay.py +++ b/soc/software/pps_end_to_end_validation/hardware_replay.py @@ -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, 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 reference trajectory from the logged filtered bias, PI residual and rate word. 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 +NAV_BIT_PERIOD_S = 0.020 PPS_RE = re.compile( @@ -286,13 +287,15 @@ def _profile(sample: FirmwarePpsSample) -> Profile: 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.""" profile = _profile(sample) if profile == Profile.HOLDOVER or sample.mode != "tracking": return math.nan kp_q46, _, max_slew_q32 = PROFILES[profile].fixed_coefficients( - UPDATE_PERIOD_S + update_period_s ) phase_q32 = round( 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]) -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``.""" pending: list[dict[str, object]] = [] latest_location: GpsContext | None = None @@ -428,12 +478,13 @@ def load_firmware_telemetry(path: Path) -> list[FirmwarePpsSample]: if not pending: return [] + update_period_s = _infer_update_period(pending, update_period_s) first_sequence = int(pending[0]["sequence"]) samples: list[FirmwarePpsSample] = [] for fields in pending: sequence = int(fields["sequence"]) 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)) return samples @@ -484,11 +535,12 @@ def observed_metrics(samples: list[FirmwarePpsSample]) -> ObservedMetrics: centered_active = [ value - active_center for value in inferred_active_phase ] + update_period_s = _sample_update_period(samples) pi_rate_errors_ppb = [ (prediction - sample.rate_command_ppm) * 1.0e3 for sample in samples if math.isfinite( - prediction := _pi_rate_prediction_ppm(sample) + prediction := _pi_rate_prediction_ppm(sample, update_period_s) ) ] return ObservedMetrics( @@ -592,7 +644,7 @@ def _interpolated_sample( float(getattr(first, name)), float(getattr(second, name)), fraction ) 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( _lerp(first.pulse_count, second.pulse_count, fraction) ) @@ -637,6 +689,7 @@ def replay_firmware_telemetry( if len(samples) < 2: raise ValueError("at least two firmware PPS samples are required") expanded = expand_samples(samples) + update_period_s = _sample_update_period(samples) first = expanded[0][0] first_profile = _profile(first) 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) ] model = FixedPointAdaptivePps( - update_period_s=UPDATE_PERIOD_S, + update_period_s=update_period_s, oscillator_ppm=oscillator_ppm, receiver_clock_bias_m=true_bias_m[0], receiver_clock_drift_mps=0.0, @@ -736,7 +789,7 @@ def replay_firmware_telemetry( 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 = [ error * 1.0e9 @@ -874,6 +927,7 @@ def write_report( replay: ReplayMetrics, sensitivity: list[ReplayMetrics], ) -> None: + update_period_s = _sample_update_period(samples) profiles = sorted({sample.profile for sample in samples}) references = sorted({sample.reference_svid for sample in samples}) lines = [ @@ -886,7 +940,8 @@ def write_report( f"- Samples/duration: {observed.samples} / {observed.duration_s:.1f} s.", f"- Status interval: {observed.status_interval_median_s:.3f} s; " 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"{observed.locked_fraction:.3f}; transitions " f"{observed.profile_transitions}; reference SVIDs {references}.", @@ -998,7 +1053,8 @@ def write_report( "", "- Missing status epochs are linearly interpolated when sequence step " "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 " "frequency chosen to make net phase slope zero over this capture. " "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 " "temperature, or a trusted PPS edge timestamp. The bit/subframe part " "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 " "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", ) 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") return parser.parse_args() def main() -> int: 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: raise SystemExit("input does not contain at least two firmware PPS records") observed = observed_metrics(samples) diff --git a/soc/software/pps_end_to_end_validation/stability_analysis.py b/soc/software/pps_end_to_end_validation/stability_analysis.py index 6a22ec3..8bc194b 100644 --- a/soc/software/pps_end_to_end_validation/stability_analysis.py +++ b/soc/software/pps_end_to_end_validation/stability_analysis.py @@ -38,6 +38,7 @@ RF_HZ = 16_367_981.0 CODE_FREQ_HZ = 1_023_000.0 SPEED_OF_LIGHT_MPS = 299_792_458.0 UPDATE_PERIOD_S = 0.2 +NAV_BIT_PERIOD_S = 0.020 BOARD_CLOCK_SIGMA_M = 15.87 BOARD_TDOP = 1.50 BOARD_GDOP = 2.92 @@ -297,14 +298,15 @@ def synthesize_epochs( *, duration_s: float, seed: int, + update_period_s: float = UPDATE_PERIOD_S, ) -> list[SyntheticEpoch]: """Combine real GDOP/satellite count with documented board noise.""" rng = random.Random(seed) 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 for update in range(count): - time_s = update * UPDATE_PERIOD_S + time_s = update * update_period_s gdop = telemetry.gdop_at(time_s) satellites = max(4, min(8, telemetry.tracked_at(time_s))) tdop = gdop * (BOARD_TDOP / BOARD_GDOP) @@ -432,8 +434,14 @@ def synthesize_epochs( 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( + update_period_s=update_period_s, oscillator_ppm=BOARD_OSCILLATOR_PPM, profiles=tuning.profiles, acquire_profile=tuning.acquire, @@ -451,7 +459,7 @@ def _run_loop(epochs: list[SyntheticEpoch], tuning: Tuning): else tuning.drift_gain ) 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) @@ -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.""" - 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) ki = ki_q46 / (1 << 46) - ticks = UPDATE_PERIOD_S * TRACKING_HZ + ticks = update_period_s * TRACKING_HZ a = 1.0 - ticks * (kp + ki) b = ticks c = -ki @@ -573,6 +584,7 @@ def write_report( telemetry: RealTelemetry, results: list[Aggregate], epoch_result, + update_period_s: float = UPDATE_PERIOD_S, ) -> None: gdop = telemetry.gdop tracked = telemetry.tracked @@ -657,7 +669,7 @@ def write_report( "## Linear stability", "", "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 |", "|---|---:|---:|---|---:|", @@ -671,7 +683,7 @@ def write_report( ) ] for name, profile in profile_rows: - poles = discrete_poles(profile) + poles = discrete_poles(profile, update_period_s) pole_text = ", ".join( 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 " "together with the next commit, so the current acquire/precision/" "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 " - "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", "", @@ -784,6 +798,12 @@ def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--duration", type=float, default=300.0) 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( "--output-dir", type=Path, @@ -797,6 +817,19 @@ def main() -> int: args = parse_args() if args.duration < 120.0 or args.seeds < 1: 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] telemetry = load_real_telemetry(software_dir) if not telemetry.gdop or not telemetry.tracked: @@ -818,6 +851,7 @@ def main() -> int: scenario, duration_s=args.duration, seed=seed, + update_period_s=update_period_s, ) for seed in range(1, args.seeds + 1) ] @@ -826,7 +860,12 @@ def main() -> int: for tuning in tuning_matrix(): runs = [ 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 ] @@ -858,6 +897,7 @@ def main() -> int: telemetry, results, epoch_result, + update_period_s, ) print(f"wrote {args.output_dir}") return 0 diff --git a/soc/software/pps_end_to_end_validation/test_hardware_replay.py b/soc/software/pps_end_to_end_validation/test_hardware_replay.py index 5d29d7f..542b970 100644 --- a/soc/software/pps_end_to_end_validation/test_hardware_replay.py +++ b/soc/software/pps_end_to_end_validation/test_hardware_replay.py @@ -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) integrator_ppm = -1.560 kp_q46, _, _ = PROFILES[Profile.ROBUST].fixed_coefficients( - UPDATE_PERIOD_S + update_period_s ) phase_q32 = round(phase_ns * 1.0e-9 * TRACKING_HZ * Q32) 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 " f"hw_phase={phase_ns:.2f}ns rate_cmd={rate_ppm:.3f}ppm " 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] source=gps lat=0 lon=0 height=0 " "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.writerow(("host_monotonic_s", "host_unix_s", "uart_line")) 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, 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): @@ -99,6 +118,21 @@ class HardwareReplayTests(unittest.TestCase): self.assertTrue(math.isfinite(replay.modeled_edge_centered_rms_ns)) 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): samples = load_firmware_telemetry(self.capture) results = replay_tuning_sensitivity(samples, warmup_s=5.0) diff --git a/soc/software/pps_end_to_end_validation/test_pps_end_to_end.py b/soc/software/pps_end_to_end_validation/test_pps_end_to_end.py index 3b40702..356ea2f 100644 --- a/soc/software/pps_end_to_end_validation/test_pps_end_to_end.py +++ b/soc/software/pps_end_to_end_validation/test_pps_end_to_end.py @@ -81,11 +81,10 @@ class PpsEndToEndTests(unittest.TestCase): self.assertGreater(result.nav_solve_interrupts, 20) self.assertEqual(result.firmware_rejected_nav_solves, 0) - def test_permitted_40_bit_period_rejects_after_how_anchor_moves(self): - # Firmware's compile-time rule accepts 40 because it divides the GPS - # week. It does not divide a 300-bit subframe, so refreshing the HOW - # sample anchor changes timer phase by half a solve period. The - # current N*20 ms snap then rejects otherwise valid capture events. + def test_non_subframe_divisor_explains_40_bit_build_restriction(self): + # N=40 divides the GPS week but not a 300-bit subframe. Refreshing the + # HOW sample anchor changes timer phase by half a solve period, so the + # model rejects solves. Firmware now prevents this configuration. self.assertEqual(30_240_000 % 40, 0) self.assertNotEqual(300 % 40, 0) result = run_end_to_end( @@ -93,6 +92,23 @@ class PpsEndToEndTests(unittest.TestCase): ) 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): result = run_end_to_end(EndToEndConfig(postpone_tow_s=0.160)) offset_s = result.pps.edges[0].dut_minus_trusted_s diff --git a/soc/software/pps_end_to_end_validation/test_stability_analysis.py b/soc/software/pps_end_to_end_validation/test_stability_analysis.py index 8adf4ea..21a368d 100644 --- a/soc/software/pps_end_to_end_validation/test_stability_analysis.py +++ b/soc/software/pps_end_to_end_validation/test_stability_analysis.py @@ -40,8 +40,23 @@ class StabilityAnalysisTests(unittest.TestCase): PROFILES[Profile.PRECISION], PROFILES[Profile.ROBUST], ): - poles = discrete_poles(profile) - self.assertTrue(all(abs(pole) < 1.0 for pole in poles)) + baseline_poles = discrete_poles(profile) + 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): epochs = synthesize_epochs( @@ -79,6 +94,25 @@ class StabilityAnalysisTests(unittest.TestCase): 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__": unittest.main() diff --git a/soc/software/pps_python/README.md b/soc/software/pps_python/README.md index 956f096..b9c8810 100644 --- a/soc/software/pps_python/README.md +++ b/soc/software/pps_python/README.md @@ -9,7 +9,8 @@ davranışlarını taklit eder: - RF `ABS_SAMPLE` sayacı: `16.367981 MHz`, 32 bit ve wrap eden 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 delay'lerle art arda tracking olayı üretmesi. - 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: ```text -integrator += Ki * phase_error * update_period +integrator += Ki_discrete * 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, 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 Tam fixed-point/holdover matrisi: diff --git a/soc/software/pps_python/adaptive_pps.py b/soc/software/pps_python/adaptive_pps.py index f2b1f4c..92fbb20 100644 --- a/soc/software/pps_python/adaptive_pps.py +++ b/soc/software/pps_python/adaptive_pps.py @@ -8,7 +8,7 @@ rounding, lock acquisition/release and holdover/re-acquisition. from __future__ import annotations -from dataclasses import dataclass +from dataclasses import dataclass, replace from enum import IntEnum import math from typing import Iterable, Mapping @@ -16,7 +16,9 @@ from typing import Iterable, Mapping TRACKING_HZ = 125_000_000 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 Q32 = 1 << 32 Q46 = 1 << 46 @@ -24,6 +26,27 @@ MAX_SIGNED_64 = (1 << 63) - 1 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): HOLDOVER = 0 ROBUST = 1 @@ -58,14 +81,74 @@ class LoopProfile: software_settle_updates: 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 - kp_q46 = round((2.0 * self.damping * omega / TRACKING_HZ) * Q46) - ki_q46 = round( - (omega * omega * update_period_s / TRACKING_HZ) * Q46 + baseline_kp = math.floor( + (2.0 * self.damping * omega / TRACKING_HZ) * Q46 + 0.5 + ) / 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 + 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 = { Profile.HOLDOVER: LoopProfile( @@ -149,10 +232,12 @@ def classify_quality(metrics: QualityMetrics) -> Profile: class QualitySelector: """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.candidate = Profile.HOLDOVER 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]: target = classify_quality(metrics) @@ -165,7 +250,11 @@ class QualitySelector: self.candidate_updates = 1 else: 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: self.profile = target self.candidate_updates = 0 @@ -339,19 +428,28 @@ class FixedPointAdaptivePps: software_drift_gain: float = 1.0 / 4.0, software_fallback_drift_gain: float = 1.0 / 16.0, ) -> 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.oscillator_fraction = oscillator_ppm * 1.0e-6 self.adaptive = adaptive self.software_filter = software_filter self.receiver_clock_bias_m = receiver_clock_bias_m 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() if 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 ) + self.acquire_profile = baseline_acquire_profile.for_update_period( + update_period_s + ) if not 0.0 < software_drift_gain <= 1.0: raise ValueError("software_drift_gain must be in (0, 1]") 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_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.parameters = self.profiles[self.profile] self.kp_q46, self.ki_q46, self.max_slew_q32 = ( @@ -652,12 +754,21 @@ class FixedPointAdaptivePps: return False, False 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 = ( predicted_bias_m - + self.parameters.software_bias_gain * innovation_m + + effective_bias_gain * innovation_m ) 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 ) else: @@ -665,7 +776,7 @@ class FixedPointAdaptivePps: self.software_clock_drift_mps = fallback_drift_mps else: self.software_clock_drift_mps += ( - self.software_fallback_drift_gain * + effective_fallback_drift_gain * (fallback_drift_mps - self.software_clock_drift_mps) ) self.software_previous_measurement_m = measured_bias_m @@ -731,7 +842,7 @@ class FixedPointAdaptivePps: if ( self.last_observation_time_s is None or observation.time_s - self.last_observation_time_s - > REFERENCE_WATCHDOG_SEC + > self.reference_watchdog_sec ): self.reference_watchdog_holdover = True self._record(observation, 0, 0) @@ -747,7 +858,7 @@ class FixedPointAdaptivePps: if ( self.last_observation_time_s is None or observation.time_s - self.last_observation_time_s - > REFERENCE_WATCHDOG_SEC + > self.reference_watchdog_sec ): self.reference_watchdog_holdover = True self._record(observation, 0, 0) @@ -817,6 +928,7 @@ def run_observations( observations: Iterable[Observation], *, finish_time_s: float, + update_period_s: float = BASELINE_UPDATE_PERIOD_S, oscillator_ppm: float = 1.65, adaptive: bool = True, software_filter: bool = True, @@ -828,6 +940,7 @@ def run_observations( software_fallback_drift_gain: float = 1.0 / 16.0, ) -> AdaptiveTrace: model = FixedPointAdaptivePps( + update_period_s=update_period_s, oscillator_ppm=oscillator_ppm, adaptive=adaptive, software_filter=software_filter, diff --git a/soc/software/pps_python/test_pps_adaptive.py b/soc/software/pps_python/test_pps_adaptive.py index 21196df..2010714 100644 --- a/soc/software/pps_python/test_pps_adaptive.py +++ b/soc/software/pps_python/test_pps_adaptive.py @@ -6,6 +6,7 @@ import unittest from pps_python.adaptive_pps import ( ACQUIRE_PROFILE, + FixedPointAdaptivePps, Observation, Profile, PROFILES, @@ -59,6 +60,61 @@ def noisy_observations( 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): """Genesys2 WLS statistics: predictor-on must beat fallback-only.""" quality_metrics = QualityMetrics(