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

fix(gps): correct week-number handling across almanac, ephemeris and flash

Browse Source 
Persisting navigation data changed its lifetime from one power cycle to
weeks, and every piece of arithmetic that had only ever run over the
shorter lifetime turned out to be wrong. Each fix below reproduces in a
host test that fails against the previous behaviour.

Almanac (gps_almanac.c, gps_lnav_parser.c)

WNa was lifted against gps_ephems.toe.week, a field nothing ever writes,
so any WNa above 128 underflowed to ~4.29e9. The propagation then reduced
tk with a while-loop, turning one visibility refresh into 4.29e9
iterations - hours on the target, inside a 10 s periodic call. WNa is the
week modulo 256, so this armed at GPS week 2433 and then for 127 weeks in
every 256. Now lifted against tow.week in signed arithmetic, refused if
impossible, and the propagation is closed form.

The IS-GPS-200 20.3.3.5.2.3 half-week wrap was also applied to tk
unconditionally. It is a week-crossover correction, not a general
reduction: a GPS week is 14.0384 orbital periods, so aliasing one week
moves a satellite 5161 km, enough to report one 5.1 degrees below the
horizon as 5.7 above. The crossover guess now lives in
gps_almanac_assumed_week(), used only before page 25 arrives, and an
almanac too old to propagate yields no prediction rather than a wrong one.

Ephemeris age (gps_ephemeris_validity.c, gps_lnav_parser.c)

The curve-fit check compared two second-of-week values and never read a
week at all, so a record exactly N weeks old read as current. Harmless
while ephemeris lived in RAM; directly reachable once persisted across a
power-off. gps_resolve_toe_week() now reconstructs Toe's week at commit
time from the subframe 1 week number and the handover word; an
unresolvable week yields no age, which reads as a rejection.

A restored record could also outrank the live broadcast, because the Toe
ordering inverts past half a week. Flash records are now marked restored
and are candidates, never incumbents.

Flash wear (gps_nav_context_store.c, gps_interrupt_handlers.c)

The CRC wear policy never engaged: the payload carried the live WLS
position and a receiver timestamp nothing on the restore path read, so
every 60 s save was an erase - about 139 days to wear-out. The seed is
now held and refreshed only on 5 km of movement, at most hourly: one
erase per day instead of 1440.

Measurement model (NavEquations.c)

The elevation weighting was floored at 10 degrees, capping its term at
17.3 m where the model's own answer is 34.4 m at 5 degrees and 86 m at 2,
so a low satellite was trusted several times more than the model allowed.
Floor lowered to 3 degrees, which is where 1/sin must still be bounded
because a zero weight fails the whole epoch. Measured on a six-satellite
geometry, 30 m of uncorrected delay now moves the fix 4.18 m through a
4-degree satellite against 48.50 m through a 72-degree one.

Both atmospheric corrections switched off below the horizon, so a
satellite crossing it lost 78.8 m of tropospheric delay in one step.
Elevation is now clamped at the horizon for the models, and the
extrapolation is paid for in variance.

Also: gps_pps_select_reference() returned early on the preferred PRN,
leaving later non-finite capture times unscreened.

Checked and found correct, with regressions added: the epoch assembly
under satellite connect/disconnect and reassignment, coalesced captures,
the 32-bit RF sample counter wrap (pseudoranges agree to 3.3 um across
it), the refusal to publish a solvable subset of an incoherent epoch, and
the Klobuchar model against IS-GPS-200 20.3.3.5.2.5.

Tests: 22 host suites, 5 RTL testbenches, sanitizer clean.

Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
onur
2026-08-10 01:45:35 +03:00
parent 471ff12e70
commit f59d6ebb43
49 changed files with 12098 additions and 478 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/tests/test_gps_measurement_model.c
+428
View File
@@ -0,0 +1,428 @@
/*
* The measurement model: how much one satellite is allowed to move the fix.
*
* Three quantities were being conflated in this code and it is worth naming
* them here, because only one of them is a bias:
*
* - the corrections, tropo() and klobuchar_delay_meters(), which are
* subtracted from the observation to remove a real physical delay;
* - the sigma, gps_nav_measurement_sigma_m(), which is not a bias at all
* and never shifts anything - it only says how much to trust a satellite;
* - the WLS clock bias, the fourth unknown, which is solved for.
*
* The defects these tests pin down were in the first two: a weighting floor
* that stopped the sigma model working below 10 degrees, and corrections that
* switched themselves off below the horizon and left their whole magnitude in
* the measurement as an uncorrected bias.
*/
#include <math.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include "gps/gps_ip_defines.h"
#include "gps/NavEquations.h"
#include "gps/matrixLib.h"
static int failures;
#define CHECK(condition, message) do { \
if(!(condition)) { \
fprintf(stderr, "FAIL: %s\n", message); \
failures++; \
} \
} while(0)
#define URA_INDEX 2u /* 4.85 m, a typical broadcast value */
/*
* The sigma model has to keep working where a satellite is worst. The floor
* used to be 10 degrees, which pinned the elevation term at 17.3 m: at 5
* degrees the model's own answer is 34.4 m and at 2 degrees it is 86 m, so a
* low satellite was trusted 4 to 25 times more than this same model said it
* should be. Nothing was ever excluded by that floor - it only stopped the
* mapping from doing its job.
*/
static void test_sigma_keeps_degrading_below_ten_degrees(void)
{
const double elevations[] = {90.0, 45.0, 30.0, 15.0, 10.0, 7.0, 5.0, 3.0};
double previous = 0.0;
printf(" elevation -> sigma relative weight vs zenith\n");
double zenith = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true,
NAN, false);
for(unsigned int i = 0u; i < (sizeof(elevations) / sizeof(elevations[0]));
i++)
{
double sigma = gps_nav_measurement_sigma_m(
URA_INDEX, elevations[i], true, NAN, false);
printf(" %5.1f deg -> %8.1f m %8.4f\n",
elevations[i], sigma, (zenith * zenith) / (sigma * sigma));
CHECK(isfinite(sigma) && (sigma > 0.0),
"sigma must stay finite and positive at every elevation");
if(i != 0u)
{
CHECK(sigma > previous,
"sigma stopped growing as elevation fell");
}
previous = sigma;
}
/* The model's own numbers, now that nothing truncates them. */
double at_ten = gps_nav_measurement_sigma_m(URA_INDEX, 10.0, true,
NAN, false);
double at_five = gps_nav_measurement_sigma_m(URA_INDEX, 5.0, true,
NAN, false);
CHECK(at_five > (1.8 * at_ten),
"a five degree satellite is still weighted like a ten degree one");
printf(" 5 deg sigma %.1f m vs 10 deg %.1f m: %.1fx the variance\n",
at_five, at_ten, (at_five * at_five) / (at_ten * at_ten));
}
/*
* The floor exists because 1/sin diverges at the horizon and a zero weight is
* not a discounted satellite - least_squares_build_postfit() refuses a
* non-positive weight and the whole epoch fails. So however low a satellite
* goes, the sigma must stay finite, positive, and monotone.
*/
static void test_sigma_is_bounded_at_and_below_the_horizon(void)
{
const double elevations[] = {1.0, 0.5, 0.0, -1.0, -5.0, -20.0};
for(unsigned int i = 0u; i < (sizeof(elevations) / sizeof(elevations[0]));
i++)
{
double sigma = gps_nav_measurement_sigma_m(
URA_INDEX, elevations[i], true, NAN, false);
CHECK(isfinite(sigma) && (sigma > 0.0),
"sigma went non-finite or non-positive below the horizon");
}
/* Below the horizon the correction is extrapolated and the signal has
* probably arrived by reflection, so it must cost more than merely being
* low. */
double just_above = gps_nav_measurement_sigma_m(URA_INDEX, 0.5, true,
NAN, false);
double just_below = gps_nav_measurement_sigma_m(URA_INDEX, -0.5, true,
NAN, false);
CHECK(just_below > just_above,
"a satellite below the horizon was not charged for it");
/* A non-finite elevation is unknown, not good: it must not be treated as
* a healthy satellite. */
double unknown = gps_nav_measurement_sigma_m(URA_INDEX, NAN, true,
NAN, false);
double at_thirty = gps_nav_measurement_sigma_m(URA_INDEX, 30.0, true,
NAN, false);
CHECK(unknown > at_thirty,
"a non-finite elevation was treated as a good satellite");
printf(" horizon bounded: 0.5 deg %.1f m, -0.5 deg %.1f m, "
"unknown %.1f m\n", just_above, just_below, unknown);
}
/*
* DLL code-tracking jitter goes as the inverse square root of carrier-to-noise
* density in linear power, so 20 dB less signal is 100 times less power and
* exactly ten times the sigma. This checks the law rather than the constant.
*/
static void test_cn0_follows_the_tracking_law(void)
{
/* Isolate the C/N0 term by holding elevation at zenith. */
double base = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true,
45.0, true);
double minus20 = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true,
25.0, true);
double ura = 4.85;
double zen = 3.0;
double common = (ura * ura) + (zen * zen);
double term_base = sqrt((base * base) - common);
double term_minus20 = sqrt((minus20 * minus20) - common);
printf(" C/N0 45 dB-Hz -> %.2f m, 25 dB-Hz -> %.2f m (ratio %.2f)\n",
term_base, term_minus20, term_minus20 / term_base);
CHECK(fabs(term_base - 3.0) < 1e-9,
"the C/N0 term is not 3 m at 45 dB-Hz");
CHECK(fabs((term_minus20 / term_base) - 10.0) < 1e-6,
"20 dB of C/N0 did not scale sigma by ten");
/* A weaker satellite is always trusted less, never more. */
double previous = 0.0;
for(double cn0 = 50.0; cn0 >= 28.0; cn0 -= 2.0)
{
double sigma = gps_nav_measurement_sigma_m(URA_INDEX, 90.0, true,
cn0, true);
CHECK(sigma > previous, "sigma did not grow as C/N0 fell");
previous = sigma;
}
}
/*
* The correction discontinuity.
*
* tropo() used to be called only for elevation above zero and Klobuchar
* returned zero at or below it, so a satellite crossing the horizon lost its
* entire atmospheric correction in one step - tens of metres appearing in the
* measurement as an uncorrected bias, from one epoch to the next, for no
* physical reason. Both are now evaluated at the horizon instead.
*
* tropo() is the public half of that pair, so the continuity is checked
* directly on it; the ionospheric half is exercised through the solve below.
*/
static void test_troposphere_is_continuous_through_the_horizon(void)
{
const double dtr = 0.01745329251994329576923690768489;
double above = tropo(sin(0.05 * dtr), 0.9);
double at = tropo(sin(0.0), 0.9);
double below = tropo(sin(-0.05 * dtr), 0.9);
printf(" troposphere at +0.05/0.00/-0.05 deg: %.2f / %.2f / %.2f m\n",
above, at, below);
CHECK(isfinite(above) && isfinite(at) && isfinite(below),
"the tropospheric model went non-finite near the horizon");
/*
* The model is genuinely steep here - about 30 m per degree - so this is
* about continuity, not flatness. What used to happen is the whole value
* dropping to zero the moment elevation went non-positive.
*/
CHECK(fabs(above - at) < 5.0,
"the tropospheric model steps across the horizon");
CHECK(fabs(below - at) < 1.0e-9,
"the tropospheric model is not clamped at the horizon");
CHECK(at > 20.0,
"the horizon tropospheric delay is implausibly small; dropping it "
"would leave that much uncorrected bias");
printf(" -> below the horizon it holds %.1f m instead of dropping to 0\n",
below);
}
/* ------------------------------------------------------------------ */
/* End to end: what a biased low satellite does to the fix. */
/* ------------------------------------------------------------------ */
/* Receiver near 40 N, 33 E, on the ellipsoid. */
static const double RECEIVER[3] = {4121000.0, 2699000.0, 4037000.0};
#define TRUE_CLOCK_BIAS_M 12345.0
/*
* Place a satellite at a given azimuth/elevation as seen from RECEIVER, at
* GPS orbital radius, by walking out along the local line of sight.
*/
static void place_satellite(double azimuth_deg, double elevation_deg,
double out[3])
{
const double dtr = 0.01745329251994329576923690768489;
double r = sqrt((RECEIVER[0] * RECEIVER[0]) +
(RECEIVER[1] * RECEIVER[1]) +
(RECEIVER[2] * RECEIVER[2]));
double ground = sqrt((RECEIVER[0] * RECEIVER[0]) +
(RECEIVER[1] * RECEIVER[1]));
double sin_lat = RECEIVER[2] / r;
double cos_lat = ground / r;
double sin_lon = RECEIVER[1] / ground;
double cos_lon = RECEIVER[0] / ground;
double el = elevation_deg * dtr;
double az = azimuth_deg * dtr;
double up = sin(el);
double north = cos(el) * cos(az);
double east = cos(el) * sin(az);
/* Local east/north/up back into ECEF. */
double dx = (-sin_lon * east) + (-sin_lat * cos_lon * north) +
(cos_lat * cos_lon * up);
double dy = (cos_lon * east) + (-sin_lat * sin_lon * north) +
(cos_lat * sin_lon * up);
double dz = (cos_lat * north) + (sin_lat * up);
/* Range to a satellite at 26560 km geocentric radius on that bearing. */
double b = 2.0 * ((RECEIVER[0] * dx) + (RECEIVER[1] * dy) +
(RECEIVER[2] * dz));
double c = (r * r) - (26560000.0 * 26560000.0);
double range = (-b + sqrt((b * b) - (4.0 * c))) / 2.0;
out[0] = RECEIVER[0] + (range * dx);
out[1] = RECEIVER[1] + (range * dy);
out[2] = RECEIVER[2] + (range * dz);
}
/*
* Solve one epoch and report how far the fix ends up from the truth.
* bias_row/bias_m inject an uncorrected error on one satellite, which is what
* a satellite whose atmospheric correction was dropped actually looks like.
*/
static bool solve_epoch(const double azimuth[], const double elevation[],
unsigned int count, int bias_row, double bias_m,
double solved_ecef_m[3], double* solved_clock_m)
{
mat_type* satpos = create_matrix((int)count, 3, "satpos");
mat_type* obs = create_matrix(1, (int)count, "obs");
mat_type* az = create_matrix(1, (int)count, "az");
mat_type* el = create_matrix(1, (int)count, "el");
unsigned char ura[GPS_TRCK_IP_COUNT];
gps_nav_solution_t solution;
bool ok = false;
if((satpos == NULL) || (obs == NULL) || (az == NULL) || (el == NULL))
{
goto done;
}
initialize_all(0.0, satpos);
initialize_all(0.0, obs);
initialize_all(0.0, az);
initialize_all(0.0, el);
for(unsigned int i = 0u; i < count; i++)
{
double sat[3];
place_satellite(azimuth[i], elevation[i], sat);
for(unsigned int k = 0u; k < 3u; k++)
{
satpos->matrix[i][k] = sat[k];
}
double dx = sat[0] - RECEIVER[0];
double dy = sat[1] - RECEIVER[1];
double dz = sat[2] - RECEIVER[2];
double range = sqrt((dx * dx) + (dy * dy) + (dz * dz));
obs->matrix[0][i] = range + TRUE_CLOCK_BIAS_M;
if((bias_row >= 0) && ((unsigned int)bias_row == i))
{
obs->matrix[0][i] += bias_m;
}
ura[i] = URA_INDEX;
}
/* No ionosphere model and no C/N0, so the only thing separating these
* satellites is geometry and the elevation weighting under test. */
if(!leastSquaresSTD(satpos, obs, (int)count, az, el, ura, NULL,
NULL, 300000.0, &solution))
{
goto done;
}
for(unsigned int k = 0u; k < 3u; k++)
{
solved_ecef_m[k] = solution.ecef_m[k];
}
*solved_clock_m = solution.clock_bias_m;
ok = true;
done:
free_matrix(satpos);
free_matrix(obs);
free_matrix(az);
free_matrix(el);
return ok;
}
/*
* Six satellites, one of them at 4 degrees. Clean, the solve must recover the
* truth; with 30 m of uncorrected delay on the low one, the fix must move by
* much less than that, because the weighting now knows how little that
* satellite is worth.
*
* The 30 m is not arbitrary: it is roughly the tropospheric delay that used to
* be dropped outright the moment a satellite crossed the horizon.
*/
static double displacement(const double a[3], const double b[3])
{
double dx = a[0] - b[0];
double dy = a[1] - b[1];
double dz = a[2] - b[2];
return sqrt((dx * dx) + (dy * dy) + (dz * dz));
}
/*
* Six satellites, one of them at 4 degrees. Inject 30 m of uncorrected delay
* on one of them and measure how far the fix moves - the displacement between
* two solves, not the distance from truth, because the synthetic observations
* deliberately carry no atmosphere and the model's own correction is a common
* offset that says nothing about the question being asked.
*
* The 30 m is not arbitrary: it is roughly the tropospheric delay that used to
* be dropped outright the moment a satellite crossed the horizon.
*
* The property under test is that the solve is *less* sensitive to a bias on a
* satellite it should not trust than to the same bias on one it should. With
* the elevation term floored at 10 degrees the two were nearly equal, which is
* what made a setting satellite as damaging as an overhead one.
*/
static void test_a_biased_low_satellite_moves_the_fix_less_than_its_bias(void)
{
const double azimuth[6] = { 10.0, 75.0, 140.0, 205.0, 280.0, 330.0};
const double elevation[6] = { 72.0, 41.0, 28.0, 55.0, 33.0, 4.0};
double clean[3];
double clean_clock = 0.0;
if(!solve_epoch(azimuth, elevation, 6u, -1, 0.0, clean, &clean_clock))
{
fprintf(stderr, "FAIL: the clean epoch did not solve\n");
failures++;
return;
}
double low[3];
double low_clock = 0.0;
double high[3];
double high_clock = 0.0;
if(!solve_epoch(azimuth, elevation, 6u, 5, 30.0, low, &low_clock) ||
!solve_epoch(azimuth, elevation, 6u, 0, 30.0, high, &high_clock))
{
fprintf(stderr, "FAIL: a biased epoch did not solve\n");
failures++;
return;
}
double low_move = displacement(low, clean);
double high_move = displacement(high, clean);
printf(" 30 m of uncorrected delay moves the fix by %.2f m on the "
"4 deg satellite, %.2f m on the 72 deg one\n",
low_move, high_move);
printf(" and the clock bias by %.2f m / %.2f m\n",
fabs(low_clock - clean_clock), fabs(high_clock - clean_clock));
/*
* What the ten degree floor did to that same satellite: it pinned the
* elevation term at 3/sin(10 deg) instead of 3/sin(4 deg), so the
* satellite entered the normal equations with several times the weight
* this model says it has earned - and the fix moved proportionally more.
*/
{
const double dtr = 0.01745329251994329576923690768489;
double ura = 4.85;
double floored = sqrt((ura * ura) +
pow(3.0 / sin(10.0 * dtr), 2.0));
double honest = gps_nav_measurement_sigma_m(URA_INDEX, 4.0, true,
NAN, false);
printf(" the 10 deg floor gave that satellite sigma %.1f m instead "
"of %.1f m: %.1fx the weight\n",
floored, honest, (honest * honest) / (floored * floored));
CHECK(honest > floored,
"the floor being regressed against no longer reproduces");
}
CHECK(low_move > 0.0, "the biased solve did not move at all");
CHECK(low_move < high_move,
"a bias on the lowest satellite pulled the fix at least as hard as "
"the same bias on the highest one");
CHECK(low_move < 30.0,
"a 30 m bias on the lowest satellite moved the fix by more than the "
"bias itself");
}
int main(void)
{
test_sigma_keeps_degrading_below_ten_degrees();
test_sigma_is_bounded_at_and_below_the_horizon();
test_cn0_follows_the_tracking_law();
test_troposphere_is_continuous_through_the_horizon();
test_a_biased_low_satellite_moves_the_fix_less_than_its_bias();
if(failures != 0)
{
fprintf(stderr, "%d check(s) failed\n", failures);
return 1;
}
printf("gps measurement model tests passed\n");
return 0;
}