feat(pps): preserve loop dynamics across nav periods
This commit is contained in:
@@ -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
|
||||||
|
|||||||
@@ -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;
|
||||||
|
|||||||
@@ -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()
|
||||||
|
|||||||
@@ -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:
|
||||||
|
|||||||
@@ -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(
|
||||||
|
|||||||
Reference in New Issue
Block a user