onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(pps): preserve loop dynamics across nav periods

Browse Source
This commit is contained in:
onur
2026-07-29 00:45:16 +03:00
parent a6bb080270
commit 347e1c671d
19 changed files with 889 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/src/gps/gps_ip.c
+3 -1
View File
@@ -1844,7 +1844,9 @@ void gps_startup_initialization(unsigned int nsp_tick)
GPS_NAV_ENABLE_FDE);
gps_init_acq_tables();
gps_pps_init(nsp_tick);
gps_pps_quality_selector_init(&gps_pps_quality_selector);
gps_pps_quality_selector_init_period(
&gps_pps_quality_selector,
(double)nsp_tick * (double)GPS_NAV_SOLVE_TIMER_TICK_MS / 1000.0);
gps_pps_applied_profile = GPS_PPS_PROFILE_HOLDOVER;
gps_pps_status_log_decimator = 0u;
gps_pps_reference_svid_valid = false;
soc/software/gps_system/src/gps/gps_pps.c
+49 -19
View File
@@ -65,7 +65,8 @@ enum {
};
#define GPS_PPS_VERSION_ADAPTIVE_LOOP 0x00020200u
#define GPS_PPS_REFERENCE_TIMEOUT_TICKS (2u * GPS_PPS_CLOCK_HZ)
#define GPS_PPS_MIN_REFERENCE_TIMEOUT_TICKS (2u * GPS_PPS_CLOCK_HZ)
#define GPS_PPS_REFERENCE_TIMEOUT_PERIODS 3u
static volatile gps_pps_regs_t* const gps_pps =
(volatile gps_pps_regs_t*)BSP_GPS_PPS_BASE;
@@ -77,6 +78,7 @@ static bool gps_pps_force_coarse;
static bool gps_pps_reference_committed;
static bool gps_pps_holdover;
static uint64_t gps_pps_last_reference_counter;
static uint64_t gps_pps_reference_timeout_ticks;
static gps_pps_time_filter_t gps_pps_time_filter;
static uint64_t gps_pps_read_u64(
@@ -94,19 +96,30 @@ static void gps_pps_write_q46(volatile uint32_t* lo, volatile uint32_t* hi, int6
*hi = (uint32_t)(encoded >> 32);
}
static void gps_pps_write_loop_profile(gps_pps_profile_t profile)
static double gps_pps_update_period_sec(void)
{
const double pi = 3.14159265358979323846;
gps_pps_loop_profile_t parameters = gps_pps_loop_profile(profile);
const double update_period_sec = (gps_pps_nav_solve_tick != 0u) ?
((double)gps_pps_nav_solve_tick * 0.020) : 0.200;
const double wn = 2.0 * pi * parameters.natural_frequency_hz;
const double q46 = 70368744177664.0;
int64_t kp_q46 = (int64_t)llround(
(2.0 * parameters.damping * wn / GPS_PPS_CLOCK_HZ) * q46);
int64_t ki_q46 = (int64_t)llround((wn * wn * update_period_sec / GPS_PPS_CLOCK_HZ) * q46);
uint32_t max_slew_q32 = (uint32_t)llround(
parameters.max_slew_ppm * 1.0e-6 * 4294967296.0);
return (gps_pps_nav_solve_tick != 0u) ?
((double)gps_pps_nav_solve_tick * 0.020) :
GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
}
static bool gps_pps_write_loop_profile(gps_pps_profile_t profile)
{
const double update_period_sec = gps_pps_update_period_sec();
gps_pps_loop_profile_t parameters =
gps_pps_loop_profile_for_period(profile, update_period_sec);
int64_t kp_q46 = 0;
int64_t ki_q46 = 0;
uint32_t max_slew_q32 = 0u;
if(!gps_pps_loop_coefficients(
parameters, update_period_sec, (double)GPS_PPS_CLOCK_HZ,
&kp_q46, &ki_q46, &max_slew_q32))
{
GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_NAV,
"PPS loop coefficient generation failed profile=%s period_ms=%.1lf\r\n",
gps_pps_profile_name(profile), update_period_sec * 1000.0);
return false;
}
gps_pps_write_q46(&gps_pps->kp_q46_lo, &gps_pps->kp_q46_hi, kp_q46);
gps_pps_write_q46(&gps_pps->ki_q46_lo, &gps_pps->ki_q46_hi, ki_q46);
@@ -118,14 +131,25 @@ static void gps_pps_write_loop_profile(gps_pps_profile_t profile)
((parameters.lock_release_ticks & 0xffu) << 16) |
((parameters.lock_acquire_updates & 0xffu) << 24);
}
return true;
}
void gps_pps_init(unsigned int nav_solve_tick)
{
gps_pps_nav_solve_tick = nav_solve_tick;
uint64_t period_ticks =
((uint64_t)GPS_PPS_CLOCK_HZ * nav_solve_tick * 20u) / 1000u;
gps_pps_reference_timeout_ticks =
GPS_PPS_REFERENCE_TIMEOUT_PERIODS * period_ticks;
if(gps_pps_reference_timeout_ticks <
(uint64_t)GPS_PPS_MIN_REFERENCE_TIMEOUT_TICKS)
{
gps_pps_reference_timeout_ticks =
GPS_PPS_MIN_REFERENCE_TIMEOUT_TICKS;
}
gps_pps->control = 0u;
gps_pps_version = gps_pps->version;
gps_pps_write_loop_profile(GPS_PPS_PROFILE_ROBUST);
(void)gps_pps_write_loop_profile(GPS_PPS_PROFILE_ROBUST);
gps_pps_active_loop_profile = GPS_PPS_PROFILE_ROBUST;
/* A GDB CPU reset does not reset the tracking-clock PPS state. Continue
* its mailbox sequence and force the first new solution to re-anchor the
@@ -149,12 +173,14 @@ void gps_pps_init(unsigned int nav_solve_tick)
GPS_PPS_CONTROL_CLEAR_STATUS;
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
"PPS init rtl=0x%08x adaptive=%u loop_config=0x%08x "
"clock_hz=%u update_ticks=%u\r\n",
"clock_hz=%u update_ticks=%u reference_timeout_ms=%lu\r\n",
gps_pps_version,
gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ? 1u : 0u,
gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ?
gps_pps->loop_config : 0u,
GPS_PPS_CLOCK_HZ, gps_pps_nav_solve_tick);
GPS_PPS_CLOCK_HZ, gps_pps_nav_solve_tick,
(unsigned long)((gps_pps_reference_timeout_ticks * 1000u) /
GPS_PPS_CLOCK_HZ));
}
void gps_pps_service(void)
@@ -170,7 +196,7 @@ void gps_pps_service(void)
uint32_t now_hi = gps_pps->raw_counter_hi;
uint64_t now = ((uint64_t)now_hi << 32) | now_lo;
if((now - gps_pps_last_reference_counter) >
(uint64_t)GPS_PPS_REFERENCE_TIMEOUT_TICKS)
gps_pps_reference_timeout_ticks)
{
gps_pps_set_holdover(true);
}
@@ -229,7 +255,10 @@ bool gps_pps_set_loop_profile(gps_pps_profile_t profile)
{
return false;
}
gps_pps_write_loop_profile(profile);
if(!gps_pps_write_loop_profile(profile))
{
return false;
}
gps_pps_active_loop_profile = profile;
return true;
}
@@ -277,7 +306,8 @@ gps_pps_time_filter_result_t gps_pps_filter_clock_bias(
double* innovation_m_out)
{
gps_pps_loop_profile_t parameters =
gps_pps_loop_profile(gps_pps_active_loop_profile);
gps_pps_loop_profile_for_period(
gps_pps_active_loop_profile, gps_pps_update_period_sec());
return gps_pps_time_filter_update(
&gps_pps_time_filter, epoch_counter, (double)GPS_PPS_CLOCK_HZ,
measured_clock_bias_m, clock_drift_valid,
soc/software/gps_system/src/gps/gps_pps_quality.c
+158 -6
View File
@@ -7,6 +7,33 @@
#define GPS_PPS_PROFILE_UPGRADE_UPDATES 8u
#define GPS_PPS_PROFILE_DEGRADE_UPDATES 3u
#define GPS_PPS_MIN_HYSTERESIS_UPDATES 2u
#define GPS_PPS_MIN_LOCK_UPDATES 2u
#define GPS_PPS_MIN_SETTLE_UPDATES 3u
#define GPS_PPS_MAX_CONFIG_UPDATES 255u
static unsigned int gps_pps_period_updates(
unsigned int baseline_updates,
double update_period_sec,
unsigned int minimum_updates)
{
if(!isfinite(update_period_sec) || (update_period_sec <= 0.0))
{
update_period_sec = GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
}
double duration_sec =
(double)baseline_updates * GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
double required = ceil(duration_sec / update_period_sec - 1.0e-12);
if(required < (double)minimum_updates)
{
return minimum_updates;
}
if(required > (double)GPS_PPS_MAX_CONFIG_UPDATES)
{
return GPS_PPS_MAX_CONFIG_UPDATES;
}
return (unsigned int)required;
}
static bool gps_pps_metrics_finite(const gps_pps_quality_metrics_t* metrics)
{
@@ -67,12 +94,26 @@ static gps_pps_profile_t gps_pps_classify_quality(
}
void gps_pps_quality_selector_init(gps_pps_quality_selector_t* selector)
{
gps_pps_quality_selector_init_period(
selector, GPS_PPS_BASELINE_UPDATE_PERIOD_SEC);
}
void gps_pps_quality_selector_init_period(
gps_pps_quality_selector_t* selector,
double update_period_sec)
{
if(selector != NULL)
{
memset(selector, 0, sizeof(*selector));
selector->profile = GPS_PPS_PROFILE_HOLDOVER;
selector->candidate = GPS_PPS_PROFILE_HOLDOVER;
selector->upgrade_updates = gps_pps_period_updates(
GPS_PPS_PROFILE_UPGRADE_UPDATES, update_period_sec,
GPS_PPS_MIN_HYSTERESIS_UPDATES);
selector->degrade_updates = gps_pps_period_updates(
GPS_PPS_PROFILE_DEGRADE_UPDATES, update_period_sec,
GPS_PPS_MIN_HYSTERESIS_UPDATES);
}
}
@@ -111,8 +152,8 @@ gps_pps_profile_t gps_pps_quality_selector_update(
unsigned int required_updates =
(target > selector->profile) ?
GPS_PPS_PROFILE_UPGRADE_UPDATES :
GPS_PPS_PROFILE_DEGRADE_UPDATES;
selector->upgrade_updates :
selector->degrade_updates;
if(selector->candidate_updates >= required_updates)
{
selector->profile = target;
@@ -127,10 +168,19 @@ gps_pps_profile_t gps_pps_quality_selector_update(
gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
{
return gps_pps_loop_profile_for_period(
profile, GPS_PPS_BASELINE_UPDATE_PERIOD_SEC);
}
gps_pps_loop_profile_t gps_pps_loop_profile_for_period(
gps_pps_profile_t profile,
double update_period_sec)
{
gps_pps_loop_profile_t parameters;
switch(profile)
{
case GPS_PPS_PROFILE_ACQUIRE:
return (gps_pps_loop_profile_t){
parameters = (gps_pps_loop_profile_t){
.natural_frequency_hz = 0.120,
.damping = 0.9,
.max_slew_ppm = 20.0,
@@ -140,8 +190,9 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
.software_bias_gain = 1.0 / 16.0,
.software_settle_updates = 12u
};
break;
case GPS_PPS_PROFILE_PRECISION:
return (gps_pps_loop_profile_t){
parameters = (gps_pps_loop_profile_t){
.natural_frequency_hz = 0.035,
.damping = 0.9,
.max_slew_ppm = 20.0,
@@ -151,8 +202,9 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
.software_bias_gain = 1.0 / 32.0,
.software_settle_updates = 12u
};
break;
case GPS_PPS_PROFILE_ROBUST:
return (gps_pps_loop_profile_t){
parameters = (gps_pps_loop_profile_t){
.natural_frequency_hz = 0.012,
.damping = 1.0,
.max_slew_ppm = 20.0,
@@ -162,9 +214,10 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
.software_bias_gain = 1.0 / 32.0,
.software_settle_updates = 16u
};
break;
case GPS_PPS_PROFILE_HOLDOVER:
default:
return (gps_pps_loop_profile_t){
parameters = (gps_pps_loop_profile_t){
.natural_frequency_hz = 0.0,
.damping = 1.0,
.max_slew_ppm = 20.0,
@@ -174,7 +227,106 @@ gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
.software_bias_gain = 1.0 / 32.0,
.software_settle_updates = 16u
};
break;
}
parameters.lock_acquire_updates = gps_pps_period_updates(
parameters.lock_acquire_updates, update_period_sec,
GPS_PPS_MIN_LOCK_UPDATES);
parameters.software_settle_updates = gps_pps_period_updates(
parameters.software_settle_updates, update_period_sec,
GPS_PPS_MIN_SETTLE_UPDATES);
return parameters;
}
bool gps_pps_loop_coefficients(
gps_pps_loop_profile_t parameters,
double update_period_sec,
double clock_frequency_hz,
int64_t* kp_q46_out,
int64_t* ki_q46_out,
uint32_t* max_slew_q32_out)
{
if((kp_q46_out == NULL) || (ki_q46_out == NULL) ||
(max_slew_q32_out == NULL) ||
!isfinite(update_period_sec) || (update_period_sec <= 0.0) ||
!isfinite(clock_frequency_hz) || (clock_frequency_hz <= 0.0) ||
!isfinite(parameters.natural_frequency_hz) ||
(parameters.natural_frequency_hz < 0.0) ||
!isfinite(parameters.damping) || (parameters.damping < 0.0) ||
!isfinite(parameters.max_slew_ppm) ||
(parameters.max_slew_ppm < 0.0))
{
return false;
}
const double pi = 3.14159265358979323846;
const double q46 = 70368744177664.0;
const double q32 = 4294967296.0;
const double baseline_sec = GPS_PPS_BASELINE_UPDATE_PERIOD_SEC;
const double wn = 2.0 * pi * parameters.natural_frequency_hz;
/* Use the actual deployed Q2.46 baseline, not its ideal double value, so
* the mapped poles are those of the proven fixed-point implementation. */
const double baseline_kp =
(double)llround(
(2.0 * parameters.damping * wn / clock_frequency_hz) * q46) /
q46;
const double baseline_ki =
(double)llround(
(wn * wn * baseline_sec / clock_frequency_hz) * q46) /
q46;
const double baseline_ticks = clock_frequency_hz * baseline_sec;
const double baseline_sum =
2.0 - baseline_ticks * (baseline_kp + baseline_ki);
const double baseline_product =
1.0 - baseline_ticks * baseline_kp;
const double discriminant =
baseline_sum * baseline_sum - 4.0 * baseline_product;
const double period_ratio = update_period_sec / baseline_sec;
double target_sum = 0.0;
double target_product = 0.0;
if(discriminant >= 0.0)
{
double root = sqrt(discriminant);
double pole_a = 0.5 * (baseline_sum + root);
double pole_b = 0.5 * (baseline_sum - root);
if((pole_a < 0.0) || (pole_b < 0.0))
{
return false;
}
double target_a = pow(pole_a, period_ratio);
double target_b = pow(pole_b, period_ratio);
target_sum = target_a + target_b;
target_product = target_a * target_b;
}
else
{
if(baseline_product <= 0.0)
{
return false;
}
double radius = sqrt(baseline_product);
double cosine = baseline_sum / (2.0 * radius);
cosine = fmax(-1.0, fmin(1.0, cosine));
double angle = acos(cosine);
double target_radius = pow(radius, period_ratio);
target_sum =
2.0 * target_radius * cos(angle * period_ratio);
target_product = target_radius * target_radius;
}
double target_ticks = clock_frequency_hz * update_period_sec;
double kp = (1.0 - target_product) / target_ticks;
double ki = (1.0 - target_sum + target_product) / target_ticks;
if(!isfinite(kp) || !isfinite(ki) || (kp < 0.0) || (ki < 0.0))
{
return false;
}
*kp_q46_out = (int64_t)llround(kp * q46);
*ki_q46_out = (int64_t)llround(ki * q46);
*max_slew_q32_out = (uint32_t)llround(
parameters.max_slew_ppm * 1.0e-6 * q32);
return true;
}
const char* gps_pps_profile_name(gps_pps_profile_t profile)
soc/software/gps_system/src/gps/gps_pps_time_filter.c
+25 -3
View File
@@ -20,6 +20,7 @@
#define GPS_PPS_FILTER_INNOVATION_CLAMP_M 300.0
#define GPS_PPS_FILTER_HARD_OUTLIER_M 1500.0
#define GPS_PPS_SPEED_OF_LIGHT_MPS 299792458.0
#define GPS_PPS_FILTER_BASELINE_PERIOD_SEC 0.200
static double clamp_double(double value, double limit)
{
@@ -34,6 +35,21 @@ static double clamp_double(double value, double limit)
return value;
}
/* Preserve the baseline discrete pole when the navigation interval changes.
* Applying a baseline gain alpha N times is equivalent to one elapsed-time
* update with alpha_T = 1 - (1 - alpha)^N. This also handles an occasional
* missed solve without slowing the clock-bias or drift estimator. */
static double gps_pps_filter_elapsed_gain(
double baseline_gain, double elapsed_sec)
{
double periods = elapsed_sec / GPS_PPS_FILTER_BASELINE_PERIOD_SEC;
if(fabs(periods - 1.0) <= 1.0e-12)
{
return baseline_gain;
}
return -expm1(periods * log1p(-baseline_gain));
}
void gps_pps_time_filter_init(gps_pps_time_filter_t* filter)
{
if(filter != NULL)
@@ -269,14 +285,20 @@ gps_pps_time_filter_result_t gps_pps_time_filter_update(
return GPS_PPS_TIME_FILTER_REJECTED;
}
double effective_bias_gain =
gps_pps_filter_elapsed_gain(bias_gain, elapsed_sec);
double effective_drift_gain = gps_pps_filter_elapsed_gain(
GPS_PPS_FILTER_DRIFT_GAIN, elapsed_sec);
double effective_fallback_drift_gain = gps_pps_filter_elapsed_gain(
GPS_PPS_FILTER_FALLBACK_DRIFT_GAIN, elapsed_sec);
filter->clock_bias_m =
predicted_clock_bias_m +
bias_gain *
effective_bias_gain *
clamp_double(innovation_m, GPS_PPS_FILTER_INNOVATION_CLAMP_M);
if(clock_drift_valid)
{
filter->clock_drift_mps +=
GPS_PPS_FILTER_DRIFT_GAIN *
effective_drift_gain *
(measured_clock_drift_mps - filter->clock_drift_mps);
}
else
@@ -291,7 +313,7 @@ gps_pps_time_filter_result_t gps_pps_time_filter_update(
else
{
filter->clock_drift_mps +=
GPS_PPS_FILTER_FALLBACK_DRIFT_GAIN *
effective_fallback_drift_gain *
(fallback_drift_mps - filter->clock_drift_mps);
}
}