fix(gps): correct week-number handling across almanac, ephemeris and flash
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
@@ -11,6 +11,15 @@ TARGETS += test_gps_satellite_velocity
|
||||
TARGETS += test_gps_pps_time_filter
|
||||
TARGETS += test_gps_pps_quality
|
||||
TARGETS += test_gps_pps_adaptive_loop
|
||||
TARGETS += test_gps_ephemeris_validity
|
||||
TARGETS += test_gps_warm_reacquisition_sim
|
||||
TARGETS += test_gps_ephemeris_store
|
||||
TARGETS += test_gps_lnav_iode
|
||||
TARGETS += test_gps_almanac
|
||||
TARGETS += test_gps_nav_context_store
|
||||
TARGETS += test_gps_nav_context_audit
|
||||
TARGETS += test_gps_pvt_consistency
|
||||
TARGETS += test_gps_measurement_model
|
||||
|
||||
.PHONY: all run sanitize clean
|
||||
|
||||
@@ -34,13 +43,66 @@ test_plic_warm_reset: test_plic_warm_reset.c $(ROOT)/../bsp/drivers/plic.c
|
||||
test_gps_acq_manager: test_gps_acq_manager.c $(ROOT)/src/gps/gps_acq_manager.c
|
||||
$(CC) $(CFLAGS) -I$(ROOT)/header $^ -o $@
|
||||
|
||||
test_gps_ephemeris_cache: test_gps_ephemeris_cache.c $(ROOT)/src/gps/gps_ephemeris_cache.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -o $@
|
||||
test_gps_ephemeris_cache: test_gps_ephemeris_cache.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_cache.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_validity.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_ephemeris_validity: test_gps_ephemeris_validity.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_validity.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_lnav_iode: test_gps_lnav_iode.c \
|
||||
$(ROOT)/src/gps/gps_lnav_parser.c \
|
||||
$(ROOT)/src/gps/gps_almanac.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_almanac: test_gps_almanac.c \
|
||||
$(ROOT)/src/gps/gps_almanac.c
|
||||
$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_nav_context_store: test_gps_nav_context_store.c \
|
||||
$(ROOT)/src/gps/gps_nav_context_store.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_store.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_validity.c \
|
||||
$(ROOT)/src/gps/gps_lnav_parser.c \
|
||||
$(ROOT)/src/gps/gps_almanac.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_nav_context_audit: test_gps_nav_context_audit.c \
|
||||
$(ROOT)/src/gps/gps_nav_context_store.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_store.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_cache.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_validity.c \
|
||||
$(ROOT)/src/gps/gps_lnav_parser.c \
|
||||
$(ROOT)/src/gps/gps_almanac.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_pvt_consistency: test_gps_pvt_consistency.c \
|
||||
$(ROOT)/src/gps/gps_nav_epoch.c \
|
||||
$(ROOT)/src/gps/gps_acq_manager.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_cache.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_validity.c \
|
||||
$(ROOT)/src/gps/gps_pps_time_filter.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_ephemeris_store: test_gps_ephemeris_store.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_store.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_validity.c \
|
||||
$(ROOT)/src/gps/gps_lnav_parser.c \
|
||||
$(ROOT)/src/gps/gps_almanac.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_warm_reacquisition_sim: test_gps_warm_reacquisition_sim.c \
|
||||
$(ROOT)/src/gps/gps_time_anchor.c \
|
||||
$(ROOT)/src/gps/gps_nav_epoch.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_receiver_hardware_sim: test_gps_receiver_hardware_sim.c \
|
||||
$(ROOT)/src/gps/gps_acq_manager.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_cache.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -o $@
|
||||
$(ROOT)/src/gps/gps_ephemeris_cache.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_validity.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_tracking_measurement: test_gps_tracking_measurement.c \
|
||||
$(ROOT)/src/gps/gps_tracking_measurement.c
|
||||
@@ -53,6 +115,13 @@ test_gps_satellite_velocity: test_gps_satellite_velocity.c \
|
||||
-Wno-unused-variable -Wno-sign-compare \
|
||||
-Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_measurement_model: test_gps_measurement_model.c \
|
||||
$(ROOT)/src/gps/NavEquations.c \
|
||||
$(ROOT)/src/gps/matrixLib.c
|
||||
$(CC) $(CFLAGS) -Wno-comment -Wno-unused-parameter \
|
||||
-Wno-unused-variable -Wno-sign-compare \
|
||||
-Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_gps_pps_time_filter: test_gps_pps_time_filter.c \
|
||||
$(ROOT)/src/gps/gps_pps_time_filter.c
|
||||
$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
|
||||
@@ -74,9 +143,18 @@ run: $(TARGETS)
|
||||
./test_plic_warm_reset
|
||||
./test_gps_acq_manager
|
||||
./test_gps_ephemeris_cache
|
||||
./test_gps_ephemeris_validity
|
||||
./test_gps_warm_reacquisition_sim
|
||||
./test_gps_ephemeris_store
|
||||
./test_gps_lnav_iode
|
||||
./test_gps_almanac
|
||||
./test_gps_nav_context_store
|
||||
./test_gps_nav_context_audit
|
||||
./test_gps_pvt_consistency
|
||||
./test_gps_receiver_hardware_sim
|
||||
./test_gps_tracking_measurement
|
||||
./test_gps_satellite_velocity
|
||||
./test_gps_measurement_model
|
||||
./test_gps_pps_time_filter
|
||||
./test_gps_pps_quality
|
||||
./test_gps_pps_adaptive_loop
|
||||
@@ -115,15 +193,34 @@ sanitize:
|
||||
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
|
||||
test_gps_ephemeris_cache.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_cache.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_validity.c -lm \
|
||||
-o test_gps_ephemeris_cache_san
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 ./test_gps_ephemeris_cache_san
|
||||
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
|
||||
test_gps_ephemeris_validity.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_validity.c -lm \
|
||||
-o test_gps_ephemeris_validity_san
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 ./test_gps_ephemeris_validity_san
|
||||
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
|
||||
test_gps_warm_reacquisition_sim.c \
|
||||
$(ROOT)/src/gps/gps_time_anchor.c \
|
||||
$(ROOT)/src/gps/gps_nav_epoch.c -lm \
|
||||
-o test_gps_warm_reacquisition_sim_san
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 ./test_gps_warm_reacquisition_sim_san
|
||||
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
|
||||
-fsanitize=address,undefined -fno-omit-frame-pointer \
|
||||
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
|
||||
test_gps_receiver_hardware_sim.c \
|
||||
$(ROOT)/src/gps/gps_acq_manager.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_cache.c \
|
||||
$(ROOT)/src/gps/gps_ephemeris_validity.c -lm \
|
||||
-o test_gps_receiver_hardware_sim_san
|
||||
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
|
||||
UBSAN_OPTIONS=halt_on_error=1 \
|
||||
@@ -177,6 +274,8 @@ clean:
|
||||
test_gps_time_anchor_san \
|
||||
test_gps_acq_manager_san test_gps_ephemeris_cache_san \
|
||||
test_gps_receiver_hardware_sim_san \
|
||||
test_gps_ephemeris_validity_san \
|
||||
test_gps_warm_reacquisition_sim_san \
|
||||
test_gps_tracking_measurement_san \
|
||||
test_gps_satellite_velocity_san \
|
||||
test_gps_pps_time_filter_san test_gps_pps_quality_san \
|
||||
|
||||
@@ -132,8 +132,199 @@ static void test_failed_hot_search_is_backed_off(void)
|
||||
assert(start_next(&manager, 35u, true) == 20u);
|
||||
}
|
||||
|
||||
/*
|
||||
* Root cause found on the board: a satellite tracked seconds ago was served the
|
||||
* 30 s hot backoff from its very first retry, while a never-seen cold PRN was
|
||||
* retried every 0.5 s. The priority was inverted - the best candidate was
|
||||
* penalised sixty times harder than the worst. Recency must buy fast retries
|
||||
* that decay with failures, not be discarded immediately.
|
||||
*/
|
||||
static void a_freshly_lost_prn_is_retried_fast_before_backing_off(void)
|
||||
{
|
||||
gps_acq_manager_t manager;
|
||||
const gps_acq_manager_config_t config = {
|
||||
.retry_delay_ticks = 5u, /* fast cadence */
|
||||
.hot_retry_delay_ticks = 300u, /* long backoff */
|
||||
.hot_window_ticks = 7200u,
|
||||
.max_hot_streak = 4u,
|
||||
.hot_fast_attempts = 3u
|
||||
};
|
||||
gps_acq_manager_init(&manager, &config);
|
||||
assert(gps_acq_manager_enable_cold(&manager, 7u));
|
||||
|
||||
uint64_t now = 1000u;
|
||||
unsigned int prn = 0u;
|
||||
bool hot = false;
|
||||
|
||||
/* Establish it as tracked, then lose it. */
|
||||
assert(gps_acq_manager_start_next(&manager, now, &prn, &hot));
|
||||
gps_acq_manager_mark_acquired(&manager, prn);
|
||||
gps_acq_manager_mark_tracking(&manager, prn);
|
||||
gps_acq_manager_mark_lost(&manager, 7u, now);
|
||||
|
||||
/* The first few retries must come at the fast cadence, not after 300. */
|
||||
for(unsigned int attempt = 0u; attempt < config.hot_fast_attempts;
|
||||
attempt++)
|
||||
{
|
||||
assert(gps_acq_manager_start_next(&manager, now, &prn, &hot));
|
||||
assert(prn == 7u);
|
||||
assert(hot);
|
||||
gps_acq_manager_mark_failed(&manager, 7u, now);
|
||||
now += config.retry_delay_ticks;
|
||||
}
|
||||
|
||||
/* Allowance spent: it must now observe the long backoff. */
|
||||
bool served_early = false;
|
||||
for(unsigned int i = 0u; i < 5u; i++)
|
||||
{
|
||||
if(gps_acq_manager_start_next(&manager, now, &prn, &hot) &&
|
||||
(prn == 7u) && hot)
|
||||
{
|
||||
served_early = true;
|
||||
}
|
||||
now += config.retry_delay_ticks;
|
||||
}
|
||||
assert(!served_early);
|
||||
|
||||
/* After the backoff it becomes a candidate again. */
|
||||
now += config.hot_retry_delay_ticks;
|
||||
bool served_late = false;
|
||||
for(unsigned int i = 0u; i < 40u; i++)
|
||||
{
|
||||
if(gps_acq_manager_start_next(&manager, now, &prn, &hot) &&
|
||||
(prn == 7u))
|
||||
{
|
||||
served_late = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
assert(served_late);
|
||||
}
|
||||
|
||||
/*
|
||||
* Almanac-directed acquisition. With one acquisition engine, sweeping PRNs
|
||||
* that are below the horizon is what starves the satellites that are actually
|
||||
* up: 75% of failed searches on the board were against below-horizon PRNs.
|
||||
* The visibility prediction must bias the scheduler hard, yet remain purely
|
||||
* advisory - it can never make a real satellite unreachable.
|
||||
*/
|
||||
static void test_invisible_prn_is_slowed_but_never_excluded(void)
|
||||
{
|
||||
const uint64_t retry = 10u;
|
||||
const uint64_t unlikely = 500u;
|
||||
gps_acq_manager_t manager;
|
||||
const gps_acq_manager_config_t config = {
|
||||
.retry_delay_ticks = retry,
|
||||
.hot_retry_delay_ticks = 1000u,
|
||||
.hot_window_ticks = 100u,
|
||||
.max_hot_streak = 4u,
|
||||
.hot_fast_attempts = 1u,
|
||||
.unlikely_retry_delay_ticks = unlikely
|
||||
};
|
||||
gps_acq_manager_init(&manager, &config);
|
||||
assert(gps_acq_manager_search_mask(&manager) == 0xffffffffu);
|
||||
|
||||
for(unsigned int prn = 0u; prn < GPS_ACQ_MANAGER_PRN_COUNT; prn++)
|
||||
{
|
||||
assert(gps_acq_manager_enable_cold(&manager, prn));
|
||||
}
|
||||
|
||||
/* Only PRN index 3 is predicted visible. */
|
||||
gps_acq_manager_set_search_mask(&manager, 1u << 3);
|
||||
|
||||
/*
|
||||
* Early on, every search must go to the one visible PRN rather than round
|
||||
* robin over the whole constellation.
|
||||
*/
|
||||
for(unsigned int attempt = 0u; attempt < 5u; attempt++)
|
||||
{
|
||||
uint64_t now = 1u + (attempt * retry);
|
||||
unsigned int prn = GPS_ACQ_MANAGER_INVALID_PRN;
|
||||
bool hot = true;
|
||||
assert(gps_acq_manager_start_next(&manager, now, &prn, &hot));
|
||||
assert(prn == 3u);
|
||||
gps_acq_manager_mark_failed(&manager, prn, now);
|
||||
}
|
||||
|
||||
/*
|
||||
* But an invisible PRN must still come round eventually, so a stale
|
||||
* almanac or a stale position self-corrects instead of blinding the
|
||||
* receiver permanently.
|
||||
*/
|
||||
bool saw_invisible = false;
|
||||
for(uint64_t now = unlikely; now <= (unlikely * 6u); now += retry)
|
||||
{
|
||||
unsigned int prn = GPS_ACQ_MANAGER_INVALID_PRN;
|
||||
bool hot = true;
|
||||
if(!gps_acq_manager_start_next(&manager, now, &prn, &hot))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
gps_acq_manager_mark_failed(&manager, prn, now);
|
||||
if(prn != 3u)
|
||||
{
|
||||
saw_invisible = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
assert(saw_invisible);
|
||||
puts(" invisible PRNs are slowed but still revisited");
|
||||
}
|
||||
|
||||
static void test_empty_mask_falls_back_to_searching_everything(void)
|
||||
{
|
||||
gps_acq_manager_t manager = make_manager(10u, 1000u, 100u, 4u);
|
||||
for(unsigned int prn = 0u; prn < GPS_ACQ_MANAGER_PRN_COUNT; prn++)
|
||||
{
|
||||
assert(gps_acq_manager_enable_cold(&manager, prn));
|
||||
}
|
||||
/*
|
||||
* A prediction that nothing is visible is always wrong, and honouring it
|
||||
* would stall acquisition completely. Degrade to the blind sweep.
|
||||
*/
|
||||
gps_acq_manager_set_search_mask(&manager, 0u);
|
||||
assert(gps_acq_manager_search_mask(&manager) == 0xffffffffu);
|
||||
|
||||
unsigned int prn = GPS_ACQ_MANAGER_INVALID_PRN;
|
||||
bool hot = true;
|
||||
assert(gps_acq_manager_start_next(&manager, 1u, &prn, &hot));
|
||||
puts(" empty visibility mask falls back to searching everything");
|
||||
}
|
||||
|
||||
static void test_visibility_does_not_disturb_a_visible_prn(void)
|
||||
{
|
||||
const uint64_t retry = 10u;
|
||||
gps_acq_manager_t manager;
|
||||
const gps_acq_manager_config_t config = {
|
||||
.retry_delay_ticks = retry,
|
||||
.hot_retry_delay_ticks = 1000u,
|
||||
.hot_window_ticks = 100u,
|
||||
.max_hot_streak = 4u,
|
||||
.hot_fast_attempts = 4u,
|
||||
.unlikely_retry_delay_ticks = 500u
|
||||
};
|
||||
gps_acq_manager_init(&manager, &config);
|
||||
assert(gps_acq_manager_enable_cold(&manager, 5u));
|
||||
gps_acq_manager_set_search_mask(&manager, 1u << 5);
|
||||
|
||||
/* A visible PRN keeps the fast cold cadence exactly as before. */
|
||||
unsigned int prn = start_next(&manager, 1u, false);
|
||||
assert(prn == 5u);
|
||||
gps_acq_manager_mark_failed(&manager, prn, 1u);
|
||||
prn = GPS_ACQ_MANAGER_INVALID_PRN;
|
||||
bool hot = true;
|
||||
assert(!gps_acq_manager_start_next(&manager, 1u + retry - 1u, &prn, &hot));
|
||||
assert(gps_acq_manager_start_next(&manager, 1u + retry, &prn, &hot));
|
||||
assert(prn == 5u);
|
||||
puts(" visible PRNs keep the fast cadence");
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_invisible_prn_is_slowed_but_never_excluded();
|
||||
test_empty_mask_falls_back_to_searching_everything();
|
||||
test_visibility_does_not_disturb_a_visible_prn();
|
||||
a_freshly_lost_prn_is_retried_fast_before_backing_off();
|
||||
test_cold_round_robin_and_retry_from_tick_zero();
|
||||
test_hot_priority_round_robin_and_cold_fairness();
|
||||
test_expired_hot_becomes_cold_and_tracking_is_excluded();
|
||||
|
||||
@@ -0,0 +1,364 @@
|
||||
/*
|
||||
* Almanac decoder, orbital propagation and visibility-mask tests.
|
||||
*
|
||||
* The reference vector is a real broadcast almanac (PRN 1, GPS week 2431)
|
||||
* quantised to the LNAV representation, with the expected ECEF position
|
||||
* produced by an independent Python implementation of the IS-GPS-200
|
||||
* user algorithm. That checks the bit layout and the propagation against
|
||||
* something other than this code's own arithmetic.
|
||||
*/
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "gps/gps_almanac.h"
|
||||
|
||||
/* -std=c11 does not expose TEST_PI. */
|
||||
#define TEST_PI 3.1415926535897932
|
||||
|
||||
/*
|
||||
* Place a field into a 30-bit LNAV word. first_bit is 1-based from the MSB
|
||||
* of the word and the low six bits are parity, matching IS-GPS-200 and the
|
||||
* decoder under test.
|
||||
*/
|
||||
static void put_field(unsigned int* word, unsigned int first_bit,
|
||||
unsigned int length, uint32_t value)
|
||||
{
|
||||
unsigned int shift = 30u - (first_bit + length - 1u);
|
||||
uint32_t mask = (length >= 32u) ? 0xffffffffu : ((1u << length) - 1u);
|
||||
*word &= ~(mask << shift);
|
||||
*word |= (value & mask) << shift;
|
||||
}
|
||||
|
||||
static void build_almanac_page(unsigned int subframe[10],
|
||||
unsigned char subframe_id,
|
||||
unsigned char sv_id,
|
||||
uint32_t ecc, uint32_t toa, int32_t delta_i,
|
||||
int32_t omega_dot, uint32_t health,
|
||||
uint32_t sqrt_a, int32_t omega0,
|
||||
int32_t omega, int32_t m0,
|
||||
int32_t af0, int32_t af1)
|
||||
{
|
||||
memset(subframe, 0, sizeof(unsigned int) * 10u);
|
||||
/* Word 2 carries the subframe ID at bits 20..22. */
|
||||
put_field(&subframe[1], 20u, 3u, subframe_id);
|
||||
put_field(&subframe[2], 1u, 2u, 1u); /* data ID = 1 (LNAV) */
|
||||
put_field(&subframe[2], 3u, 6u, sv_id);
|
||||
put_field(&subframe[2], 9u, 16u, ecc);
|
||||
put_field(&subframe[3], 1u, 8u, toa);
|
||||
put_field(&subframe[3], 9u, 16u, (uint32_t)delta_i);
|
||||
put_field(&subframe[4], 1u, 16u, (uint32_t)omega_dot);
|
||||
put_field(&subframe[4], 17u, 8u, health);
|
||||
put_field(&subframe[5], 1u, 24u, sqrt_a);
|
||||
put_field(&subframe[6], 1u, 24u, (uint32_t)omega0);
|
||||
put_field(&subframe[7], 1u, 24u, (uint32_t)omega);
|
||||
put_field(&subframe[8], 1u, 24u, (uint32_t)m0);
|
||||
/* af0 is split: bits 1..8 are the MSBs, bits 20..22 the LSBs. */
|
||||
put_field(&subframe[9], 1u, 8u, ((uint32_t)af0 >> 3) & 0xffu);
|
||||
put_field(&subframe[9], 9u, 11u, (uint32_t)af1);
|
||||
put_field(&subframe[9], 20u, 3u, (uint32_t)af0 & 0x7u);
|
||||
}
|
||||
|
||||
/* Real broadcast almanac for PRN 1, quantised to the LNAV fields. */
|
||||
#define REF_ECC 3902
|
||||
#define REF_TOA 36
|
||||
#define REF_DELTA_I 2439
|
||||
#define REF_OMEGA_DOT (-691)
|
||||
#define REF_SQRT_A 10554475
|
||||
#define REF_OMEGA0 677285
|
||||
#define REF_OMEGA 459593
|
||||
#define REF_M0 301402
|
||||
#define REF_WEEK 2431u
|
||||
#define REF_TOW 550000.0
|
||||
|
||||
static void build_reference_page(unsigned int subframe[10], unsigned char sv_id,
|
||||
uint32_t health)
|
||||
{
|
||||
build_almanac_page(subframe, 5u, sv_id, REF_ECC, REF_TOA, REF_DELTA_I,
|
||||
REF_OMEGA_DOT, health, REF_SQRT_A, REF_OMEGA0,
|
||||
REF_OMEGA, REF_M0, -5, 3);
|
||||
}
|
||||
|
||||
static void test_field_decoding(void)
|
||||
{
|
||||
unsigned int subframe[10];
|
||||
gps_almanac_t almanac;
|
||||
gps_almanac_reset(&almanac);
|
||||
build_reference_page(subframe, 1u, 0u);
|
||||
|
||||
assert(gps_almanac_store_page(&almanac, subframe, 5u));
|
||||
const gps_almanac_entry_t* e = &almanac.sv[0];
|
||||
assert(e->valid);
|
||||
assert(e->prn == 1u);
|
||||
assert(e->health == 0u);
|
||||
|
||||
assert(fabs(e->ecc - (REF_ECC * ldexp(1.0, -21))) < 1e-15);
|
||||
assert(fabs(e->toa_sec - (REF_TOA * 4096.0)) < 1e-9);
|
||||
assert(fabs(e->sqrt_a - (REF_SQRT_A * ldexp(1.0, -11))) < 1e-9);
|
||||
/* Negative OMEGADOT must survive sign extension of a 16-bit field. */
|
||||
assert(e->omega_dot_rps < 0.0);
|
||||
assert(fabs(e->omega_dot_rps -
|
||||
(REF_OMEGA_DOT * ldexp(1.0, -38) * TEST_PI)) < 1e-18);
|
||||
/* Inclination is an offset from 0.30 semicircles, not an absolute value. */
|
||||
assert(fabs(e->i0_rad -
|
||||
((0.30 + (REF_DELTA_I * ldexp(1.0, -19))) * TEST_PI)) < 1e-12);
|
||||
/* af0 is reassembled from two separated bit groups. */
|
||||
assert(fabs(e->af0_sec - (-5.0 * ldexp(1.0, -20))) < 1e-18);
|
||||
assert(fabs(e->af1_sec_per_sec - (3.0 * ldexp(1.0, -38))) < 1e-24);
|
||||
assert(gps_almanac_valid_count(&almanac) == 1u);
|
||||
printf(" field decoding ok\n");
|
||||
}
|
||||
|
||||
static void test_propagation_against_reference(void)
|
||||
{
|
||||
unsigned int subframe[10];
|
||||
gps_almanac_t almanac;
|
||||
gps_almanac_reset(&almanac);
|
||||
build_reference_page(subframe, 1u, 0u);
|
||||
assert(gps_almanac_store_page(&almanac, subframe, 5u));
|
||||
|
||||
double ecef[3];
|
||||
assert(gps_almanac_satellite_ecef(&almanac.sv[0], REF_WEEK, REF_WEEK,
|
||||
REF_TOW, ecef));
|
||||
|
||||
/*
|
||||
* Independent Python implementation of the same user algorithm, with tk as
|
||||
* the actual elapsed time since toa - here 402544 s, or 4.66 days.
|
||||
*
|
||||
* This vector replaces an earlier one generated with tk wrapped into the
|
||||
* half week. That wrap was a misreading of IS-GPS-200 20.3.3.5.2.3, which
|
||||
* is a week-crossover correction rather than a general reduction, and it
|
||||
* put this satellite 5161 km from where it actually is - evaluating the
|
||||
* orbit 2.3 days *before* toa instead of 4.7 days after it.
|
||||
*/
|
||||
const double expected[3] = {19464181.4246, 11780273.3193, 13764877.9908};
|
||||
for(unsigned int i = 0u; i < 3u; i++)
|
||||
{
|
||||
double error = fabs(ecef[i] - expected[i]);
|
||||
if(error > 1.0)
|
||||
{
|
||||
printf(" axis %u: got %.4f expected %.4f (error %.4f m)\n",
|
||||
i, ecef[i], expected[i], error);
|
||||
}
|
||||
assert(error <= 1.0);
|
||||
}
|
||||
|
||||
double radius = sqrt((ecef[0] * ecef[0]) + (ecef[1] * ecef[1]) +
|
||||
(ecef[2] * ecef[2]));
|
||||
assert(fabs(radius - 26591.4e3) < 5.0e3);
|
||||
printf(" propagation matches reference within 1 m\n");
|
||||
}
|
||||
|
||||
static void test_week_rollover_is_symmetric(void)
|
||||
{
|
||||
unsigned int subframe[10];
|
||||
gps_almanac_t almanac;
|
||||
gps_almanac_reset(&almanac);
|
||||
build_reference_page(subframe, 1u, 0u);
|
||||
assert(gps_almanac_store_page(&almanac, subframe, 5u));
|
||||
|
||||
/*
|
||||
* The same instant expressed as (week, tow) or (week+1, tow-604800) must
|
||||
* propagate to the same place, otherwise a week boundary would step the
|
||||
* predicted satellite position and briefly corrupt the visible set.
|
||||
*/
|
||||
double a[3];
|
||||
double b[3];
|
||||
assert(gps_almanac_satellite_ecef(&almanac.sv[0], REF_WEEK, REF_WEEK,
|
||||
600000.0, a));
|
||||
assert(gps_almanac_satellite_ecef(&almanac.sv[0], REF_WEEK, REF_WEEK + 1u,
|
||||
600000.0 - 604800.0, b));
|
||||
for(unsigned int i = 0u; i < 3u; i++)
|
||||
{
|
||||
assert(fabs(a[i] - b[i]) < 1.0);
|
||||
}
|
||||
printf(" week rollover consistent\n");
|
||||
}
|
||||
|
||||
static void test_page_rejection(void)
|
||||
{
|
||||
unsigned int subframe[10];
|
||||
gps_almanac_t almanac;
|
||||
gps_almanac_reset(&almanac);
|
||||
|
||||
/* SV ID 1..24 belongs to subframe 5; the same page on subframe 4 is not
|
||||
* an almanac page and must not be stored. */
|
||||
build_reference_page(subframe, 1u, 0u);
|
||||
assert(!gps_almanac_store_page(&almanac, subframe, 4u));
|
||||
|
||||
/* SV ID 56 on subframe 4 is the ionosphere page, not an almanac. */
|
||||
build_almanac_page(subframe, 4u, 56u, REF_ECC, REF_TOA, REF_DELTA_I,
|
||||
REF_OMEGA_DOT, 0u, REF_SQRT_A, REF_OMEGA0,
|
||||
REF_OMEGA, REF_M0, 0, 0);
|
||||
assert(!gps_almanac_store_page(&almanac, subframe, 4u));
|
||||
|
||||
/* An all-zero (never uploaded) page has sqrt_a = 0 and must be rejected
|
||||
* rather than stored as an entry that divides by zero when propagated. */
|
||||
build_almanac_page(subframe, 5u, 7u, 0u, 0u, 0, 0, 0u, 0u, 0, 0, 0, 0, 0);
|
||||
assert(!gps_almanac_store_page(&almanac, subframe, 5u));
|
||||
|
||||
assert(gps_almanac_valid_count(&almanac) == 0u);
|
||||
printf(" page rejection ok\n");
|
||||
}
|
||||
|
||||
/* Receiver position used for the visibility tests (Bolu, Turkey). */
|
||||
static void receiver_ecef(double out[3])
|
||||
{
|
||||
const double lat = 40.735696 * TEST_PI / 180.0;
|
||||
const double lon = 31.598649 * TEST_PI / 180.0;
|
||||
const double h = 803.0;
|
||||
const double a = 6378137.0;
|
||||
const double f = 1.0 / 298.257223563;
|
||||
const double e2 = f * (2.0 - f);
|
||||
double n = a / sqrt(1.0 - (e2 * sin(lat) * sin(lat)));
|
||||
out[0] = (n + h) * cos(lat) * cos(lon);
|
||||
out[1] = (n + h) * cos(lat) * sin(lon);
|
||||
out[2] = ((n * (1.0 - e2)) + h) * sin(lat);
|
||||
}
|
||||
|
||||
static void test_visibility_fallbacks(void)
|
||||
{
|
||||
gps_almanac_t almanac;
|
||||
uint32_t mask = 0u;
|
||||
double rx[3];
|
||||
receiver_ecef(rx);
|
||||
|
||||
/* No almanac at all: everything stays searchable. */
|
||||
gps_almanac_reset(&almanac);
|
||||
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
|
||||
&mask) == 0u);
|
||||
assert(mask == 0xffffffffu);
|
||||
|
||||
/*
|
||||
* Almanac entries but no subframe 5 page 25 yet: visibility must still be
|
||||
* usable, falling back to the current week. Page 25 comes round only
|
||||
* once per 12.5 minutes and waiting for it would leave the receiver
|
||||
* sweeping blindly for that whole window after a reset.
|
||||
*/
|
||||
unsigned int subframe[10];
|
||||
build_reference_page(subframe, 1u, 0u);
|
||||
assert(gps_almanac_store_page(&almanac, subframe, 5u));
|
||||
assert(!almanac.reference_valid);
|
||||
mask = 0u;
|
||||
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
|
||||
&mask) == 1u);
|
||||
assert((mask & 1u) != 0u);
|
||||
|
||||
/*
|
||||
* And that fallback must agree with the explicit reference when the
|
||||
* reference does arrive, for an almanac of the current week.
|
||||
*/
|
||||
uint32_t mask_with_reference = 0u;
|
||||
gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
|
||||
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
|
||||
&mask_with_reference) == 1u);
|
||||
assert(mask_with_reference == mask);
|
||||
gps_almanac_reset(&almanac);
|
||||
assert(gps_almanac_store_page(&almanac, subframe, 5u));
|
||||
|
||||
/* No receiver position: all searchable. */
|
||||
gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
|
||||
mask = 0u;
|
||||
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, NULL, 0.0,
|
||||
&mask) == 0u);
|
||||
assert(mask == 0xffffffffu);
|
||||
printf(" visibility fallbacks ok\n");
|
||||
}
|
||||
|
||||
static void test_visibility_selects_and_never_hides_unknowns(void)
|
||||
{
|
||||
gps_almanac_t almanac;
|
||||
unsigned int subframe[10];
|
||||
uint32_t mask = 0u;
|
||||
double rx[3];
|
||||
receiver_ecef(rx);
|
||||
|
||||
gps_almanac_reset(&almanac);
|
||||
gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
|
||||
build_reference_page(subframe, 1u, 0u);
|
||||
assert(gps_almanac_store_page(&almanac, subframe, 5u));
|
||||
|
||||
/* PRN 1 is above the horizon for this place and time (verified against
|
||||
* the same almanac independently), so bit 0 must be set. */
|
||||
double elevation = 0.0;
|
||||
assert(gps_almanac_look_angles(&almanac.sv[0], REF_WEEK, REF_WEEK,
|
||||
REF_TOW, rx, &elevation, NULL));
|
||||
assert(elevation > 0.0);
|
||||
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
|
||||
&mask) >= 1u);
|
||||
assert((mask & 1u) != 0u);
|
||||
|
||||
/* Every PRN with no almanac entry yet must stay searchable, or a
|
||||
* partially collected almanac would hide satellites we simply have not
|
||||
* heard about. */
|
||||
for(unsigned int prn_index = 1u; prn_index < 32u; prn_index++)
|
||||
{
|
||||
assert((mask & (1u << prn_index)) != 0u);
|
||||
}
|
||||
|
||||
/* An unhealthy satellite is dropped from the visible set even though it
|
||||
* is geometrically up. */
|
||||
gps_almanac_reset(&almanac);
|
||||
gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
|
||||
build_reference_page(subframe, 1u, 0x3fu);
|
||||
assert(gps_almanac_store_page(&almanac, subframe, 5u));
|
||||
mask = 0xffffffffu;
|
||||
assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
|
||||
&mask) == 0u);
|
||||
assert((mask & 1u) == 0u);
|
||||
printf(" visibility selection ok\n");
|
||||
}
|
||||
|
||||
static void test_satellite_sets_below_and_rises(void)
|
||||
{
|
||||
gps_almanac_t almanac;
|
||||
unsigned int subframe[10];
|
||||
double rx[3];
|
||||
receiver_ecef(rx);
|
||||
gps_almanac_reset(&almanac);
|
||||
gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
|
||||
build_reference_page(subframe, 1u, 0u);
|
||||
assert(gps_almanac_store_page(&almanac, subframe, 5u));
|
||||
|
||||
/*
|
||||
* Sweep a full orbit. A real GPS satellite must spend a substantial part
|
||||
* of the day both above and below the horizon; a propagation sign error
|
||||
* or a bad inclination typically pins it to one side.
|
||||
*/
|
||||
unsigned int above = 0u;
|
||||
unsigned int below = 0u;
|
||||
for(unsigned int step = 0u; step < 720u; step++)
|
||||
{
|
||||
double tow = 100000.0 + (step * 60.0);
|
||||
double elevation = 0.0;
|
||||
assert(gps_almanac_look_angles(&almanac.sv[0], REF_WEEK, REF_WEEK,
|
||||
tow, rx, &elevation, NULL));
|
||||
assert(elevation >= -90.0 && elevation <= 90.0);
|
||||
if(elevation > 0.0)
|
||||
{
|
||||
above++;
|
||||
}
|
||||
else
|
||||
{
|
||||
below++;
|
||||
}
|
||||
}
|
||||
assert(above > 60u);
|
||||
assert(below > 60u);
|
||||
printf(" rise/set over 12 h: %u above, %u below\n", above, below);
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_field_decoding();
|
||||
test_propagation_against_reference();
|
||||
test_week_rollover_is_symmetric();
|
||||
test_page_rejection();
|
||||
test_visibility_fallbacks();
|
||||
test_visibility_selects_and_never_hides_unknowns();
|
||||
test_satellite_sets_below_and_rises();
|
||||
printf("gps almanac tests passed\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -12,9 +12,16 @@ static gps_ephemeris_t make_ephemeris(
|
||||
memset(&ephemeris, 0, sizeof(ephemeris));
|
||||
ephemeris.SVID = (unsigned char)prn;
|
||||
ephemeris.iode = iode;
|
||||
ephemeris.iodc = iode;
|
||||
ephemeris.ephemeris_valid = true;
|
||||
ephemeris.sva = 2u;
|
||||
ephemeris.sqrta = 5153.6;
|
||||
ephemeris.A = ephemeris.sqrta * ephemeris.sqrta;
|
||||
ephemeris.ecc = 0.008;
|
||||
ephemeris.toe.week = 2430;
|
||||
ephemeris.toe_week_valid = true;
|
||||
ephemeris.toe.sec = 123456.0;
|
||||
ephemeris.toc.sec = 123456.0;
|
||||
return ephemeris;
|
||||
}
|
||||
|
||||
@@ -72,10 +79,86 @@ static void test_validation_independence_and_invalidate(void)
|
||||
&cache, 19u, 40u, &output));
|
||||
}
|
||||
|
||||
static void test_store_if_new_reports_real_changes_only(void)
|
||||
{
|
||||
gps_ephemeris_cache_t cache;
|
||||
gps_ephemeris_cache_init(&cache, 1000u);
|
||||
gps_ephemeris_t eph = make_ephemeris(9u, 30u);
|
||||
bool changed = true;
|
||||
|
||||
assert(gps_ephemeris_cache_store_if_new(&cache, 9u, &eph, 10u,
|
||||
&changed));
|
||||
assert(changed);
|
||||
|
||||
/* Re-decoding the same data set refreshes the age datum but reports no
|
||||
* change, so no flash write is triggered. */
|
||||
assert(gps_ephemeris_cache_store_if_new(&cache, 9u, &eph, 20u,
|
||||
&changed));
|
||||
assert(!changed);
|
||||
|
||||
gps_ephemeris_t output;
|
||||
assert(gps_ephemeris_cache_restore(&cache, 9u, 1019u, &output));
|
||||
assert(output.iode == 30u);
|
||||
|
||||
/* A newer upload is a real change. */
|
||||
gps_ephemeris_t newer = make_ephemeris(9u, 31u);
|
||||
newer.toe.sec = eph.toe.sec + 7200.0;
|
||||
assert(gps_ephemeris_cache_store_if_new(&cache, 9u, &newer, 30u,
|
||||
&changed));
|
||||
assert(changed);
|
||||
assert(gps_ephemeris_cache_restore(&cache, 9u, 30u, &output));
|
||||
assert(output.iode == 31u);
|
||||
|
||||
/* An out-of-order older upload is rejected outright. */
|
||||
assert(!gps_ephemeris_cache_store_if_new(&cache, 9u, &eph, 40u,
|
||||
&changed));
|
||||
assert(!changed);
|
||||
assert(gps_ephemeris_cache_restore(&cache, 9u, 40u, &output));
|
||||
assert(output.iode == 31u);
|
||||
|
||||
/* A structurally invalid record never enters the cache. */
|
||||
gps_ephemeris_t corrupt = make_ephemeris(10u, 5u);
|
||||
corrupt.iodc = 6u;
|
||||
assert(!gps_ephemeris_cache_store_if_new(&cache, 10u, &corrupt, 50u,
|
||||
&changed));
|
||||
assert(!changed);
|
||||
}
|
||||
|
||||
static void test_restore_at_applies_fit_window(void)
|
||||
{
|
||||
gps_ephemeris_cache_t cache;
|
||||
gps_ephemeris_cache_init(&cache, 1000000u);
|
||||
gps_ephemeris_t eph = make_ephemeris(12u, 44u);
|
||||
gps_ephemeris_t output;
|
||||
bool changed = false;
|
||||
|
||||
assert(gps_ephemeris_cache_store_if_new(&cache, 12u, &eph, 0u,
|
||||
&changed));
|
||||
|
||||
/* Inside the four hour curve fit. */
|
||||
assert(gps_ephemeris_cache_restore_at(&cache, 12u, 10u, true, 2430,
|
||||
eph.toe.sec + 3600.0, &output));
|
||||
assert(output.iode == 44u);
|
||||
|
||||
/* Without receiver time the record stays available as a candidate. */
|
||||
assert(gps_ephemeris_cache_restore_at(&cache, 12u, 10u, false, 0, 0.0,
|
||||
&output));
|
||||
|
||||
/* Past the fit interval the entry is rejected and dropped, so a later
|
||||
* in-window query cannot resurrect it. */
|
||||
assert(!gps_ephemeris_cache_restore_at(&cache, 12u, 10u, true, 2430,
|
||||
eph.toe.sec + 9000.0, &output));
|
||||
assert(!gps_ephemeris_cache_restore_at(&cache, 12u, 10u, true, 2430,
|
||||
eph.toe.sec, &output));
|
||||
assert(!gps_ephemeris_cache_restore(&cache, 12u, 10u, &output));
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_store_restore_and_expiry();
|
||||
test_validation_independence_and_invalidate();
|
||||
test_store_if_new_reports_real_changes_only();
|
||||
test_restore_at_applies_fit_window();
|
||||
puts("gps ephemeris cache tests passed");
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,663 @@
|
||||
/*
|
||||
* Persistent ephemeris store against a simulated SPI NOR device.
|
||||
*
|
||||
* The simulator enforces the properties that make flash different from RAM:
|
||||
* programming can only clear bits, a program to a non-erased slot corrupts it,
|
||||
* and a write can be cut short at any byte to model power loss.
|
||||
*/
|
||||
|
||||
#include <assert.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "gps/gps_ephemeris_store.h"
|
||||
#include "gps/gps_ephemeris_validity.h"
|
||||
|
||||
#define SECTOR_SIZE 4096u
|
||||
#define SECTORS 2u
|
||||
#define FLASH_SIZE (SECTOR_SIZE * SECTORS)
|
||||
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
typedef struct {
|
||||
uint8_t data[FLASH_SIZE];
|
||||
uint32_t erase_count;
|
||||
uint32_t program_count;
|
||||
/* Power-loss injection: when armed, the Nth program truncates. */
|
||||
int32_t fail_after_programs;
|
||||
uint32_t truncate_bytes;
|
||||
bool powered;
|
||||
} sim_flash_t;
|
||||
|
||||
static void sim_init(sim_flash_t* f)
|
||||
{
|
||||
memset(f, 0, sizeof(*f));
|
||||
memset(f->data, 0xff, sizeof(f->data));
|
||||
f->fail_after_programs = -1;
|
||||
f->powered = true;
|
||||
}
|
||||
|
||||
static void sim_read(void* ctx, uint32_t address, void* dst, uint32_t len)
|
||||
{
|
||||
sim_flash_t* f = (sim_flash_t*)ctx;
|
||||
assert((address + len) <= FLASH_SIZE);
|
||||
memcpy(dst, &f->data[address], len);
|
||||
}
|
||||
|
||||
static bool sim_erase(void* ctx, uint32_t address)
|
||||
{
|
||||
sim_flash_t* f = (sim_flash_t*)ctx;
|
||||
if(!f->powered)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
assert((address % SECTOR_SIZE) == 0u);
|
||||
assert((address + SECTOR_SIZE) <= FLASH_SIZE);
|
||||
memset(&f->data[address], 0xff, SECTOR_SIZE);
|
||||
f->erase_count++;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool sim_program(void* ctx, uint32_t address, const void* src,
|
||||
uint32_t len)
|
||||
{
|
||||
sim_flash_t* f = (sim_flash_t*)ctx;
|
||||
if(!f->powered)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
const uint8_t* bytes = (const uint8_t*)src;
|
||||
assert((address + len) <= FLASH_SIZE);
|
||||
|
||||
uint32_t effective = len;
|
||||
bool cut = false;
|
||||
if(f->fail_after_programs == 0)
|
||||
{
|
||||
effective = (f->truncate_bytes < len) ? f->truncate_bytes : len;
|
||||
cut = true;
|
||||
}
|
||||
if(f->fail_after_programs >= 0)
|
||||
{
|
||||
f->fail_after_programs--;
|
||||
}
|
||||
|
||||
for(uint32_t i = 0u; i < effective; i++)
|
||||
{
|
||||
/* NOR programming can only clear bits. */
|
||||
f->data[address + i] &= bytes[i];
|
||||
}
|
||||
f->program_count++;
|
||||
if(cut)
|
||||
{
|
||||
f->powered = false;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static gps_ephemeris_store_flash_t sim_interface(sim_flash_t* f)
|
||||
{
|
||||
gps_ephemeris_store_flash_t iface;
|
||||
memset(&iface, 0, sizeof(iface));
|
||||
iface.read = sim_read;
|
||||
iface.erase_sector = sim_erase;
|
||||
iface.program = sim_program;
|
||||
iface.ctx = f;
|
||||
iface.base_address = 0u;
|
||||
iface.sector_size = SECTOR_SIZE;
|
||||
return iface;
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------ */
|
||||
/* LNAV subframe construction */
|
||||
/* ------------------------------------------------------------------ */
|
||||
|
||||
/*
|
||||
* Build the minimum subframe content the decoder needs: the subframe id in the
|
||||
* HOW, plus the fields the validity rules read back. The store re-parses these
|
||||
* with the real decoder, so anything malformed here shows up as a rejected
|
||||
* record rather than a silently wrong ephemeris.
|
||||
*/
|
||||
static void build_subframes(unsigned int words[3][10],
|
||||
unsigned int iodc,
|
||||
unsigned int toe_sec,
|
||||
unsigned int health)
|
||||
{
|
||||
memset(words, 0, sizeof(unsigned int) * 3u * 10u);
|
||||
unsigned int iode = iodc & 0xffu;
|
||||
|
||||
for(unsigned int sf = 0u; sf < 3u; sf++)
|
||||
{
|
||||
/* HOW: subframe id at bits 10..8, TOW count above bit 13. */
|
||||
words[sf][1] = ((100u + sf) << 13) | ((sf + 1u) << 8);
|
||||
}
|
||||
|
||||
/* Subframe 1: week, URA, health, IODC, Toc. */
|
||||
words[0][2] = (380u << 20) | (0u << 18) | (2u << 14) |
|
||||
((health & 0x3fu) << 8) | (((iodc >> 8) & 0x03u) << 6);
|
||||
words[0][7] = ((iodc & 0xffu) << 22) | ((toe_sec >> 4) << 6);
|
||||
|
||||
/* Subframe 2: IODE, sqrtA, eccentricity, Toe, fit flag clear. */
|
||||
words[1][2] = (iode << 22);
|
||||
/* ecc = 0.008 -> 0.008 / 2^-33 */
|
||||
unsigned int ecc_raw = (unsigned int)(0.008 * 8589934592.0);
|
||||
words[1][5] = ((ecc_raw >> 24) & 0xffu) << 6;
|
||||
words[1][6] = (ecc_raw & 0xffffffu) << 6;
|
||||
/* sqrtA = 5153.6 -> / 2^-19 */
|
||||
unsigned int sqrta_raw = (unsigned int)(5153.6 * 524288.0);
|
||||
words[1][7] = ((sqrta_raw >> 24) & 0xffu) << 6;
|
||||
words[1][8] = (sqrta_raw & 0xffffffu) << 6;
|
||||
words[1][9] = ((toe_sec >> 4) << 14);
|
||||
|
||||
/* Subframe 3: IODE must match subframe 2. */
|
||||
words[2][9] = (iode << 22);
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
unsigned int count;
|
||||
unsigned int prn[GPS_PRN_COUNT];
|
||||
unsigned char iode[GPS_PRN_COUNT];
|
||||
double toe[GPS_PRN_COUNT];
|
||||
} load_result_t;
|
||||
|
||||
static void load_handler(void* ctx, unsigned int prn,
|
||||
const gps_ephemeris_t* ephemeris)
|
||||
{
|
||||
load_result_t* result = (load_result_t*)ctx;
|
||||
result->prn[result->count] = prn;
|
||||
result->iode[result->count] = ephemeris->iode;
|
||||
result->toe[result->count] = ephemeris->toe.sec;
|
||||
result->count++;
|
||||
}
|
||||
|
||||
static bool save_one(gps_ephemeris_store_t* store, unsigned int prn,
|
||||
unsigned int iodc, unsigned int toe_sec,
|
||||
bool* wrote_out)
|
||||
{
|
||||
unsigned int words[3][10];
|
||||
build_subframes(words, iodc, toe_sec, 0u);
|
||||
|
||||
/* Parse with the live decoder so the stored record and the in-RAM
|
||||
* ephemeris always agree by construction. */
|
||||
gps_frame_t frame;
|
||||
memset(&frame, 0, sizeof(frame));
|
||||
frame.gps_ephems.SVID = (unsigned char)prn;
|
||||
for(unsigned int sf = 0u; sf < 3u; sf++)
|
||||
{
|
||||
unsigned int how = 0u;
|
||||
(void)parse_subframe(words[sf], &frame, &how);
|
||||
}
|
||||
frame.gps_ephems.SVID = (unsigned char)prn;
|
||||
if(!frame.gps_ephems.ephemeris_valid)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return gps_ephemeris_store_save(store, prn, &frame.gps_ephems,
|
||||
(const unsigned int (*)[10])words,
|
||||
wrote_out);
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------ */
|
||||
|
||||
static void empty_flash_mounts_and_first_save_formats(void)
|
||||
{
|
||||
sim_flash_t flash;
|
||||
sim_init(&flash);
|
||||
gps_ephemeris_store_t store;
|
||||
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
|
||||
|
||||
CHECK(gps_ephemeris_store_mount(&store, &iface),
|
||||
"a blank device failed to mount");
|
||||
load_result_t result;
|
||||
memset(&result, 0, sizeof(result));
|
||||
CHECK(gps_ephemeris_store_load_all(&store, load_handler, &result) == 0u,
|
||||
"a blank device returned records");
|
||||
|
||||
bool wrote = false;
|
||||
CHECK(save_one(&store, 7u, 40u, 172800u, &wrote),
|
||||
"the first save failed");
|
||||
CHECK(wrote, "the first save did not write");
|
||||
CHECK(flash.erase_count == 2u,
|
||||
"formatting did not erase both sectors exactly once");
|
||||
|
||||
memset(&result, 0, sizeof(result));
|
||||
CHECK(gps_ephemeris_store_load_all(&store, load_handler, &result) == 1u,
|
||||
"the saved record was not returned");
|
||||
CHECK((result.prn[0] == 7u) && (result.iode[0] == 40u),
|
||||
"the restored record has the wrong identity");
|
||||
CHECK(result.toe[0] == 172800.0, "the restored Toe is wrong");
|
||||
}
|
||||
|
||||
static void records_survive_remount(void)
|
||||
{
|
||||
sim_flash_t flash;
|
||||
sim_init(&flash);
|
||||
gps_ephemeris_store_t store;
|
||||
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
|
||||
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
|
||||
|
||||
for(unsigned int prn = 1u; prn <= 8u; prn++)
|
||||
{
|
||||
bool wrote = false;
|
||||
CHECK(save_one(&store, prn, 40u + prn, 172800u, &wrote),
|
||||
"save failed while filling the store");
|
||||
}
|
||||
|
||||
/* Cold boot. */
|
||||
gps_ephemeris_store_t remounted;
|
||||
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
|
||||
"remount after writes failed");
|
||||
load_result_t result;
|
||||
memset(&result, 0, sizeof(result));
|
||||
unsigned int loaded =
|
||||
gps_ephemeris_store_load_all(&remounted, load_handler, &result);
|
||||
CHECK(loaded == 8u, "not every persisted PRN came back after remount");
|
||||
for(unsigned int i = 0u; i < result.count; i++)
|
||||
{
|
||||
CHECK(result.iode[i] == (unsigned char)(40u + result.prn[i]),
|
||||
"a restored record was attributed to the wrong PRN");
|
||||
}
|
||||
}
|
||||
|
||||
static void newer_upload_supersedes_and_old_one_is_ignored(void)
|
||||
{
|
||||
sim_flash_t flash;
|
||||
sim_init(&flash);
|
||||
gps_ephemeris_store_t store;
|
||||
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
|
||||
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
|
||||
|
||||
bool wrote = false;
|
||||
CHECK(save_one(&store, 3u, 40u, 172800u, &wrote), "first save failed");
|
||||
CHECK(save_one(&store, 3u, 41u, 180000u, &wrote), "second save failed");
|
||||
|
||||
gps_ephemeris_store_t remounted;
|
||||
CHECK(gps_ephemeris_store_mount(&remounted, &iface), "remount failed");
|
||||
load_result_t result;
|
||||
memset(&result, 0, sizeof(result));
|
||||
CHECK(gps_ephemeris_store_load_all(&remounted, load_handler, &result) == 1u,
|
||||
"a superseded PRN returned more than one record");
|
||||
CHECK(result.iode[0] == 41u,
|
||||
"the older upload won over the newer one after remount");
|
||||
CHECK(result.toe[0] == 180000.0, "the restored Toe is not the newest");
|
||||
}
|
||||
|
||||
static void compaction_reclaims_a_full_sector(void)
|
||||
{
|
||||
sim_flash_t flash;
|
||||
sim_init(&flash);
|
||||
gps_ephemeris_store_t store;
|
||||
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
|
||||
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
|
||||
|
||||
const uint32_t slots = SECTOR_SIZE / GPS_EPH_STORE_SLOT_SIZE;
|
||||
/* Rewrite one PRN until the sector must roll over, and keep a second PRN
|
||||
* that must survive the compaction. */
|
||||
bool wrote = false;
|
||||
CHECK(save_one(&store, 9u, 60u, 100800u, &wrote), "seed save failed");
|
||||
uint32_t toe = 172800u;
|
||||
for(uint32_t i = 0u; i < (slots * 2u); i++)
|
||||
{
|
||||
CHECK(save_one(&store, 4u, 40u + (i % 200u), toe, &wrote),
|
||||
"save failed during compaction stress");
|
||||
toe += 16u;
|
||||
}
|
||||
CHECK(store.compactions > 0u, "the store never compacted");
|
||||
CHECK(flash.erase_count <= (2u + (store.compactions * 2u)),
|
||||
"compaction erased more sectors than it should");
|
||||
|
||||
gps_ephemeris_store_t remounted;
|
||||
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
|
||||
"remount after compaction failed");
|
||||
load_result_t result;
|
||||
memset(&result, 0, sizeof(result));
|
||||
unsigned int loaded =
|
||||
gps_ephemeris_store_load_all(&remounted, load_handler, &result);
|
||||
CHECK(loaded == 2u, "compaction lost a live PRN");
|
||||
bool saw_9 = false;
|
||||
for(unsigned int i = 0u; i < result.count; i++)
|
||||
{
|
||||
if(result.prn[i] == 9u)
|
||||
{
|
||||
saw_9 = true;
|
||||
CHECK(result.iode[i] == 60u,
|
||||
"the carried-over PRN changed during compaction");
|
||||
}
|
||||
}
|
||||
CHECK(saw_9, "the untouched PRN did not survive compaction");
|
||||
}
|
||||
|
||||
static void a_torn_record_is_skipped_and_the_store_still_mounts(void)
|
||||
{
|
||||
sim_flash_t flash;
|
||||
sim_init(&flash);
|
||||
gps_ephemeris_store_t store;
|
||||
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
|
||||
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
|
||||
|
||||
bool wrote = false;
|
||||
CHECK(save_one(&store, 5u, 40u, 172800u, &wrote), "first save failed");
|
||||
CHECK(save_one(&store, 6u, 50u, 172800u, &wrote), "second save failed");
|
||||
|
||||
/* Cut the next record halfway through: magic lands, CRC does not. */
|
||||
flash.fail_after_programs = 0;
|
||||
flash.truncate_bytes = 32u;
|
||||
(void)save_one(&store, 5u, 41u, 180000u, &wrote);
|
||||
|
||||
flash.powered = true;
|
||||
flash.fail_after_programs = -1;
|
||||
gps_ephemeris_store_t remounted;
|
||||
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
|
||||
"the store did not mount after a torn write");
|
||||
CHECK(remounted.records_rejected > 0u,
|
||||
"the torn record was not detected");
|
||||
load_result_t result;
|
||||
memset(&result, 0, sizeof(result));
|
||||
unsigned int loaded =
|
||||
gps_ephemeris_store_load_all(&remounted, load_handler, &result);
|
||||
CHECK(loaded == 2u, "a torn write destroyed good records");
|
||||
for(unsigned int i = 0u; i < result.count; i++)
|
||||
{
|
||||
if(result.prn[i] == 5u)
|
||||
{
|
||||
CHECK(result.iode[i] == 40u,
|
||||
"the torn record was accepted over the good one");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void power_loss_during_compaction_leaves_a_mountable_store(void)
|
||||
{
|
||||
const uint32_t slots = SECTOR_SIZE / GPS_EPH_STORE_SLOT_SIZE;
|
||||
/* Cut power at each program of the compaction sequence in turn. */
|
||||
for(int32_t cut = 0; cut < 6; cut++)
|
||||
{
|
||||
sim_flash_t flash;
|
||||
sim_init(&flash);
|
||||
gps_ephemeris_store_t store;
|
||||
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
|
||||
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
|
||||
|
||||
bool wrote = false;
|
||||
CHECK(save_one(&store, 2u, 30u, 100800u, &wrote), "seed save failed");
|
||||
for(uint32_t i = 0u; i + 2u < slots; i++)
|
||||
{
|
||||
CHECK(save_one(&store, 8u, 40u + (i % 200u), 172800u + (i * 16u),
|
||||
&wrote),
|
||||
"fill save failed");
|
||||
}
|
||||
|
||||
/* The next save triggers compaction; cut power inside it. */
|
||||
flash.fail_after_programs = cut;
|
||||
flash.truncate_bytes = 16u;
|
||||
(void)save_one(&store, 8u, 99u, 200000u, &wrote);
|
||||
|
||||
/* Power restored: the fault must not still be armed, or the recovery
|
||||
* write is measuring the injector rather than the store. */
|
||||
flash.powered = true;
|
||||
flash.fail_after_programs = -1;
|
||||
gps_ephemeris_store_t remounted;
|
||||
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
|
||||
"the store did not mount after power loss in compaction");
|
||||
load_result_t result;
|
||||
memset(&result, 0, sizeof(result));
|
||||
(void)gps_ephemeris_store_load_all(&remounted, load_handler, &result);
|
||||
/* The store may legitimately have lost the in-flight record, but it
|
||||
* must never return a corrupt one, and it must stay writable. */
|
||||
for(unsigned int i = 0u; i < result.count; i++)
|
||||
{
|
||||
CHECK(result.prn[i] < GPS_PRN_COUNT,
|
||||
"a corrupt PRN survived power loss");
|
||||
}
|
||||
bool wrote_after = false;
|
||||
CHECK(save_one(&remounted, 11u, 70u, 172800u, &wrote_after),
|
||||
"the store was not writable after power loss");
|
||||
}
|
||||
}
|
||||
|
||||
static void a_corrupt_header_falls_back_to_the_other_sector(void)
|
||||
{
|
||||
sim_flash_t flash;
|
||||
sim_init(&flash);
|
||||
gps_ephemeris_store_t store;
|
||||
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
|
||||
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
|
||||
bool wrote = false;
|
||||
CHECK(save_one(&store, 12u, 44u, 172800u, &wrote), "save failed");
|
||||
|
||||
/* Corrupt the active sector header only. */
|
||||
flash.data[4] ^= 0xffu;
|
||||
gps_ephemeris_store_t remounted;
|
||||
CHECK(gps_ephemeris_store_mount(&remounted, &iface),
|
||||
"mount failed with a corrupt header");
|
||||
load_result_t result;
|
||||
memset(&result, 0, sizeof(result));
|
||||
(void)gps_ephemeris_store_load_all(&remounted, load_handler, &result);
|
||||
/* Sector 1 was erased at format and has no valid header either, so the
|
||||
* store must come up empty rather than trusting garbage. */
|
||||
CHECK(result.count == 0u,
|
||||
"records were returned from a sector with no valid header");
|
||||
CHECK(save_one(&remounted, 12u, 44u, 172800u, &wrote),
|
||||
"the store did not recover after header corruption");
|
||||
}
|
||||
|
||||
static void rewriting_the_same_ephemeris_is_the_callers_decision(void)
|
||||
{
|
||||
/*
|
||||
* The store itself always appends what it is given; the wear policy lives
|
||||
* in gps_ephemeris_supersedes, which the caller applies first. Verify the
|
||||
* two agree so no repeat write can reach flash.
|
||||
*/
|
||||
sim_flash_t flash;
|
||||
sim_init(&flash);
|
||||
gps_ephemeris_store_t store;
|
||||
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
|
||||
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
|
||||
|
||||
unsigned int words[3][10];
|
||||
build_subframes(words, 40u, 172800u, 0u);
|
||||
gps_frame_t frame;
|
||||
memset(&frame, 0, sizeof(frame));
|
||||
frame.gps_ephems.SVID = 15u;
|
||||
for(unsigned int sf = 0u; sf < 3u; sf++)
|
||||
{
|
||||
unsigned int how = 0u;
|
||||
(void)parse_subframe(words[sf], &frame, &how);
|
||||
}
|
||||
frame.gps_ephems.SVID = 15u;
|
||||
|
||||
CHECK(!gps_ephemeris_supersedes(&frame.gps_ephems, &frame.gps_ephems),
|
||||
"an identical re-decode was reported as new information");
|
||||
|
||||
bool wrote = false;
|
||||
CHECK(gps_ephemeris_store_save(&store, 15u, &frame.gps_ephems,
|
||||
(const unsigned int (*)[10])words, &wrote),
|
||||
"save failed");
|
||||
uint32_t writes_after_first = store.writes;
|
||||
/* The caller would not call save again, but confirm the record is stable
|
||||
* if it did: same content, one extra slot, same restored result. */
|
||||
CHECK(gps_ephemeris_store_save(&store, 15u, &frame.gps_ephems,
|
||||
(const unsigned int (*)[10])words, &wrote),
|
||||
"second save failed");
|
||||
CHECK(store.writes == (writes_after_first + 1u),
|
||||
"an unexpected number of flash writes occurred");
|
||||
|
||||
gps_ephemeris_store_t remounted;
|
||||
CHECK(gps_ephemeris_store_mount(&remounted, &iface), "remount failed");
|
||||
load_result_t result;
|
||||
memset(&result, 0, sizeof(result));
|
||||
CHECK(gps_ephemeris_store_load_all(&remounted, load_handler, &result) == 1u,
|
||||
"duplicate saves produced duplicate restores");
|
||||
}
|
||||
|
||||
static void an_unhealthy_or_inconsistent_record_is_refused(void)
|
||||
{
|
||||
sim_flash_t flash;
|
||||
sim_init(&flash);
|
||||
gps_ephemeris_store_t store;
|
||||
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
|
||||
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
|
||||
|
||||
unsigned int words[3][10];
|
||||
build_subframes(words, 40u, 172800u, 0u);
|
||||
gps_frame_t frame;
|
||||
memset(&frame, 0, sizeof(frame));
|
||||
frame.gps_ephems.SVID = 20u;
|
||||
for(unsigned int sf = 0u; sf < 3u; sf++)
|
||||
{
|
||||
unsigned int how = 0u;
|
||||
(void)parse_subframe(words[sf], &frame, &how);
|
||||
}
|
||||
frame.gps_ephems.SVID = 20u;
|
||||
|
||||
gps_ephemeris_t unhealthy = frame.gps_ephems;
|
||||
unhealthy.svhlth = 0x3fu;
|
||||
bool wrote = true;
|
||||
CHECK(!gps_ephemeris_store_save(&store, 20u, &unhealthy,
|
||||
(const unsigned int (*)[10])words, &wrote),
|
||||
"an unhealthy satellite was persisted");
|
||||
CHECK(!wrote, "a refused save reported a write");
|
||||
|
||||
CHECK(!gps_ephemeris_store_save(&store, 21u, &frame.gps_ephems,
|
||||
(const unsigned int (*)[10])words, &wrote),
|
||||
"a record was persisted under the wrong PRN");
|
||||
}
|
||||
|
||||
static void crc_detects_single_bit_corruption(void)
|
||||
{
|
||||
uint8_t buffer[64];
|
||||
for(unsigned int i = 0u; i < sizeof(buffer); i++)
|
||||
{
|
||||
buffer[i] = (uint8_t)i;
|
||||
}
|
||||
uint32_t base = gps_ephemeris_store_crc32(buffer, sizeof(buffer));
|
||||
for(unsigned int i = 0u; i < sizeof(buffer); i++)
|
||||
{
|
||||
for(unsigned int bit = 0u; bit < 8u; bit++)
|
||||
{
|
||||
buffer[i] ^= (uint8_t)(1u << bit);
|
||||
CHECK(gps_ephemeris_store_crc32(buffer, sizeof(buffer)) != base,
|
||||
"CRC missed a single-bit flip");
|
||||
buffer[i] ^= (uint8_t)(1u << bit);
|
||||
}
|
||||
}
|
||||
/* Known-answer check against the standard CRC-32 of "123456789". */
|
||||
CHECK(gps_ephemeris_store_crc32("123456789", 9u) == 0xcbf43926u,
|
||||
"CRC-32 does not match the reference value");
|
||||
}
|
||||
|
||||
static void slot_cap_forces_compaction_on_a_real_sector(void)
|
||||
{
|
||||
/* The hardware test hook: cap slots so compaction is reachable without
|
||||
* writing a full sector, while still exercising real erases. */
|
||||
sim_flash_t flash;
|
||||
sim_init(&flash);
|
||||
gps_ephemeris_store_t store;
|
||||
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
|
||||
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
|
||||
gps_ephemeris_store_limit_slots(&store, 4u);
|
||||
CHECK(store.slots_per_sector == 4u, "slot cap was not applied");
|
||||
|
||||
bool wrote = false;
|
||||
CHECK(save_one(&store, 5u, 30u, 100800u, &wrote), "seed save failed");
|
||||
uint32_t toe = 172800u;
|
||||
for(unsigned int i = 0u; i < 12u; i++)
|
||||
{
|
||||
CHECK(save_one(&store, 6u, 40u + i, toe, &wrote), "fill save failed");
|
||||
toe += 16u;
|
||||
}
|
||||
CHECK(store.compactions > 0u, "slot cap did not reach compaction");
|
||||
CHECK(flash.erase_count >= 3u, "compaction did not perform real erases");
|
||||
|
||||
gps_ephemeris_store_t remounted;
|
||||
CHECK(gps_ephemeris_store_mount(&remounted, &iface), "remount failed");
|
||||
gps_ephemeris_store_limit_slots(&remounted, 4u);
|
||||
load_result_t result;
|
||||
memset(&result, 0, sizeof(result));
|
||||
unsigned int loaded =
|
||||
gps_ephemeris_store_load_all(&remounted, load_handler, &result);
|
||||
CHECK(loaded == 2u, "capped compaction lost a live PRN");
|
||||
|
||||
/* The cap only shrinks, and refuses degenerate values. */
|
||||
gps_ephemeris_store_limit_slots(&remounted, 1u);
|
||||
CHECK(remounted.slots_per_sector == 4u, "cap accepted a degenerate value");
|
||||
gps_ephemeris_store_limit_slots(&remounted, 999u);
|
||||
CHECK(remounted.slots_per_sector == 4u, "cap grew the sector");
|
||||
}
|
||||
|
||||
static void compaction_that_cannot_fit_latches_instead_of_eroding_flash(void)
|
||||
{
|
||||
/*
|
||||
* Regression for a destructive failure mode found on hardware: compaction
|
||||
* erases the spare sector before discovering the live set does not fit.
|
||||
* Without a latch, every subsequent save retried and burned two more erase
|
||||
* cycles, indefinitely.
|
||||
*/
|
||||
sim_flash_t flash;
|
||||
sim_init(&flash);
|
||||
gps_ephemeris_store_t store;
|
||||
gps_ephemeris_store_flash_t iface = sim_interface(&flash);
|
||||
CHECK(gps_ephemeris_store_mount(&store, &iface), "mount failed");
|
||||
gps_ephemeris_store_limit_slots(&store, 4u); /* header + 3 records */
|
||||
|
||||
bool wrote = false;
|
||||
/* Four distinct PRNs cannot be carried into three record slots. */
|
||||
for(unsigned int prn = 1u; prn <= 4u; prn++)
|
||||
{
|
||||
(void)save_one(&store, prn, 40u + prn, 172800u, &wrote);
|
||||
}
|
||||
/* Force the sector full so the next save must compact. */
|
||||
for(unsigned int i = 0u; i < 4u; i++)
|
||||
{
|
||||
(void)save_one(&store, 4u, 90u + i, 180000u + (i * 16u), &wrote);
|
||||
}
|
||||
|
||||
CHECK(store.capacity_exceeded, "over-capacity compaction did not latch");
|
||||
uint32_t erases_after_latch = flash.erase_count;
|
||||
|
||||
/* Every further save must be refused without touching the device. */
|
||||
for(unsigned int i = 0u; i < 10u; i++)
|
||||
{
|
||||
CHECK(!save_one(&store, 2u, 50u + i, 190000u, &wrote),
|
||||
"a save succeeded after the capacity latch");
|
||||
CHECK(!wrote, "a latched store reported a write");
|
||||
}
|
||||
CHECK(flash.erase_count == erases_after_latch,
|
||||
"the latched store kept erasing flash");
|
||||
|
||||
/* A remount clears the latch, so recovery is possible once the config or
|
||||
* the live set changes. */
|
||||
gps_ephemeris_store_t remounted;
|
||||
CHECK(gps_ephemeris_store_mount(&remounted, &iface), "remount failed");
|
||||
CHECK(!remounted.capacity_exceeded, "the latch survived a remount");
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
crc_detects_single_bit_corruption();
|
||||
compaction_that_cannot_fit_latches_instead_of_eroding_flash();
|
||||
slot_cap_forces_compaction_on_a_real_sector();
|
||||
empty_flash_mounts_and_first_save_formats();
|
||||
records_survive_remount();
|
||||
newer_upload_supersedes_and_old_one_is_ignored();
|
||||
compaction_reclaims_a_full_sector();
|
||||
a_torn_record_is_skipped_and_the_store_still_mounts();
|
||||
power_loss_during_compaction_leaves_a_mountable_store();
|
||||
a_corrupt_header_falls_back_to_the_other_sector();
|
||||
rewriting_the_same_ephemeris_is_the_callers_decision();
|
||||
an_unhealthy_or_inconsistent_record_is_refused();
|
||||
if(failures == 0)
|
||||
{
|
||||
puts("gps ephemeris store tests passed");
|
||||
}
|
||||
return failures == 0 ? 0 : 1;
|
||||
}
|
||||
@@ -0,0 +1,253 @@
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "gps/gps_ephemeris_validity.h"
|
||||
|
||||
#define WEEK_SEC 604800.0
|
||||
|
||||
static gps_ephemeris_t make_ephemeris(unsigned int prn, unsigned int iodc)
|
||||
{
|
||||
gps_ephemeris_t eph;
|
||||
memset(&eph, 0, sizeof(eph));
|
||||
eph.SVID = (unsigned char)prn;
|
||||
eph.ephemeris_valid = true;
|
||||
eph.iodc = iodc;
|
||||
eph.iode = (unsigned char)(iodc & 0xffu);
|
||||
eph.sva = 2u;
|
||||
eph.svhlth = 0u;
|
||||
eph.update = 0u;
|
||||
eph.sqrta = 5153.6;
|
||||
eph.A = eph.sqrta * eph.sqrta;
|
||||
eph.ecc = 0.008;
|
||||
eph.toe.sec = 172800.0;
|
||||
eph.toc.sec = 172800.0;
|
||||
eph.WN = 430u;
|
||||
eph.tow.week = 2430;
|
||||
/* Resolved from WN and the handover word by gps_resolve_toe_week(). */
|
||||
eph.toe.week = 2430;
|
||||
eph.toe_week_valid = true;
|
||||
return eph;
|
||||
}
|
||||
|
||||
static void test_fit_interval_table(void)
|
||||
{
|
||||
/* Fit flag clear is always the nominal four hour curve fit. */
|
||||
assert(gps_ephemeris_fit_interval_hours(0u, false) == 4.0);
|
||||
assert(gps_ephemeris_fit_interval_hours(500u, false) == 4.0);
|
||||
|
||||
/* IS-GPS-200 Table 20-XII ranges. */
|
||||
assert(gps_ephemeris_fit_interval_hours(240u, true) == 8.0);
|
||||
assert(gps_ephemeris_fit_interval_hours(247u, true) == 8.0);
|
||||
assert(gps_ephemeris_fit_interval_hours(248u, true) == 14.0);
|
||||
assert(gps_ephemeris_fit_interval_hours(496u, true) == 14.0);
|
||||
assert(gps_ephemeris_fit_interval_hours(497u, true) == 26.0);
|
||||
assert(gps_ephemeris_fit_interval_hours(504u, true) == 50.0);
|
||||
assert(gps_ephemeris_fit_interval_hours(511u, true) == 74.0);
|
||||
|
||||
/* Untabulated IODC with the flag set falls back conservatively, below
|
||||
* the shortest long fit the ICD defines. */
|
||||
double fallback = gps_ephemeris_fit_interval_hours(100u, true);
|
||||
assert(fallback == GPS_EPHEMERIS_LONG_FIT_FALLBACK_HOURS);
|
||||
assert(fallback < 8.0);
|
||||
}
|
||||
|
||||
static void test_static_rules(void)
|
||||
{
|
||||
gps_ephemeris_t eph = make_ephemeris(7u, 40u);
|
||||
assert(gps_ephemeris_check_static(&eph, 7u) == GPS_EPHEMERIS_OK);
|
||||
|
||||
assert(gps_ephemeris_check_static(NULL, 7u) ==
|
||||
GPS_EPHEMERIS_REJECT_NULL);
|
||||
assert(gps_ephemeris_check_static(&eph, 8u) ==
|
||||
GPS_EPHEMERIS_REJECT_PRN);
|
||||
|
||||
gps_ephemeris_t uncommitted = eph;
|
||||
uncommitted.ephemeris_valid = false;
|
||||
assert(gps_ephemeris_check_static(&uncommitted, 7u) ==
|
||||
GPS_EPHEMERIS_REJECT_UNCOMMITTED);
|
||||
|
||||
/* IODE must equal the eight LSBs of IODC. */
|
||||
gps_ephemeris_t mismatched = eph;
|
||||
mismatched.iode = (unsigned char)(mismatched.iode + 1u);
|
||||
assert(gps_ephemeris_check_static(&mismatched, 7u) ==
|
||||
GPS_EPHEMERIS_REJECT_IOD);
|
||||
|
||||
/* A ten bit IODC whose low byte matches is accepted. */
|
||||
gps_ephemeris_t wide_iodc = make_ephemeris(7u, 0x123u);
|
||||
assert(wide_iodc.iode == 0x23u);
|
||||
assert(gps_ephemeris_check_static(&wide_iodc, 7u) == GPS_EPHEMERIS_OK);
|
||||
|
||||
gps_ephemeris_t unhealthy = eph;
|
||||
unhealthy.svhlth = 0x20u;
|
||||
assert(gps_ephemeris_check_static(&unhealthy, 7u) ==
|
||||
GPS_EPHEMERIS_REJECT_HEALTH);
|
||||
|
||||
gps_ephemeris_t no_accuracy = eph;
|
||||
no_accuracy.sva = 15u;
|
||||
assert(gps_ephemeris_check_static(&no_accuracy, 7u) ==
|
||||
GPS_EPHEMERIS_REJECT_ACCURACY);
|
||||
|
||||
gps_ephemeris_t bad_orbit = eph;
|
||||
bad_orbit.ecc = 1.5;
|
||||
assert(gps_ephemeris_check_static(&bad_orbit, 7u) ==
|
||||
GPS_EPHEMERIS_REJECT_PARAMETERS);
|
||||
|
||||
gps_ephemeris_t bad_toe = eph;
|
||||
bad_toe.toe.sec = WEEK_SEC;
|
||||
assert(gps_ephemeris_check_static(&bad_toe, 7u) ==
|
||||
GPS_EPHEMERIS_REJECT_PARAMETERS);
|
||||
}
|
||||
|
||||
static void test_fit_window(void)
|
||||
{
|
||||
gps_ephemeris_t eph = make_ephemeris(7u, 40u);
|
||||
const double toe = eph.toe.sec;
|
||||
|
||||
/* Nominal four hour fit: usable for two hours either side of Toe. */
|
||||
assert(gps_ephemeris_max_toe_offset_sec(&eph) == 7200.0);
|
||||
assert(gps_ephemeris_check(&eph, 7u, true, 2430, toe) ==
|
||||
GPS_EPHEMERIS_OK);
|
||||
assert(gps_ephemeris_check(&eph, 7u, true, 2430, toe + 7199.0) ==
|
||||
GPS_EPHEMERIS_OK);
|
||||
assert(gps_ephemeris_check(&eph, 7u, true, 2430, toe - 7199.0) ==
|
||||
GPS_EPHEMERIS_OK);
|
||||
assert(gps_ephemeris_check(&eph, 7u, true, 2430, toe + 7201.0) ==
|
||||
GPS_EPHEMERIS_REJECT_AGE);
|
||||
assert(gps_ephemeris_check(&eph, 7u, true, 2430, toe - 7201.0) ==
|
||||
GPS_EPHEMERIS_REJECT_AGE);
|
||||
|
||||
/* An extended fit interval widens the same window. */
|
||||
gps_ephemeris_t extended = make_ephemeris(7u, 250u);
|
||||
extended.fit_flag = true;
|
||||
assert(gps_ephemeris_max_toe_offset_sec(&extended) == 14.0 * 1800.0);
|
||||
assert(gps_ephemeris_check(&extended, 7u, true, 2430, toe + 20000.0) ==
|
||||
GPS_EPHEMERIS_OK);
|
||||
|
||||
/* Without receiver time only the static rules apply, so a boot-time
|
||||
* flash record stays a usable candidate. */
|
||||
assert(gps_ephemeris_check(&eph, 7u, false, 0, 0.0) ==
|
||||
GPS_EPHEMERIS_OK);
|
||||
}
|
||||
|
||||
static void test_week_rollover(void)
|
||||
{
|
||||
/* Toe 600 s before the end of the week, receiver time 600 s after the
|
||||
* rollover: the true age is 1200 s, not almost a full week. */
|
||||
gps_ephemeris_t eph = make_ephemeris(11u, 60u);
|
||||
eph.toe.sec = WEEK_SEC - 600.0;
|
||||
|
||||
double offset = 0.0;
|
||||
assert(gps_ephemeris_toe_offset_sec(&eph, 2431, 600.0, &offset));
|
||||
assert(fabs(offset - 1200.0) < 1e-6);
|
||||
assert(gps_ephemeris_check(&eph, 11u, true, 2431, 600.0) ==
|
||||
GPS_EPHEMERIS_OK);
|
||||
|
||||
/* The same record is stale eight hours into the new week. */
|
||||
assert(gps_ephemeris_check(&eph, 11u, true, 2431, 28800.0) ==
|
||||
GPS_EPHEMERIS_REJECT_AGE);
|
||||
|
||||
/* Toe just after a rollover with receiver time just before it gives a
|
||||
* small negative age rather than a week-long one. */
|
||||
gps_ephemeris_t fresh = make_ephemeris(11u, 60u);
|
||||
fresh.toe.sec = 600.0;
|
||||
/* Toe belongs to the week after the one it was broadcast in. */
|
||||
fresh.toe.week = 2431;
|
||||
assert(gps_ephemeris_toe_offset_sec(&fresh, 2430, WEEK_SEC - 600.0,
|
||||
&offset));
|
||||
assert(fabs(offset + 1200.0) < 1e-6);
|
||||
assert(gps_ephemeris_check(&fresh, 11u, true, 2430,
|
||||
WEEK_SEC - 600.0) == GPS_EPHEMERIS_OK);
|
||||
}
|
||||
|
||||
/*
|
||||
* Toe is a second-of-week, so before its week was resolved the age check could
|
||||
* only work modulo one week: a record exactly N weeks old, evaluated at a
|
||||
* similar time of week, read as brand new. While ephemeris lives in RAM and
|
||||
* expires in hours that is unreachable. Persisting it to flash makes it
|
||||
* directly reachable - switch the receiver off and on again a week later at a
|
||||
* similar hour and it would accept its own week-old orbits as current, which
|
||||
* is worth kilometres of position error with nothing to flag it.
|
||||
*/
|
||||
static void test_whole_week_staleness_is_not_invisible(void)
|
||||
{
|
||||
gps_ephemeris_t eph = make_ephemeris(11u, 60u);
|
||||
double offset = 0.0;
|
||||
|
||||
/* Same second of week, one week on. */
|
||||
assert(gps_ephemeris_toe_offset_sec(&eph, 2431, eph.toe.sec, &offset));
|
||||
assert(fabs(offset - WEEK_SEC) < 1e-6);
|
||||
assert(gps_ephemeris_check(&eph, 11u, true, 2431, eph.toe.sec) ==
|
||||
GPS_EPHEMERIS_REJECT_AGE);
|
||||
|
||||
/* And four weeks on, which is what a shelved receiver comes back to. */
|
||||
assert(gps_ephemeris_check(&eph, 11u, true, 2434, eph.toe.sec) ==
|
||||
GPS_EPHEMERIS_REJECT_AGE);
|
||||
|
||||
/* A record whose week could not be resolved has an unknown vintage and
|
||||
* must be refused rather than assumed current. */
|
||||
gps_ephemeris_t unresolved = make_ephemeris(11u, 60u);
|
||||
unresolved.toe_week_valid = false;
|
||||
assert(!gps_ephemeris_toe_offset_sec(&unresolved, 2430,
|
||||
unresolved.toe.sec + 60.0, &offset));
|
||||
assert(gps_ephemeris_check(&unresolved, 11u, true, 2430,
|
||||
unresolved.toe.sec + 60.0) ==
|
||||
GPS_EPHEMERIS_REJECT_AGE);
|
||||
/* Without receiver time it is still a usable cold-start candidate; the
|
||||
* epoch gate re-checks it the moment time exists. */
|
||||
assert(gps_ephemeris_check(&unresolved, 11u, false, 0, 0.0) ==
|
||||
GPS_EPHEMERIS_OK);
|
||||
printf(" whole-week staleness is visible to the age check\n");
|
||||
}
|
||||
|
||||
static void test_supersede(void)
|
||||
{
|
||||
gps_ephemeris_t current = make_ephemeris(19u, 40u);
|
||||
gps_ephemeris_t repeat = current;
|
||||
|
||||
/* The same data set decoded again is not new information. */
|
||||
assert(!gps_ephemeris_supersedes(&repeat, ¤t));
|
||||
|
||||
/* Health or transmitted accuracy can change inside one issue of data. */
|
||||
gps_ephemeris_t health_change = current;
|
||||
health_change.svhlth = 0x3fu;
|
||||
assert(gps_ephemeris_supersedes(&health_change, ¤t));
|
||||
|
||||
/* A newer upload with a later Toe replaces it. */
|
||||
gps_ephemeris_t newer = make_ephemeris(19u, 41u);
|
||||
newer.toe.sec = current.toe.sec + 7200.0;
|
||||
assert(gps_ephemeris_supersedes(&newer, ¤t));
|
||||
|
||||
/* An older Toe with a different IODE does not: that is a replayed or
|
||||
* out-of-order decode. */
|
||||
gps_ephemeris_t older = make_ephemeris(19u, 39u);
|
||||
older.toe.sec = current.toe.sec - 7200.0;
|
||||
assert(!gps_ephemeris_supersedes(&older, ¤t));
|
||||
|
||||
/* Toe advancing across the week boundary still counts as newer. */
|
||||
gps_ephemeris_t before_rollover = make_ephemeris(19u, 42u);
|
||||
before_rollover.toe.sec = WEEK_SEC - 1800.0;
|
||||
gps_ephemeris_t after_rollover = make_ephemeris(19u, 43u);
|
||||
after_rollover.toe.sec = 1800.0;
|
||||
assert(gps_ephemeris_supersedes(&after_rollover, &before_rollover));
|
||||
assert(!gps_ephemeris_supersedes(&before_rollover, &after_rollover));
|
||||
|
||||
/* Nothing supersedes across PRNs, and anything replaces an empty slot. */
|
||||
gps_ephemeris_t other_prn = make_ephemeris(20u, 41u);
|
||||
assert(!gps_ephemeris_supersedes(&other_prn, ¤t));
|
||||
assert(gps_ephemeris_supersedes(¤t, NULL));
|
||||
assert(!gps_ephemeris_supersedes(NULL, ¤t));
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_fit_interval_table();
|
||||
test_static_rules();
|
||||
test_fit_window();
|
||||
test_week_rollover();
|
||||
test_whole_week_staleness_is_not_invisible();
|
||||
test_supersede();
|
||||
puts("gps ephemeris validity tests passed");
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,168 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include "gps/gps_ip_defines.h"
|
||||
|
||||
static int failures;
|
||||
#define CHECK(c,m) do{ if(!(c)){ fprintf(stderr,"FAIL: %s\n",m); failures++; } }while(0)
|
||||
|
||||
int main(void)
|
||||
{
|
||||
unsigned int sf[10];
|
||||
unsigned char iode = 0u;
|
||||
|
||||
/* Subframe 1: IODC is split, two MSBs in word 3 bits 7..6, eight LSBs in
|
||||
* word 8 bits 29..22. The low byte must equal the IODE. */
|
||||
memset(sf, 0, sizeof(sf));
|
||||
sf[2] = (0x02u << 6); /* IODC[9:8] = 2 */
|
||||
sf[7] = (0x5Au << 22); /* IODC[7:0] = 0x5A */
|
||||
CHECK(gps_lnav_broadcast_iode(sf, 1u, &iode), "sf1 iode not extracted");
|
||||
CHECK(iode == 0x5Au, "sf1 iode wrong");
|
||||
|
||||
/* Subframe 2: IODE in word 3 bits 29..22. */
|
||||
memset(sf, 0, sizeof(sf));
|
||||
sf[2] = (0xA3u << 22);
|
||||
CHECK(gps_lnav_broadcast_iode(sf, 2u, &iode), "sf2 iode not extracted");
|
||||
CHECK(iode == 0xA3u, "sf2 iode wrong");
|
||||
|
||||
/* Subframe 3: IODE in word 10 bits 29..22. */
|
||||
memset(sf, 0, sizeof(sf));
|
||||
sf[9] = (0x17u << 22);
|
||||
CHECK(gps_lnav_broadcast_iode(sf, 3u, &iode), "sf3 iode not extracted");
|
||||
CHECK(iode == 0x17u, "sf3 iode wrong");
|
||||
|
||||
/* Subframes 4 and 5 carry no issue of data. */
|
||||
CHECK(!gps_lnav_broadcast_iode(sf, 4u, &iode), "sf4 reported an iode");
|
||||
CHECK(!gps_lnav_broadcast_iode(sf, 5u, &iode), "sf5 reported an iode");
|
||||
CHECK(!gps_lnav_broadcast_iode(NULL, 2u, &iode), "null subframe accepted");
|
||||
CHECK(!gps_lnav_broadcast_iode(sf, 2u, NULL), "null output accepted");
|
||||
|
||||
/* All three subframes of one data set must agree, which is what the
|
||||
* cross-check relies on. */
|
||||
unsigned int set1[10], set2[10], set3[10];
|
||||
memset(set1,0,sizeof(set1)); memset(set2,0,sizeof(set2)); memset(set3,0,sizeof(set3));
|
||||
set1[2] = (0x01u << 6); set1[7] = (0x77u << 22);
|
||||
set2[2] = (0x77u << 22);
|
||||
set3[9] = (0x77u << 22);
|
||||
unsigned char a=0,b=0,c=0;
|
||||
gps_lnav_broadcast_iode(set1,1u,&a);
|
||||
gps_lnav_broadcast_iode(set2,2u,&b);
|
||||
gps_lnav_broadcast_iode(set3,3u,&c);
|
||||
CHECK((a==b)&&(b==c)&&(a==0x77u), "one data set gave inconsistent iode");
|
||||
|
||||
/*
|
||||
* Restoring a persisted page must repopulate the raw-word cache, not just
|
||||
* the parsed model. The periodic save rebuilds its payload from that
|
||||
* cache, so a restore that only set the parsed form would write the page
|
||||
* back as absent and wipe the stored model on the first save after every
|
||||
* reboot. That regression reached hardware once and was caught only by
|
||||
* watching "iono=0" appear in the save telemetry.
|
||||
*/
|
||||
unsigned int page18[10];
|
||||
memset(page18, 0, sizeof(page18));
|
||||
/* data ID 1, SV ID 56 -> subframe 4 page 18, with non-zero coefficients. */
|
||||
page18[2] = (1u << 28) | (56u << 22) | (0x1au << 14) | (0x0bu << 6);
|
||||
page18[3] = (0x21u << 22) | (0x33u << 14) | (0x44u << 6);
|
||||
page18[4] = (0x55u << 22) | (0x66u << 14) | (0x77u << 6);
|
||||
page18[5] = (0x12u << 22) | (0x34u << 14) | (0x56u << 6);
|
||||
|
||||
uint32_t words[10];
|
||||
CHECK(!gps_lnav_get_iono_page(words),
|
||||
"iono page reported before anything was stored");
|
||||
CHECK(gps_lnav_restore_iono_page(page18), "restoring page 18 failed");
|
||||
CHECK(gps_lnav_get_iono_page(words),
|
||||
"restored iono page did not repopulate the raw-word cache");
|
||||
for(unsigned int i = 0u; i < 10u; i++)
|
||||
{
|
||||
CHECK(words[i] == (uint32_t)page18[i],
|
||||
"restored iono raw words differ from what was restored");
|
||||
}
|
||||
|
||||
/* Same contract for almanac pages. */
|
||||
unsigned int alm[10];
|
||||
memset(alm, 0, sizeof(alm));
|
||||
alm[2] = (1u << 28) | (5u << 22) | (0x0f3eu << 6); /* SV ID 5 */
|
||||
alm[3] = (36u << 22) | (2439u << 6);
|
||||
alm[4] = (0xfd50u << 14);
|
||||
alm[5] = (10554475u << 6);
|
||||
uint32_t pages[GPS_ALMANAC_PRN_COUNT][10];
|
||||
CHECK(gps_lnav_restore_almanac_page(alm, 5u),
|
||||
"restoring an almanac page failed");
|
||||
uint32_t mask = gps_lnav_get_almanac_pages(pages);
|
||||
CHECK((mask & (1u << 4)) != 0u,
|
||||
"restored almanac page not present in the raw-word cache");
|
||||
for(unsigned int i = 0u; i < 10u; i++)
|
||||
{
|
||||
CHECK(pages[4][i] == (uint32_t)alm[i],
|
||||
"restored almanac raw words differ from what was restored");
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Toe carries no week on the air. The decoder reconstructs it from the
|
||||
* subframe 1 week number and the handover word of the subframe that
|
||||
* carried Toe, which is the only self-consistent pair available: Toe is
|
||||
* within a couple of hours of its own transmission, so the week that puts
|
||||
* it closest to that transmission is the right one.
|
||||
*
|
||||
* Without this the curve-fit age check works modulo one week and a record
|
||||
* exactly N weeks old reads as current - harmless while ephemeris lives in
|
||||
* RAM, reachable the moment it is persisted across a power-off.
|
||||
*/
|
||||
{
|
||||
struct { const char* name; unsigned int wn; double how_tow;
|
||||
double toe_sec; int expect_week; } cases[] = {
|
||||
/* HOW TOW is a count of six-second units and Toe is a 16-bit
|
||||
* field scaled by 16, so both are quantised on the air. */
|
||||
{"mid-week", 430u, 172800.0, 172800.0, 430},
|
||||
{"toe two hours ahead", 430u, 172800.0, 180000.0, 430},
|
||||
/* Broadcast at the end of the week, Toe early in the next one. */
|
||||
{"toe crosses forward", 430u, 604194.0, 1808.0, 431},
|
||||
/* Broadcast just after the rollover, Toe late in the previous. */
|
||||
{"toe crosses back", 431u, 1806.0, 604192.0, 430},
|
||||
};
|
||||
for(unsigned int c = 0u; c < 4u; c++)
|
||||
{
|
||||
gps_frame_t frame;
|
||||
unsigned int s1[10], s2[10], s3[10];
|
||||
memset(&frame, 0, sizeof(frame));
|
||||
memset(s1, 0, sizeof(s1));
|
||||
memset(s2, 0, sizeof(s2));
|
||||
memset(s3, 0, sizeof(s3));
|
||||
|
||||
/* HOW: TOW count in six-second units, then the subframe id. */
|
||||
unsigned int how = (unsigned int)(cases[c].how_tow / 6.0);
|
||||
s1[1] = (how << 13) | (1u << 8);
|
||||
s2[1] = (how << 13) | (2u << 8);
|
||||
s3[1] = (how << 13) | (3u << 8);
|
||||
|
||||
/* Subframe 1: week number and the IODC whose low byte is the IODE. */
|
||||
s1[2] = (cases[c].wn << 20);
|
||||
s1[7] = (0x64u << 22);
|
||||
/* Subframes 2 and 3 carry the matching IODE, and 2 carries Toe. */
|
||||
s2[2] = (0x64u << 22);
|
||||
s2[9] = (((unsigned int)(cases[c].toe_sec / 16.0)) << 14);
|
||||
s3[9] = (0x64u << 22);
|
||||
|
||||
parse_subframe(s1, &frame, NULL);
|
||||
parse_subframe(s2, &frame, NULL);
|
||||
parse_subframe(s3, &frame, NULL);
|
||||
|
||||
CHECK(frame.gps_ephems.ephemeris_valid,
|
||||
"consistent ephemeris set was not committed");
|
||||
CHECK(frame.gps_ephems.toe_week_valid,
|
||||
"toe week was not resolved");
|
||||
if(frame.gps_ephems.toe.week != cases[c].expect_week)
|
||||
{
|
||||
fprintf(stderr, "FAIL: %s: toe week %d, expected %d\n",
|
||||
cases[c].name, frame.gps_ephems.toe.week,
|
||||
cases[c].expect_week);
|
||||
failures++;
|
||||
}
|
||||
CHECK(frame.gps_ephems.toe.sec == cases[c].toe_sec,
|
||||
"toe seconds did not survive the round trip");
|
||||
}
|
||||
}
|
||||
|
||||
if(failures==0) puts("gps lnav iode tests passed");
|
||||
return failures==0?0:1;
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -0,0 +1,754 @@
|
||||
/*
|
||||
* Audit regressions for the persistent navigation context.
|
||||
*
|
||||
* Each test here reproduces a defect that was found by reading the code and
|
||||
* confirmed by running it, and each one fails against the code as it stood
|
||||
* before the accompanying fix. They are grouped in one binary because they
|
||||
* share one story: persistence changed the lifetime of the almanac, the
|
||||
* ionosphere page and the last position from "one power cycle" to "weeks", and
|
||||
* three separate pieces of arithmetic were only ever correct over the shorter
|
||||
* lifetime.
|
||||
*
|
||||
* The flash side runs against the same simulated SPI NOR as
|
||||
* test_gps_nav_context_store.c: programming can only clear bits and a program
|
||||
* into a non-erased page corrupts it.
|
||||
*/
|
||||
|
||||
#define _POSIX_C_SOURCE 200809L
|
||||
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/wait.h>
|
||||
|
||||
#include "gps/gps_ip_defines.h"
|
||||
#include "gps/gps_almanac.h"
|
||||
#include "gps/gps_ephemeris_cache.h"
|
||||
#include "gps/gps_ephemeris_validity.h"
|
||||
#include "gps/gps_nav_context_store.h"
|
||||
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
/* ------------------------------------------------------------------ */
|
||||
/* LNAV word construction, shared with test_gps_almanac.c conventions. */
|
||||
/* ------------------------------------------------------------------ */
|
||||
|
||||
static void put_field(unsigned int* word, unsigned int first_bit,
|
||||
unsigned int length, uint32_t value)
|
||||
{
|
||||
unsigned int shift = 30u - (first_bit + length - 1u);
|
||||
uint32_t mask = (length >= 32u) ? 0xffffffffu : ((1u << length) - 1u);
|
||||
*word &= ~(mask << shift);
|
||||
*word |= (value & mask) << shift;
|
||||
}
|
||||
|
||||
/* Subframe 5 page 25: the almanac reference week (WNa) and toa. */
|
||||
static void build_page25(unsigned int subframe[10], unsigned int wna,
|
||||
unsigned int toa_raw)
|
||||
{
|
||||
memset(subframe, 0, sizeof(unsigned int) * 10u);
|
||||
put_field(&subframe[1], 20u, 3u, 5u); /* HOW subframe id */
|
||||
put_field(&subframe[2], 1u, 2u, 1u); /* data ID 1 = LNAV */
|
||||
put_field(&subframe[2], 3u, 6u, 51u); /* SV ID 51 = page 25 */
|
||||
put_field(&subframe[2], 9u, 8u, toa_raw);
|
||||
put_field(&subframe[2], 17u, 8u, wna);
|
||||
}
|
||||
|
||||
/*
|
||||
* The almanac reference week is broadcast as WNa, the eight least significant
|
||||
* bits of the week, and has to be lifted against a week the receiver already
|
||||
* knows. The lift ran against gps_ephems.toe.week, and nothing in the
|
||||
* receiver ever writes that field - LNAV broadcasts Toe as a second-of-week
|
||||
* only. Lifting against the zero it holds put the result 256 weeks below the
|
||||
* receiver for every WNa above 128, which in unsigned arithmetic underflowed
|
||||
* to about 4.29e9.
|
||||
*
|
||||
* GPS week 2431 (the week this was found in) has WNa 127, so the fault was
|
||||
* two weeks in the future rather than present: it arms at week 2433 and then
|
||||
* for 127 weeks out of every 256.
|
||||
*/
|
||||
static void test_almanac_reference_week_is_lifted_sanely(void)
|
||||
{
|
||||
const int frame_week = 383; /* the 10-bit broadcast scale: 2431 & 1023 */
|
||||
|
||||
/*
|
||||
* The arithmetic as it stood, reproduced here so the defect is stated and
|
||||
* not just described. Run against the real parser before the fix this
|
||||
* produced 4294967169 for WNa 129, and gps_almanac_satellite_ecef() then
|
||||
* spent 4.2 s of a 5 GHz x86 walking it back one week at a time.
|
||||
*/
|
||||
{
|
||||
unsigned int broken_frame_week = 0u; /* toe.week: never written */
|
||||
unsigned int broken =
|
||||
(broken_frame_week & ~0xffu) | (129u & 0xffu);
|
||||
if((broken + 128u) < broken_frame_week) { broken += 256u; }
|
||||
else if(broken > (broken_frame_week + 128u)) { broken -= 256u; }
|
||||
printf(" the previous lift gave week %u for WNa 129 "
|
||||
"(receiver week %d)\n", broken, frame_week);
|
||||
CHECK(broken > 1000000u,
|
||||
"the defect being regressed against no longer reproduces");
|
||||
}
|
||||
|
||||
for(unsigned int wna = 0u; wna <= 255u; wna++)
|
||||
{
|
||||
gps_frame_t frame;
|
||||
unsigned int subframe[10];
|
||||
memset(&frame, 0, sizeof(frame));
|
||||
frame.gps_ephems.tow.week = frame_week;
|
||||
build_page25(subframe, wna, 36u);
|
||||
|
||||
gps_lnav_reset_almanac();
|
||||
parse_subframe(subframe, &frame, NULL);
|
||||
|
||||
const gps_almanac_t* almanac = gps_lnav_get_almanac();
|
||||
if(!almanac->reference_valid)
|
||||
{
|
||||
continue; /* refusing to adopt a week is always allowed */
|
||||
}
|
||||
long delta = (long)almanac->week - (long)frame_week;
|
||||
if((delta > 128) || (delta < -128))
|
||||
{
|
||||
fprintf(stderr,
|
||||
"FAIL: wna=%u lifted to week %u, %ld weeks from the "
|
||||
"receiver's %d\n",
|
||||
wna, almanac->week, delta, frame_week);
|
||||
failures++;
|
||||
return;
|
||||
}
|
||||
}
|
||||
printf(" WNa lifts to within +/-128 weeks for all 256 values\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* The consequence of the above, and the reason it is a hang and not a wrong
|
||||
* answer. Propagation reduced tk with `while(tk < -302400) tk += 604800;`, so
|
||||
* a week number 4.29e9 out cost 4.29e9 iterations - measured at 4.2 s per
|
||||
* satellite on a 5 GHz x86, which on a 50 MHz in-order Rocket core is hours,
|
||||
* times 32 satellites, inside the 10 s visibility refresh.
|
||||
*
|
||||
* Run it in a child under a wall-clock limit so a regression shows up as a
|
||||
* failed test rather than as a hung test run.
|
||||
*/
|
||||
static void test_propagation_cannot_take_unbounded_time(void)
|
||||
{
|
||||
gps_almanac_entry_t entry;
|
||||
memset(&entry, 0, sizeof(entry));
|
||||
entry.valid = true;
|
||||
entry.prn = 1u;
|
||||
entry.sqrt_a = 5153.6;
|
||||
entry.ecc = 0.006;
|
||||
entry.toa_sec = 147456.0;
|
||||
entry.i0_rad = (0.30 + 0.0047) * 3.1415926535897932;
|
||||
entry.omega_dot_rps = -8.0e-9;
|
||||
entry.omega0_rad = 1.0;
|
||||
entry.omega_rad = 0.5;
|
||||
entry.m0_rad = 2.0;
|
||||
|
||||
pid_t pid = fork();
|
||||
if(pid == 0)
|
||||
{
|
||||
double ecef[3];
|
||||
alarm(5);
|
||||
/* Every week number a corrupt lift can produce. */
|
||||
(void)gps_almanac_satellite_ecef(&entry, 4294967169u, 383u,
|
||||
300000.0, ecef);
|
||||
(void)gps_almanac_satellite_ecef(&entry, 0u, 4294967169u,
|
||||
300000.0, ecef);
|
||||
(void)gps_almanac_satellite_ecef(&entry, 383u, 0u, 300000.0, ecef);
|
||||
_exit(0);
|
||||
}
|
||||
CHECK(pid > 0, "fork");
|
||||
int status = 0;
|
||||
(void)waitpid(pid, &status, 0);
|
||||
CHECK(WIFEXITED(status) && (WEXITSTATUS(status) == 0),
|
||||
"propagation with a corrupt week number did not return promptly");
|
||||
printf(" propagation is closed form for every corrupt week number\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* A week number that cannot be a GPS week must not be adopted at all, from the
|
||||
* air or from flash. Without this a single bad record on the flash would keep
|
||||
* the visibility model switched off across every future boot.
|
||||
*/
|
||||
static void test_impossible_reference_week_is_refused(void)
|
||||
{
|
||||
gps_almanac_t almanac;
|
||||
gps_almanac_reset(&almanac);
|
||||
CHECK(!gps_almanac_set_reference(&almanac, 4294967169u, 147456.0),
|
||||
"an impossible week was adopted");
|
||||
CHECK(!almanac.reference_valid, "an impossible week was marked valid");
|
||||
CHECK(!gps_almanac_set_reference(&almanac, 2431u, 604800.0),
|
||||
"a toa outside the week was adopted");
|
||||
CHECK(gps_almanac_set_reference(&almanac, 2431u, 147456.0),
|
||||
"a real reference was refused");
|
||||
CHECK(almanac.reference_valid, "a real reference was not marked valid");
|
||||
printf(" impossible almanac references refused\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* The half-week wrap was applied to tk unconditionally, described as
|
||||
* IS-GPS-200 20.3.3.5.2.3. That paragraph is a week-crossover correction, not
|
||||
* a general reduction, and applying it as one is wrong because a GPS week is
|
||||
* 14.0384 orbital periods rather than 14: folding a week away moves the
|
||||
* satellite about 14 degrees along its orbit.
|
||||
*
|
||||
* Verified against an independent Python implementation of Table 20-IV: for
|
||||
* the almanac in test_gps_almanac.c, aliasing one week displaces the satellite
|
||||
* by 5161 km, and from a receiver at 40 N a satellite genuinely 5.1 degrees
|
||||
* below the horizon is reported 5.7 degrees above it. Persistence is what
|
||||
* made this reachable: an almanac read back from flash is routinely older than
|
||||
* half a week, whereas one collected live never is.
|
||||
*/
|
||||
static void test_stale_almanac_is_refused_not_aliased(void)
|
||||
{
|
||||
gps_almanac_entry_t entry;
|
||||
memset(&entry, 0, sizeof(entry));
|
||||
entry.valid = true;
|
||||
entry.prn = 1u;
|
||||
entry.ecc = 3902.0 * ldexp(1.0, -21);
|
||||
entry.toa_sec = 36.0 * 4096.0;
|
||||
entry.i0_rad = (0.30 + (2439.0 * ldexp(1.0, -19))) * 3.1415926535897932;
|
||||
entry.omega_dot_rps = -691.0 * ldexp(1.0, -38) * 3.1415926535897932;
|
||||
entry.sqrt_a = 10554475.0 * ldexp(1.0, -11);
|
||||
entry.omega0_rad = 677285.0 * ldexp(1.0, -23) * 3.1415926535897932;
|
||||
entry.omega_rad = 459593.0 * ldexp(1.0, -23) * 3.1415926535897932;
|
||||
entry.m0_rad = 301402.0 * ldexp(1.0, -23) * 3.1415926535897932;
|
||||
|
||||
/* Receiver near 40 N, 33 E. */
|
||||
const double receiver[3] = {4121000.0, 2699000.0, 4037000.0};
|
||||
|
||||
/*
|
||||
* 5.784 days after toa, expressed honestly as week 2432. The old code
|
||||
* folded tk back to -1.216 days and answered +5.67 degrees; the truth is
|
||||
* -5.10 degrees, on the other side of the -5 degree acquisition mask.
|
||||
*/
|
||||
double elevation = 0.0;
|
||||
CHECK(gps_almanac_look_angles(&entry, 2431u, 2432u, 42456.0, receiver,
|
||||
&elevation, NULL),
|
||||
"a 5.8 day old almanac should still propagate");
|
||||
CHECK(fabs(elevation - (-5.10)) < 0.2,
|
||||
"a 5.8 day old almanac was aliased by a week instead of propagated");
|
||||
|
||||
/*
|
||||
* What the unconditional wrap evaluated instead. tk was 499800 s; the
|
||||
* wrap folded it to -105000 s, which is the same as asking for the
|
||||
* satellite one week earlier - so it can be reproduced exactly by naming
|
||||
* that earlier instant.
|
||||
*/
|
||||
double aliased = 0.0;
|
||||
CHECK(gps_almanac_look_angles(&entry, 2431u, 2431u, 42456.0, receiver,
|
||||
&aliased, NULL),
|
||||
"alias reference case");
|
||||
printf(" 5.8 day old almanac: elevation %+.2f deg, "
|
||||
"one-week alias says %+.2f deg (%.1f deg apart)\n",
|
||||
elevation, aliased, fabs(elevation - aliased));
|
||||
CHECK(fabs(elevation - aliased) > 10.0,
|
||||
"the aliasing being regressed against no longer reproduces");
|
||||
CHECK((elevation < GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG) &&
|
||||
(aliased > GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG),
|
||||
"this case no longer straddles the acquisition elevation mask");
|
||||
|
||||
/* Beyond the usable window there must be no answer at all, so the PRN
|
||||
* stays fully searchable rather than being demoted on a guess. */
|
||||
double ignored = 0.0;
|
||||
CHECK(!gps_almanac_look_angles(&entry, 2431u, 2435u, 42456.0, receiver,
|
||||
&ignored, NULL),
|
||||
"an almanac 26 days old produced a look angle");
|
||||
printf(" 26 day old almanac produces no prediction\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* Before subframe 5 page 25 arrives there is no WNa, and the receiver has to
|
||||
* assume the almanac is current. That assumption belongs in one place and has
|
||||
* to reproduce the answer the real week gives, including across a week
|
||||
* boundary - otherwise the visible set would step every Sunday morning.
|
||||
*/
|
||||
static void test_assumed_week_matches_the_real_one(void)
|
||||
{
|
||||
gps_almanac_entry_t entry;
|
||||
memset(&entry, 0, sizeof(entry));
|
||||
entry.valid = true;
|
||||
entry.toa_sec = 589824.0; /* late in the week */
|
||||
|
||||
/* Same instant, one week later in the numbering: toa is in the previous
|
||||
* week, so the guess has to reach back for it. */
|
||||
CHECK(gps_almanac_assumed_week(&entry, 2432u, 100.0) == 2431u,
|
||||
"assumed week did not reach back across the week boundary");
|
||||
CHECK(gps_almanac_assumed_week(&entry, 2431u, 600000.0) == 2431u,
|
||||
"assumed week moved when it should not have");
|
||||
|
||||
entry.toa_sec = 4096.0; /* early in the week */
|
||||
CHECK(gps_almanac_assumed_week(&entry, 2431u, 600000.0) == 2432u,
|
||||
"assumed week did not reach forward across the week boundary");
|
||||
printf(" assumed almanac week is exact either side of a rollover\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* Health only ever demotes, so it must not be trusted further than the orbit
|
||||
* it came with. Since persistence an entry can be weeks old and still carry a
|
||||
* health byte that was true when it was written and is not now; an almanac too
|
||||
* old to say where a satellite is cannot be allowed to say it is sick.
|
||||
*/
|
||||
static void test_stale_health_does_not_demote(void)
|
||||
{
|
||||
gps_almanac_t almanac;
|
||||
uint32_t mask = 0u;
|
||||
const double receiver[3] = {4121000.0, 2699000.0, 4037000.0};
|
||||
|
||||
gps_almanac_reset(&almanac);
|
||||
almanac.sv[0].valid = true;
|
||||
almanac.sv[0].prn = 1u;
|
||||
almanac.sv[0].health = 0x3fu; /* marked unhealthy long ago */
|
||||
almanac.sv[0].ecc = 0.006;
|
||||
almanac.sv[0].toa_sec = 147456.0;
|
||||
almanac.sv[0].i0_rad = 0.3047 * 3.1415926535897932;
|
||||
almanac.sv[0].sqrt_a = 5153.6;
|
||||
almanac.sv[0].omega0_rad = 1.0;
|
||||
almanac.sv[0].omega_rad = 0.5;
|
||||
almanac.sv[0].m0_rad = 2.0;
|
||||
/* Give it a second entry so the mask is not the all-ones fallback. */
|
||||
almanac.sv[1] = almanac.sv[0];
|
||||
almanac.sv[1].prn = 2u;
|
||||
almanac.sv[1].health = 0u;
|
||||
gps_almanac_set_reference(&almanac, 2431u, 147456.0);
|
||||
|
||||
/* Fresh almanac: an unhealthy satellite is worth de-prioritising. */
|
||||
(void)gps_almanac_visible_mask(&almanac, 2431u, 200000.0, receiver,
|
||||
-5.0, &mask);
|
||||
CHECK((mask & 1u) == 0u,
|
||||
"a fresh unhealthy satellite was not demoted");
|
||||
|
||||
/* Four weeks later the same health byte proves nothing. */
|
||||
(void)gps_almanac_visible_mask(&almanac, 2435u, 200000.0, receiver,
|
||||
-5.0, &mask);
|
||||
CHECK((mask & 1u) != 0u,
|
||||
"a four-week-old health byte still demoted a satellite");
|
||||
printf(" stale health no longer demotes; fresh health still does\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* The seed persisted to flash says where the receiver was, and a receiver that
|
||||
* was carried somewhere else while switched off has no way to know that until
|
||||
* it has a fix. Aiming the acquisition mask at the old site would then demote
|
||||
* satellites that are up at the new one, which is the one failure mode the
|
||||
* whole feature is supposed to avoid.
|
||||
*
|
||||
* Measure it: build a realistic constellation, take the true visible set at
|
||||
* the new site, and check what a mask computed from the old seed would say.
|
||||
* The widened seed mask has to keep every genuinely visible satellite
|
||||
* searchable at the fast cadence.
|
||||
*/
|
||||
static void test_migration_between_power_cycles(void)
|
||||
{
|
||||
gps_almanac_t almanac;
|
||||
gps_almanac_reset(&almanac);
|
||||
for(unsigned int i = 0u; i < GPS_ALMANAC_PRN_COUNT; i++)
|
||||
{
|
||||
gps_almanac_entry_t* e = &almanac.sv[i];
|
||||
e->valid = true;
|
||||
e->prn = (unsigned char)(i + 1u);
|
||||
e->health = 0u;
|
||||
e->ecc = 0.006;
|
||||
e->toa_sec = 147456.0;
|
||||
e->i0_rad = 0.3047 * 3.1415926535897932;
|
||||
e->sqrt_a = 5153.6;
|
||||
/* Six planes, spread within each - the real constellation's shape. */
|
||||
e->omega0_rad = (double)(i % 6u) * (2.0 * 3.1415926535897932 / 6.0);
|
||||
e->omega_rad = 0.0;
|
||||
e->m0_rad = (double)(i / 6u) * (2.0 * 3.1415926535897932 / 6.0) +
|
||||
(double)(i % 6u) * 0.7;
|
||||
}
|
||||
gps_almanac_set_reference(&almanac, 2431u, 147456.0);
|
||||
|
||||
/* Powered off in Ankara, powered on 1600 km away in Berlin. */
|
||||
const double seed[3] = {4121000.0, 2699000.0, 4037000.0};
|
||||
const double actual[3] = {3783000.0, 901000.0, 5038000.0};
|
||||
|
||||
uint32_t truth = 0u;
|
||||
uint32_t narrow = 0u;
|
||||
uint32_t wide = 0u;
|
||||
unsigned int visible = gps_almanac_visible_mask(
|
||||
&almanac, 2431u, 300000.0, actual,
|
||||
GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG, &truth);
|
||||
(void)gps_almanac_visible_mask(&almanac, 2431u, 300000.0, seed,
|
||||
GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG,
|
||||
&narrow);
|
||||
(void)gps_almanac_visible_mask(&almanac, 2431u, 300000.0, seed,
|
||||
GPS_ACQ_VISIBILITY_SEED_MASK_ELEVATION_DEG,
|
||||
&wide);
|
||||
|
||||
unsigned int missed_narrow = 0u;
|
||||
unsigned int missed_wide = 0u;
|
||||
for(unsigned int i = 0u; i < GPS_ALMANAC_PRN_COUNT; i++)
|
||||
{
|
||||
uint32_t bit = 1u << i;
|
||||
if((truth & bit) == 0u)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
if((narrow & bit) == 0u) { missed_narrow++; }
|
||||
if((wide & bit) == 0u) { missed_wide++; }
|
||||
}
|
||||
|
||||
printf(" 1600 km migration: %u satellites up at the new site; "
|
||||
"old seed at %.0f deg demotes %u of them, at %.0f deg demotes %u\n",
|
||||
visible, (double)GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG,
|
||||
missed_narrow, (double)GPS_ACQ_VISIBILITY_SEED_MASK_ELEVATION_DEG,
|
||||
missed_wide);
|
||||
CHECK(missed_wide == 0u,
|
||||
"the widened seed mask still demoted a visible satellite");
|
||||
CHECK(missed_wide <= missed_narrow,
|
||||
"widening the mask made the prediction worse");
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------ */
|
||||
/* Flash: the wear policy against a realistic duty cycle. */
|
||||
/* ------------------------------------------------------------------ */
|
||||
|
||||
#define SECTOR_SIZE 8192u
|
||||
#define FLASH_SIZE (SECTOR_SIZE * 2u)
|
||||
|
||||
typedef struct {
|
||||
uint8_t data[FLASH_SIZE];
|
||||
uint32_t erase_count;
|
||||
uint32_t program_count;
|
||||
} sim_flash_t;
|
||||
|
||||
static void sim_read(void* ctx, uint32_t address, void* dst, uint32_t len)
|
||||
{
|
||||
memcpy(dst, &((sim_flash_t*)ctx)->data[address], len);
|
||||
}
|
||||
|
||||
static bool sim_erase(void* ctx, uint32_t address)
|
||||
{
|
||||
sim_flash_t* f = (sim_flash_t*)ctx;
|
||||
memset(&f->data[address], 0xff, SECTOR_SIZE);
|
||||
f->erase_count++;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool sim_program(void* ctx, uint32_t address, const void* src,
|
||||
uint32_t len)
|
||||
{
|
||||
sim_flash_t* f = (sim_flash_t*)ctx;
|
||||
const uint8_t* in = (const uint8_t*)src;
|
||||
for(uint32_t i = 0u; i < len; i++)
|
||||
{
|
||||
f->data[address + i] &= in[i]; /* programming only clears bits */
|
||||
}
|
||||
f->program_count++;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void sim_bind(gps_ephemeris_store_flash_t* flash, sim_flash_t* sim)
|
||||
{
|
||||
memset(sim, 0, sizeof(*sim));
|
||||
memset(sim->data, 0xff, sizeof(sim->data));
|
||||
memset(flash, 0, sizeof(*flash));
|
||||
flash->read = sim_read;
|
||||
flash->erase_sector = sim_erase;
|
||||
flash->program = sim_program;
|
||||
flash->ctx = sim;
|
||||
flash->base_address = 0u;
|
||||
flash->sector_size = SECTOR_SIZE;
|
||||
}
|
||||
|
||||
static void fill_static_almanac(gps_nav_context_payload_t* p)
|
||||
{
|
||||
for(unsigned int prn = 0u; prn < 24u; prn++)
|
||||
{
|
||||
for(unsigned int w = 0u; w < GPS_NAV_CONTEXT_WORDS; w++)
|
||||
{
|
||||
p->almanac_words[prn][w] = 0x11000000u + (prn * 97u) + w;
|
||||
}
|
||||
p->almanac_present_mask |= (1u << prn);
|
||||
}
|
||||
p->almanac_reference_valid = 1u;
|
||||
p->almanac_week = 2431u;
|
||||
p->almanac_toa_sec = 147456u;
|
||||
p->iono_valid = 1u;
|
||||
for(unsigned int w = 0u; w < GPS_NAV_CONTEXT_WORDS; w++)
|
||||
{
|
||||
p->iono_words[w] = 0x22000000u + w;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* One day of a stationary receiver: the constellation broadcasts the same
|
||||
* almanac, page 18 does not change, and the WLS position wanders by a few
|
||||
* metres of noise around one point, exactly as the board's own log shows.
|
||||
*
|
||||
* The whole point of the store's CRC comparison is that this costs nothing.
|
||||
* It did not: the payload carried the raw WLS position and the receiver time
|
||||
* it was taken at, both of which move every epoch, so the CRC differed on
|
||||
* every save and the erase count followed the 60 s timer. Board evidence:
|
||||
* generation advanced 12 -> 17 across one short capture with a single skipped
|
||||
* write. That is 1440 erase cycles a day against a 100k cycle endurance
|
||||
* shared by two sectors: about 139 days to wear-out, to persist a seed that is
|
||||
* only ever read once, at the next boot.
|
||||
*/
|
||||
#define SAVES_PER_DAY (24u * 60u) /* GPS_NAV_CONTEXT_SAVE_PERIOD_TICKS = 60 s */
|
||||
|
||||
/* The receiver stands still; the WLS fix scatters by metres, as it does on the
|
||||
* board, and receiver time advances monotonically. */
|
||||
static void stationary_fix(unsigned int i, double fix[3])
|
||||
{
|
||||
static const double truth[3] = {4121000.0, 2699000.0, 4037000.0};
|
||||
fix[0] = truth[0] + (double)((int)(i % 17u) - 8);
|
||||
fix[1] = truth[1] + (double)((int)(i % 11u) - 5);
|
||||
fix[2] = truth[2] + (double)((int)(i % 23u) - 11);
|
||||
}
|
||||
|
||||
/* One day of periodic saves, returning the number of sector erases. */
|
||||
static uint32_t run_one_day(bool hold_position, uint32_t* skipped_out)
|
||||
{
|
||||
sim_flash_t sim;
|
||||
gps_ephemeris_store_flash_t flash;
|
||||
gps_nav_context_store_t store;
|
||||
sim_bind(&flash, &sim);
|
||||
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
|
||||
|
||||
double held[3] = {0.0, 0.0, 0.0};
|
||||
bool held_valid = false;
|
||||
uint32_t held_week = 0u;
|
||||
double held_tow = 0.0;
|
||||
uint64_t held_tick = 0u;
|
||||
|
||||
for(unsigned int i = 0u; i < SAVES_PER_DAY; i++)
|
||||
{
|
||||
uint64_t now = (uint64_t)(i + 1u) * 60u * GPS_SYSTICK_HZ;
|
||||
double fix[3];
|
||||
stationary_fix(i, fix);
|
||||
|
||||
bool take;
|
||||
if(hold_position)
|
||||
{
|
||||
/* The policy gps_nav_context_flash_service() now applies. */
|
||||
bool moved = !held_valid ||
|
||||
gps_nav_context_position_changed(
|
||||
held, fix, GPS_NAV_CONTEXT_POSITION_EPSILON_M);
|
||||
bool due = (held_tick == 0u) ||
|
||||
((now - held_tick) >= GPS_NAV_CONTEXT_POSITION_REFRESH_TICKS);
|
||||
take = moved && due;
|
||||
}
|
||||
else
|
||||
{
|
||||
/* What it did before: take whatever the last fix said, every
|
||||
* time, along with the receiver time it was taken at. */
|
||||
take = true;
|
||||
}
|
||||
if(take)
|
||||
{
|
||||
held[0] = fix[0];
|
||||
held[1] = fix[1];
|
||||
held[2] = fix[2];
|
||||
held_valid = true;
|
||||
held_tick = now;
|
||||
held_week = 2431u;
|
||||
held_tow = 550000.0 + (double)i * 60.0;
|
||||
}
|
||||
|
||||
gps_nav_context_payload_t payload;
|
||||
memset(&payload, 0, sizeof(payload));
|
||||
fill_static_almanac(&payload);
|
||||
if(held_valid)
|
||||
{
|
||||
payload.position_valid = 1u;
|
||||
payload.position_ecef_m[0] = held[0];
|
||||
payload.position_ecef_m[1] = held[1];
|
||||
payload.position_ecef_m[2] = held[2];
|
||||
payload.position_week = held_week;
|
||||
payload.position_tow_sec = held_tow;
|
||||
}
|
||||
|
||||
bool wrote = false;
|
||||
CHECK(gps_nav_context_store_save(&store, &payload, &wrote), "save");
|
||||
}
|
||||
|
||||
if(skipped_out != NULL)
|
||||
{
|
||||
*skipped_out = store.skipped_writes;
|
||||
}
|
||||
return sim.erase_count;
|
||||
}
|
||||
|
||||
static void test_stationary_receiver_does_not_wear_the_flash(void)
|
||||
{
|
||||
uint32_t before_skipped = 0u;
|
||||
uint32_t after_skipped = 0u;
|
||||
uint32_t before = run_one_day(false, &before_skipped);
|
||||
uint32_t after = run_one_day(true, &after_skipped);
|
||||
|
||||
/* Two sectors alternate, so each sees half the cycles; S25FL256S is
|
||||
* specified for 100k program/erase cycles per sector. */
|
||||
printf(" 24 h stationary, %u saves: taking every fix -> %u erases "
|
||||
"(%u skipped), %.1f years to 100k cycles\n",
|
||||
SAVES_PER_DAY, before, before_skipped,
|
||||
(before > 0u) ? (100000.0 * 2.0) / ((double)before * 365.0) : 0.0);
|
||||
printf(" 24 h stationary, %u saves: held seed -> %u erases "
|
||||
"(%u skipped), %.0f years to 100k cycles\n",
|
||||
SAVES_PER_DAY, after, after_skipped,
|
||||
(after > 0u) ? (100000.0 * 2.0) / ((double)after * 365.0) : 0.0);
|
||||
|
||||
CHECK(before >= (SAVES_PER_DAY - 1u),
|
||||
"the write amplification being regressed against no longer reproduces");
|
||||
CHECK(after <= 2u,
|
||||
"a stationary receiver erased the flash more than twice in a day");
|
||||
CHECK(after_skipped >= (SAVES_PER_DAY - 2u),
|
||||
"unchanged saves were not skipped");
|
||||
}
|
||||
|
||||
/*
|
||||
* The other half of the same policy: a receiver that really has moved must
|
||||
* still update its seed, otherwise a relocated receiver would cold start with
|
||||
* a visibility prediction aimed at the old site for ever.
|
||||
*/
|
||||
static void test_a_moved_receiver_still_updates_its_seed(void)
|
||||
{
|
||||
const double from[3] = {4121000.0, 2699000.0, 4037000.0};
|
||||
double to[3] = {4121000.0 + 60000.0, 2699000.0, 4037000.0};
|
||||
CHECK(gps_nav_context_position_changed(from, to,
|
||||
GPS_NAV_CONTEXT_POSITION_EPSILON_M),
|
||||
"a 60 km move was not seen as a move");
|
||||
|
||||
to[0] = from[0] + 3.0;
|
||||
CHECK(!gps_nav_context_position_changed(
|
||||
from, to, GPS_NAV_CONTEXT_POSITION_EPSILON_M),
|
||||
"3 m of WLS scatter was treated as a move");
|
||||
|
||||
double nan_position[3] = {0.0, 0.0, 0.0};
|
||||
nan_position[0] = NAN;
|
||||
CHECK(!gps_nav_context_position_changed(
|
||||
from, nan_position, GPS_NAV_CONTEXT_POSITION_EPSILON_M),
|
||||
"a non-finite position was accepted as a move");
|
||||
printf(" seed refresh triggers on kilometres, not on scatter\n");
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------ */
|
||||
/* Ephemeris restored from flash versus what is actually on the air. */
|
||||
/* ------------------------------------------------------------------ */
|
||||
|
||||
static void make_ephemeris(gps_ephemeris_t* eph, unsigned int prn,
|
||||
unsigned char iode, double toe_sec)
|
||||
{
|
||||
memset(eph, 0, sizeof(*eph));
|
||||
eph->ephemeris_valid = true;
|
||||
eph->SVID = (unsigned char)prn;
|
||||
eph->iode = iode;
|
||||
eph->iodc = iode;
|
||||
eph->toe.sec = toe_sec;
|
||||
eph->toc.sec = toe_sec;
|
||||
eph->toe.week = 2431;
|
||||
eph->toe_week_valid = true;
|
||||
eph->A = 26560000.0;
|
||||
eph->sqrta = 5153.6;
|
||||
eph->ecc = 0.006;
|
||||
eph->sva = 2u;
|
||||
eph->svhlth = 0u;
|
||||
eph->update = 0u;
|
||||
}
|
||||
|
||||
/*
|
||||
* Toe is broadcast as a second-of-week, so two Toe values are ordered with the
|
||||
* signed half-week rule. That is exact between records from the same era and
|
||||
* silently inverted once the gap exceeds half a week - which is precisely what
|
||||
* persistence introduced. Switch the receiver off on Friday and on again on
|
||||
* Tuesday and the four-day-old flash record outranks the live broadcast: the
|
||||
* cache refuses the fresh data set, the flash keeps being rewritten with the
|
||||
* stale one, and every subsequent cold start restores it again.
|
||||
*/
|
||||
static void test_broadcast_beats_a_record_read_back_from_flash(void)
|
||||
{
|
||||
gps_ephemeris_cache_t cache;
|
||||
gps_ephemeris_t restored;
|
||||
gps_ephemeris_t broadcast;
|
||||
gps_ephemeris_t out;
|
||||
|
||||
/* A four-day gap: the raw Toe difference is 345600 s, past half a week,
|
||||
* so the half-week rule reads the newer record as the older one. */
|
||||
make_ephemeris(&restored, 5u, 0x40u, 100000.0);
|
||||
make_ephemeris(&broadcast, 5u, 0x41u, 100000.0 + 345600.0 - 604800.0 +
|
||||
604800.0);
|
||||
broadcast.toe.sec = fmod(100000.0 + 345600.0, 604800.0);
|
||||
|
||||
CHECK(!gps_ephemeris_supersedes(&broadcast, &restored),
|
||||
"the half-week rule was expected to mis-order these; test is stale");
|
||||
|
||||
gps_ephemeris_cache_init(&cache, 1000000u);
|
||||
CHECK(gps_ephemeris_cache_store_restored(&cache, 5u, &restored, 10u),
|
||||
"flash record not adopted");
|
||||
|
||||
bool changed = false;
|
||||
CHECK(gps_ephemeris_cache_store_if_new(&cache, 5u, &broadcast, 20u,
|
||||
&changed),
|
||||
"live broadcast was refused in favour of a flash record");
|
||||
CHECK(changed, "adopting the live broadcast was not reported as a change");
|
||||
CHECK(gps_ephemeris_cache_restore(&cache, 5u, 30u, &out), "restore");
|
||||
CHECK(out.iode == 0x41u, "the cache kept the stale flash record");
|
||||
printf(" live broadcast replaces a stale flash record\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* The flag must not become a way for flash to overwrite live data, and it must
|
||||
* clear once used, so ordinary Toe ordering resumes for everything after.
|
||||
*/
|
||||
static void test_restored_never_displaces_live_data(void)
|
||||
{
|
||||
gps_ephemeris_cache_t cache;
|
||||
gps_ephemeris_t live;
|
||||
gps_ephemeris_t stale;
|
||||
gps_ephemeris_t older;
|
||||
gps_ephemeris_t out;
|
||||
|
||||
gps_ephemeris_cache_init(&cache, 1000000u);
|
||||
make_ephemeris(&live, 7u, 0x20u, 300000.0);
|
||||
make_ephemeris(&stale, 7u, 0x10u, 100000.0);
|
||||
|
||||
bool changed = false;
|
||||
CHECK(gps_ephemeris_cache_store_if_new(&cache, 7u, &live, 10u, &changed),
|
||||
"live record not stored");
|
||||
CHECK(!gps_ephemeris_cache_store_restored(&cache, 7u, &stale, 20u),
|
||||
"a flash record displaced live data");
|
||||
CHECK(gps_ephemeris_cache_restore(&cache, 7u, 30u, &out), "restore");
|
||||
CHECK(out.iode == 0x20u, "live record was lost");
|
||||
|
||||
/* And after one broadcast has cleared the flag, an older data set is
|
||||
* rejected on Toe again as it always was. */
|
||||
make_ephemeris(&older, 7u, 0x1fu, 290000.0);
|
||||
CHECK(!gps_ephemeris_cache_store_if_new(&cache, 7u, &older, 40u, &changed),
|
||||
"an older data set was adopted after the restored flag cleared");
|
||||
printf(" restored records never outrank live ones\n");
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_almanac_reference_week_is_lifted_sanely();
|
||||
test_propagation_cannot_take_unbounded_time();
|
||||
test_impossible_reference_week_is_refused();
|
||||
test_stale_almanac_is_refused_not_aliased();
|
||||
test_assumed_week_matches_the_real_one();
|
||||
test_stale_health_does_not_demote();
|
||||
test_migration_between_power_cycles();
|
||||
test_stationary_receiver_does_not_wear_the_flash();
|
||||
test_a_moved_receiver_still_updates_its_seed();
|
||||
test_broadcast_beats_a_record_read_back_from_flash();
|
||||
test_restored_never_displaces_live_data();
|
||||
|
||||
if(failures != 0)
|
||||
{
|
||||
fprintf(stderr, "%d check(s) failed\n", failures);
|
||||
return 1;
|
||||
}
|
||||
printf("gps nav context audit tests passed\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,350 @@
|
||||
/*
|
||||
* Persistent navigation context store against a simulated SPI NOR device.
|
||||
*
|
||||
* The simulator enforces what makes flash different from RAM: programming can
|
||||
* only clear bits, a program into a non-erased page corrupts it, and a write
|
||||
* can be cut short at any byte to model power loss.
|
||||
*/
|
||||
|
||||
#include <assert.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "gps/gps_nav_context_store.h"
|
||||
|
||||
#define SECTOR_SIZE 8192u
|
||||
#define SECTORS 2u
|
||||
#define FLASH_SIZE (SECTOR_SIZE * SECTORS)
|
||||
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
typedef struct {
|
||||
uint8_t data[FLASH_SIZE];
|
||||
uint32_t erase_count;
|
||||
uint32_t program_count;
|
||||
int32_t fail_after_programs;
|
||||
uint32_t truncate_bytes;
|
||||
} sim_flash_t;
|
||||
|
||||
static void sim_init(sim_flash_t* f)
|
||||
{
|
||||
memset(f, 0, sizeof(*f));
|
||||
memset(f->data, 0xff, sizeof(f->data));
|
||||
f->fail_after_programs = -1;
|
||||
}
|
||||
|
||||
static void sim_read(void* ctx, uint32_t address, void* dst, uint32_t len)
|
||||
{
|
||||
sim_flash_t* f = (sim_flash_t*)ctx;
|
||||
assert((address + len) <= FLASH_SIZE);
|
||||
memcpy(dst, &f->data[address], len);
|
||||
}
|
||||
|
||||
static bool sim_erase(void* ctx, uint32_t address)
|
||||
{
|
||||
sim_flash_t* f = (sim_flash_t*)ctx;
|
||||
assert((address % SECTOR_SIZE) == 0u);
|
||||
assert(address < FLASH_SIZE);
|
||||
memset(&f->data[address], 0xff, SECTOR_SIZE);
|
||||
f->erase_count++;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool sim_program(void* ctx, uint32_t address, const void* src,
|
||||
uint32_t len)
|
||||
{
|
||||
sim_flash_t* f = (sim_flash_t*)ctx;
|
||||
const uint8_t* in = (const uint8_t*)src;
|
||||
assert((address + len) <= FLASH_SIZE);
|
||||
/* A real page program cannot span a 256-byte page. */
|
||||
assert(((address % 256u) + len) <= 256u);
|
||||
|
||||
uint32_t writable = len;
|
||||
bool truncated = false;
|
||||
if((f->fail_after_programs >= 0) &&
|
||||
(f->program_count == (uint32_t)f->fail_after_programs))
|
||||
{
|
||||
writable = f->truncate_bytes;
|
||||
truncated = true;
|
||||
}
|
||||
for(uint32_t i = 0u; i < writable; i++)
|
||||
{
|
||||
/* Programming can only clear bits. */
|
||||
f->data[address + i] &= in[i];
|
||||
}
|
||||
f->program_count++;
|
||||
return !truncated;
|
||||
}
|
||||
|
||||
static void bind(gps_ephemeris_store_flash_t* flash, sim_flash_t* sim)
|
||||
{
|
||||
memset(flash, 0, sizeof(*flash));
|
||||
flash->read = sim_read;
|
||||
flash->erase_sector = sim_erase;
|
||||
flash->program = sim_program;
|
||||
flash->ctx = sim;
|
||||
flash->base_address = 0u;
|
||||
flash->sector_size = SECTOR_SIZE;
|
||||
}
|
||||
|
||||
static void fill_payload(gps_nav_context_payload_t* p, uint32_t seed)
|
||||
{
|
||||
memset(p, 0, sizeof(*p));
|
||||
for(unsigned int prn = 0u; prn < 8u; prn++)
|
||||
{
|
||||
for(unsigned int w = 0u; w < GPS_NAV_CONTEXT_WORDS; w++)
|
||||
{
|
||||
p->almanac_words[prn][w] = (seed * 7u) + (prn * 31u) + w;
|
||||
}
|
||||
p->almanac_present_mask |= (1u << prn);
|
||||
}
|
||||
p->almanac_reference_valid = 1u;
|
||||
p->almanac_week = 2431u;
|
||||
p->almanac_toa_sec = 147456u;
|
||||
p->iono_valid = 1u;
|
||||
for(unsigned int w = 0u; w < GPS_NAV_CONTEXT_WORDS; w++)
|
||||
{
|
||||
p->iono_words[w] = seed + w;
|
||||
}
|
||||
p->position_valid = 1u;
|
||||
p->position_ecef_m[0] = 4128000.0 + seed;
|
||||
p->position_ecef_m[1] = 2530000.0;
|
||||
p->position_ecef_m[2] = 4136000.0;
|
||||
p->position_week = 2431u;
|
||||
p->position_tow_sec = 550000.0;
|
||||
}
|
||||
|
||||
static void test_empty_mount_holds_nothing(void)
|
||||
{
|
||||
sim_flash_t sim;
|
||||
gps_ephemeris_store_flash_t flash;
|
||||
gps_nav_context_store_t store;
|
||||
gps_nav_context_payload_t payload;
|
||||
sim_init(&sim);
|
||||
bind(&flash, &sim);
|
||||
|
||||
CHECK(gps_nav_context_store_mount(&store, &flash), "blank store mounts");
|
||||
CHECK(!gps_nav_context_store_load(&store, &payload),
|
||||
"blank store holds nothing");
|
||||
CHECK(sim.erase_count == 0u, "mounting does not erase");
|
||||
printf(" empty mount ok\n");
|
||||
}
|
||||
|
||||
static void test_round_trip(void)
|
||||
{
|
||||
sim_flash_t sim;
|
||||
gps_ephemeris_store_flash_t flash;
|
||||
gps_nav_context_store_t store;
|
||||
gps_nav_context_payload_t written;
|
||||
gps_nav_context_payload_t read;
|
||||
sim_init(&sim);
|
||||
bind(&flash, &sim);
|
||||
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
|
||||
|
||||
fill_payload(&written, 3u);
|
||||
bool wrote = false;
|
||||
CHECK(gps_nav_context_store_save(&store, &written, &wrote), "save");
|
||||
CHECK(wrote, "first save writes");
|
||||
CHECK(gps_nav_context_store_load(&store, &read), "load after save");
|
||||
CHECK(memcmp(&written, &read, sizeof(written)) == 0,
|
||||
"payload survives round trip");
|
||||
|
||||
/* And it survives a remount, which is the case that actually matters. */
|
||||
gps_nav_context_store_t remounted;
|
||||
CHECK(gps_nav_context_store_mount(&remounted, &flash), "remount");
|
||||
memset(&read, 0, sizeof(read));
|
||||
CHECK(gps_nav_context_store_load(&remounted, &read), "load after remount");
|
||||
CHECK(memcmp(&written, &read, sizeof(written)) == 0,
|
||||
"payload survives remount");
|
||||
printf(" round trip ok\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* The wear policy, and the property the feature was asked for: saving the same
|
||||
* content again must not touch the flash at all. Almanac pages are rebroadcast
|
||||
* continuously, so without this the erase count would follow the save timer
|
||||
* rather than how often the constellation publishes new data.
|
||||
*/
|
||||
static void test_unchanged_save_does_not_touch_flash(void)
|
||||
{
|
||||
sim_flash_t sim;
|
||||
gps_ephemeris_store_flash_t flash;
|
||||
gps_nav_context_store_t store;
|
||||
gps_nav_context_payload_t payload;
|
||||
sim_init(&sim);
|
||||
bind(&flash, &sim);
|
||||
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
|
||||
|
||||
fill_payload(&payload, 5u);
|
||||
bool wrote = false;
|
||||
CHECK(gps_nav_context_store_save(&store, &payload, &wrote), "first save");
|
||||
CHECK(wrote, "first save writes");
|
||||
|
||||
uint32_t erases = sim.erase_count;
|
||||
uint32_t programs = sim.program_count;
|
||||
for(unsigned int i = 0u; i < 50u; i++)
|
||||
{
|
||||
wrote = true;
|
||||
CHECK(gps_nav_context_store_save(&store, &payload, &wrote),
|
||||
"repeat save succeeds");
|
||||
CHECK(!wrote, "repeat save reports no write");
|
||||
}
|
||||
CHECK(sim.erase_count == erases, "repeat saves cause no erase");
|
||||
CHECK(sim.program_count == programs, "repeat saves cause no program");
|
||||
CHECK(store.skipped_writes == 50u, "skipped writes counted");
|
||||
|
||||
/* A genuine change must still be written. */
|
||||
gps_nav_context_payload_t changed;
|
||||
fill_payload(&changed, 9u);
|
||||
CHECK(gps_nav_context_store_save(&store, &changed, &wrote), "changed save");
|
||||
CHECK(wrote, "changed content writes");
|
||||
CHECK(sim.erase_count == (erases + 1u), "changed save erases once");
|
||||
printf(" unchanged saves never reach the flash (%u skipped, %u erases)\n",
|
||||
store.skipped_writes, sim.erase_count);
|
||||
}
|
||||
|
||||
static void test_alternates_sectors(void)
|
||||
{
|
||||
sim_flash_t sim;
|
||||
gps_ephemeris_store_flash_t flash;
|
||||
gps_nav_context_store_t store;
|
||||
gps_nav_context_payload_t payload;
|
||||
sim_init(&sim);
|
||||
bind(&flash, &sim);
|
||||
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
|
||||
|
||||
unsigned int previous = 0u;
|
||||
for(unsigned int i = 0u; i < 4u; i++)
|
||||
{
|
||||
fill_payload(&payload, i + 1u);
|
||||
bool wrote = false;
|
||||
CHECK(gps_nav_context_store_save(&store, &payload, &wrote), "save");
|
||||
CHECK(wrote, "distinct content writes");
|
||||
if(i != 0u)
|
||||
{
|
||||
CHECK(store.active_sector != previous,
|
||||
"each save lands in the other sector");
|
||||
}
|
||||
previous = store.active_sector;
|
||||
}
|
||||
printf(" A/B alternation ok\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* Power loss during a save must never lose the copy that was already good.
|
||||
* The header is programmed last, so a torn save leaves a sector that does not
|
||||
* validate and mount falls back to the previous generation.
|
||||
*/
|
||||
static void test_power_loss_keeps_previous_generation(void)
|
||||
{
|
||||
sim_flash_t sim;
|
||||
gps_ephemeris_store_flash_t flash;
|
||||
gps_nav_context_store_t store;
|
||||
gps_nav_context_payload_t first;
|
||||
gps_nav_context_payload_t second;
|
||||
gps_nav_context_payload_t read;
|
||||
sim_init(&sim);
|
||||
bind(&flash, &sim);
|
||||
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
|
||||
|
||||
fill_payload(&first, 11u);
|
||||
bool wrote = false;
|
||||
CHECK(gps_nav_context_store_save(&store, &first, &wrote), "first save");
|
||||
|
||||
/* Cut the very next save short partway through its payload. */
|
||||
sim.fail_after_programs = (int32_t)sim.program_count + 2;
|
||||
sim.truncate_bytes = 7u;
|
||||
fill_payload(&second, 22u);
|
||||
(void)gps_nav_context_store_save(&store, &second, &wrote);
|
||||
|
||||
gps_nav_context_store_t remounted;
|
||||
CHECK(gps_nav_context_store_mount(&remounted, &flash),
|
||||
"remount after power loss");
|
||||
CHECK(gps_nav_context_store_load(&remounted, &read),
|
||||
"still holds a payload");
|
||||
CHECK(memcmp(&first, &read, sizeof(first)) == 0,
|
||||
"the torn save did not replace the good copy");
|
||||
printf(" power loss preserves previous generation\n");
|
||||
}
|
||||
|
||||
static void test_corrupt_payload_is_rejected(void)
|
||||
{
|
||||
sim_flash_t sim;
|
||||
gps_ephemeris_store_flash_t flash;
|
||||
gps_nav_context_store_t store;
|
||||
gps_nav_context_payload_t payload;
|
||||
gps_nav_context_payload_t read;
|
||||
sim_init(&sim);
|
||||
bind(&flash, &sim);
|
||||
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
|
||||
fill_payload(&payload, 4u);
|
||||
bool wrote = false;
|
||||
CHECK(gps_nav_context_store_save(&store, &payload, &wrote), "save");
|
||||
|
||||
/*
|
||||
* Clear a bit in the payload; only bit clears are physically possible on
|
||||
* NOR. Find a byte that actually has a bit set, otherwise the "corruption"
|
||||
* is a no-op and the test passes for the wrong reason.
|
||||
*/
|
||||
bool corrupted = false;
|
||||
for(uint32_t i = 256u; i < (256u + sizeof(payload)); i++)
|
||||
{
|
||||
if(sim.data[i] != 0x00u)
|
||||
{
|
||||
sim.data[i] &= (uint8_t)(sim.data[i] - 1u);
|
||||
corrupted = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
CHECK(corrupted, "test actually corrupted a payload byte");
|
||||
|
||||
gps_nav_context_store_t remounted;
|
||||
CHECK(gps_nav_context_store_mount(&remounted, &flash), "remount");
|
||||
CHECK(!gps_nav_context_store_load(&remounted, &read),
|
||||
"payload failing CRC is not served");
|
||||
printf(" corrupt payload rejected\n");
|
||||
}
|
||||
|
||||
static void test_format_clears(void)
|
||||
{
|
||||
sim_flash_t sim;
|
||||
gps_ephemeris_store_flash_t flash;
|
||||
gps_nav_context_store_t store;
|
||||
gps_nav_context_payload_t payload;
|
||||
sim_init(&sim);
|
||||
bind(&flash, &sim);
|
||||
CHECK(gps_nav_context_store_mount(&store, &flash), "mount");
|
||||
fill_payload(&payload, 6u);
|
||||
bool wrote = false;
|
||||
CHECK(gps_nav_context_store_save(&store, &payload, &wrote), "save");
|
||||
CHECK(gps_nav_context_store_format(&store), "format");
|
||||
CHECK(!gps_nav_context_store_load(&store, &payload), "format clears");
|
||||
printf(" format ok\n");
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_empty_mount_holds_nothing();
|
||||
test_round_trip();
|
||||
test_unchanged_save_does_not_touch_flash();
|
||||
test_alternates_sectors();
|
||||
test_power_loss_keeps_previous_generation();
|
||||
test_corrupt_payload_is_rejected();
|
||||
test_format_clears();
|
||||
if(failures != 0)
|
||||
{
|
||||
fprintf(stderr, "%d check(s) failed\n", failures);
|
||||
return 1;
|
||||
}
|
||||
printf("gps nav context store tests passed\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,499 @@
|
||||
/*
|
||||
* PVT consistency under the things that actually happen on the board:
|
||||
* satellites appearing and disappearing mid-epoch, a 32-bit sample counter
|
||||
* that wraps every 262 seconds, coalesced interrupts, and a cold start whose
|
||||
* flash data is older than the receiver has any way to know.
|
||||
*
|
||||
* The question each test answers is not "does it compute a position" but "can
|
||||
* this produce a position that is wrong without saying so".
|
||||
*/
|
||||
|
||||
#include <assert.h>
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "gps/gps_ip_defines.h"
|
||||
#include "gps/gps_acq_manager.h"
|
||||
#include "gps/gps_almanac.h"
|
||||
#include "gps/gps_ephemeris_cache.h"
|
||||
#include "gps/gps_ephemeris_validity.h"
|
||||
#include "gps/gps_nav_epoch.h"
|
||||
#include "gps/gps_pps.h"
|
||||
#include "gps/gps_pps_time_filter.h"
|
||||
#include "gps/Navigation.h"
|
||||
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
/* The receiver's own constants, so the test cannot drift from the firmware. */
|
||||
#define SAMPLE_RATE_HZ GPS_RF_SAMPLE_RATE_HZ
|
||||
#define SAMPLES_PER_MS (SAMPLE_RATE_HZ / 1000.0)
|
||||
#define SYNC_WINDOW ((uint32_t)(SAMPLES_PER_MS * \
|
||||
(double)GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS))
|
||||
#define TOW_TOLERANCE GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC
|
||||
|
||||
/* ================================================================== */
|
||||
/* 1. Cold start: what does the acquisition engine actually search? */
|
||||
/* ================================================================== */
|
||||
|
||||
/*
|
||||
* At power-on there is no receiver time - it arrives only once a satellite is
|
||||
* tracked and its HOW decoded - so the almanac cannot be evaluated and the
|
||||
* visibility model must stand aside completely. Anything less would mean a
|
||||
* cold start searching a subset chosen by data it cannot yet validate.
|
||||
*/
|
||||
static void test_cold_start_searches_every_prn(void)
|
||||
{
|
||||
gps_acq_manager_t manager;
|
||||
gps_acq_manager_config_t config = {
|
||||
.retry_delay_ticks = GPS_ACQ_RETRY_DELAY_TICKS,
|
||||
.hot_retry_delay_ticks = GPS_ACQ_HOT_RETRY_DELAY_TICKS,
|
||||
.hot_window_ticks = GPS_ACQ_REACQ_PRIORITY_TICKS,
|
||||
.max_hot_streak = GPS_ACQ_MAX_HOT_STREAK,
|
||||
.hot_fast_attempts = GPS_ACQ_HOT_FAST_ATTEMPTS,
|
||||
.unlikely_retry_delay_ticks = GPS_ACQ_UNLIKELY_RETRY_DELAY_TICKS
|
||||
};
|
||||
gps_acq_manager_init(&manager, &config);
|
||||
CHECK(gps_acq_manager_search_mask(&manager) == 0xffffffffu,
|
||||
"acquisition did not start with every PRN searchable");
|
||||
|
||||
for(unsigned int prn = 0u; prn < GPS_ACQ_MANAGER_PRN_COUNT; prn++)
|
||||
{
|
||||
CHECK(gps_acq_manager_enable_cold(&manager, prn), "enable cold");
|
||||
}
|
||||
|
||||
/*
|
||||
* gps_refresh_acq_visibility() forces the mask wide whenever receiver time
|
||||
* is unknown. Reproduce that and confirm one full sweep really does reach
|
||||
* all 32 PRNs before any of them is retried.
|
||||
*/
|
||||
gps_acq_manager_set_search_mask(&manager, 0xffffffffu);
|
||||
uint32_t swept = 0u;
|
||||
uint64_t now = 1u;
|
||||
for(unsigned int i = 0u; i < GPS_ACQ_MANAGER_PRN_COUNT; i++)
|
||||
{
|
||||
unsigned int prn = 0u;
|
||||
bool hot = false;
|
||||
CHECK(gps_acq_manager_start_next(&manager, now, &prn, &hot),
|
||||
"cold start had nothing to search");
|
||||
CHECK((swept & (1u << prn)) == 0u,
|
||||
"a PRN was searched twice before the sweep completed");
|
||||
swept |= 1u << prn;
|
||||
gps_acq_manager_mark_failed(&manager, prn, now);
|
||||
now += GPS_ACQ_RETRY_DELAY_TICKS;
|
||||
}
|
||||
CHECK(swept == 0xffffffffu, "the cold-start sweep missed a PRN");
|
||||
printf(" cold start with no time: all 32 PRNs searched, none twice\n");
|
||||
|
||||
/*
|
||||
* And an empty visibility model - which is what a corrupt or absent flash
|
||||
* almanac produces - must degrade to the same blind sweep, never to
|
||||
* silence.
|
||||
*/
|
||||
gps_acq_manager_set_search_mask(&manager, 0u);
|
||||
CHECK(gps_acq_manager_search_mask(&manager) == 0xffffffffu,
|
||||
"an empty visibility mask silenced acquisition");
|
||||
printf(" empty visibility model degrades to a blind sweep\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* The other half of the cold-start question: a flash ephemeris is installed
|
||||
* while time is unknown, so what stops a months-old orbit reaching the
|
||||
* position solve?
|
||||
*
|
||||
* gps_collect_completed_epoch_locked() qualifies every channel with
|
||||
* time_known = true against the epoch's own transmit time, which comes from
|
||||
* the decoded HOW and not from anything persisted. So the age check is
|
||||
* unavoidable by the time a measurement can influence a fix, however the
|
||||
* ephemeris got into the cache.
|
||||
*/
|
||||
static void test_stale_ephemeris_cannot_reach_the_solve(void)
|
||||
{
|
||||
gps_ephemeris_t eph;
|
||||
memset(&eph, 0, sizeof(eph));
|
||||
eph.ephemeris_valid = true;
|
||||
eph.SVID = 5u;
|
||||
eph.iode = 0x30u;
|
||||
eph.iodc = 0x30u;
|
||||
eph.toe.sec = 100000.0;
|
||||
eph.toc.sec = 100000.0;
|
||||
eph.toe.week = 2431;
|
||||
eph.toe_week_valid = true;
|
||||
eph.A = 26560000.0;
|
||||
eph.sqrta = 5153.6;
|
||||
eph.ecc = 0.006;
|
||||
eph.sva = 2u;
|
||||
eph.fit_flag = false; /* nominal four hour curve fit */
|
||||
|
||||
/* While time is unknown only the structural rules can apply - this is the
|
||||
* cold-start window, and it is why the epoch gate exists. */
|
||||
CHECK(gps_ephemeris_check(&eph, 5u, false, 0, 0.0) == GPS_EPHEMERIS_OK,
|
||||
"structural check rejected a well-formed record");
|
||||
|
||||
/* Inside the fit interval: usable. */
|
||||
CHECK(gps_ephemeris_check(&eph, 5u, true, 2431, 103000.0) ==
|
||||
GPS_EPHEMERIS_OK,
|
||||
"a record 50 minutes old was rejected");
|
||||
|
||||
/* Past it: refused, whatever it claims about itself. */
|
||||
CHECK(gps_ephemeris_check(&eph, 5u, true, 2431, 100000.0 + 8000.0) ==
|
||||
GPS_EPHEMERIS_REJECT_AGE,
|
||||
"a record past its curve fit interval was accepted");
|
||||
/* A week later, expressed as a wrapped second-of-week - the shape a
|
||||
* flash record takes after a long power-off. */
|
||||
CHECK(gps_ephemeris_check(&eph, 5u, true, 2432, 100000.0) ==
|
||||
GPS_EPHEMERIS_REJECT_AGE,
|
||||
"a week-old record was accepted");
|
||||
printf(" restored ephemeris is age-checked before it can reach a fix\n");
|
||||
}
|
||||
|
||||
/* ================================================================== */
|
||||
/* 2. The 32-bit sample counter wraps every 262 s. */
|
||||
/* ================================================================== */
|
||||
|
||||
/*
|
||||
* Pseudoranges are differences of corrected sample counts against one
|
||||
* reference, scaled to metres. The counter is 32 bits at 16.368 MHz, so it
|
||||
* wraps mid-flight roughly every four and a half minutes. If the wrap leaked
|
||||
* into the differences it would put one satellite 2^31 samples - about 39
|
||||
* million kilometres - away from the rest, and the resulting position would be
|
||||
* wrong with a perfectly ordinary-looking residual on the other channels.
|
||||
*
|
||||
* Build one epoch twice, once away from the wrap and once straddling it, and
|
||||
* require the metre-domain pseudoranges to be identical.
|
||||
*/
|
||||
static void test_sample_wrap_does_not_disturb_pseudoranges(void)
|
||||
{
|
||||
/* Six channels spread over 20 ms of capture skew, with sub-sample code
|
||||
* phase corrections, which is the realistic shape of one epoch. */
|
||||
const int32_t offsets[6] = {0, -12345, 48000, -230000, 91000, 150000};
|
||||
const double code_phase[6] = {0.31, 0.77, 0.02, 0.55, 0.94, 0.18};
|
||||
|
||||
double away[6];
|
||||
double across[6];
|
||||
|
||||
for(unsigned int pass = 0u; pass < 2u; pass++)
|
||||
{
|
||||
/* Pass 0 sits mid-range; pass 1 puts the reference just past the
|
||||
* wrap so half the channels are still near 0xFFFFFFFF. */
|
||||
uint32_t base = (pass == 0u) ? 0x40000000u : 0x00000064u;
|
||||
uint32_t reference = base;
|
||||
double lifted[6];
|
||||
for(unsigned int i = 0u; i < 6u; i++)
|
||||
{
|
||||
uint32_t raw = (uint32_t)((int64_t)base + offsets[i]);
|
||||
double corrected = (double)raw - code_phase[i];
|
||||
lifted[i] = gps_nav_epoch_unwrap_sample(raw, corrected,
|
||||
reference);
|
||||
}
|
||||
/* Exactly what gps_ip.c forms: milliseconds relative to the
|
||||
* reference, then metres. */
|
||||
for(unsigned int i = 0u; i < 6u; i++)
|
||||
{
|
||||
double delta_ms = (lifted[i] / SAMPLES_PER_MS) -
|
||||
(lifted[0] / SAMPLES_PER_MS);
|
||||
double metres = delta_ms * (SPEED_OF_LIGHT / 1000.0);
|
||||
if(pass == 0u) { away[i] = metres; } else { across[i] = metres; }
|
||||
}
|
||||
}
|
||||
|
||||
double worst = 0.0;
|
||||
for(unsigned int i = 0u; i < 6u; i++)
|
||||
{
|
||||
double difference = fabs(away[i] - across[i]);
|
||||
if(difference > worst)
|
||||
{
|
||||
worst = difference;
|
||||
}
|
||||
}
|
||||
printf(" sample counter wrap: worst pseudorange difference %.9f m\n",
|
||||
worst);
|
||||
CHECK(worst < 1.0e-3, "the 32-bit sample wrap moved a pseudorange");
|
||||
|
||||
/* The epoch selector's own comparisons must agree across the wrap too,
|
||||
* otherwise a coherent epoch would be rejected once every 262 s. */
|
||||
/* Sized to the API's declared parameter, not to the count in use: the
|
||||
* signature promises GPS_NAV_EPOCH_MAX_CANDIDATES entries even though it
|
||||
* only reads candidate_count of them. */
|
||||
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES];
|
||||
memset(candidates, 0, sizeof(candidates));
|
||||
gps_nav_epoch_selection_t selection;
|
||||
for(unsigned int i = 0u; i < 6u; i++)
|
||||
{
|
||||
candidates[i].week = 2431;
|
||||
candidates[i].tow_sec = 300000.0;
|
||||
candidates[i].raw_sample =
|
||||
(uint32_t)((int64_t)0x00000064 + offsets[i]);
|
||||
}
|
||||
CHECK(gps_nav_epoch_validate_complete(candidates, 6u, SYNC_WINDOW,
|
||||
TOW_TOLERANCE, &selection),
|
||||
"a coherent epoch straddling the counter wrap was rejected");
|
||||
CHECK(selection.selected_count == 6u, "epoch lost a channel at the wrap");
|
||||
printf(" epoch selection is wrap-safe\n");
|
||||
}
|
||||
|
||||
/* ================================================================== */
|
||||
/* 3. Satellites connecting and disconnecting. */
|
||||
/* ================================================================== */
|
||||
|
||||
#define PERIOD_TICKS ((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_SOLVE_PERIOD_MS / 1000u)
|
||||
#define WINDOW_TICKS (((uint64_t)GPS_PPS_CLOCK_HZ * \
|
||||
GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS) / 1000u)
|
||||
#define HOLDOFF_TICKS ((uint64_t)1000000)
|
||||
|
||||
static gps_nav_epoch_event_result_t push(gps_nav_epoch_assembly_t* assembly,
|
||||
uint64_t counter,
|
||||
unsigned int channel,
|
||||
uint32_t ready_mask)
|
||||
{
|
||||
return gps_nav_epoch_assembly_push(assembly, counter, counter, channel,
|
||||
ready_mask, PERIOD_TICKS,
|
||||
WINDOW_TICKS, HOLDOFF_TICKS);
|
||||
}
|
||||
|
||||
/*
|
||||
* An epoch is an all-or-nothing hardware measurement set: the expected channel
|
||||
* set is frozen when the first event of a period arrives, so the satellite
|
||||
* geometry of a published epoch cannot depend on interrupt timing. These are
|
||||
* the transitions that have to hold that line.
|
||||
*/
|
||||
static void test_epoch_assembly_under_connect_and_disconnect(void)
|
||||
{
|
||||
gps_nav_epoch_assembly_t assembly;
|
||||
uint64_t counter = 1000000u;
|
||||
|
||||
/* --- a satellite acquired mid-epoch must wait for the next one --- */
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"first event rejected");
|
||||
CHECK(push(&assembly, counter + 10u, 1u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED, "second event rejected");
|
||||
/* Channel 4 has just locked, so the live ready mask has grown. */
|
||||
CHECK(push(&assembly, counter + 20u, 4u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_INVALID,
|
||||
"a channel that joined mid-epoch was admitted to it");
|
||||
CHECK(push(&assembly, counter + 30u, 2u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED, "third event rejected");
|
||||
CHECK(push(&assembly, counter + 40u, 3u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_COMPLETE,
|
||||
"the frozen expected set did not complete");
|
||||
printf(" a satellite acquired mid-epoch joins the next epoch, not this one\n");
|
||||
|
||||
/* --- a satellite lost mid-epoch must release the epoch --- */
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
CHECK(push(&assembly, counter, 0u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"push 0");
|
||||
CHECK(push(&assembly, counter + 5u, 1u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1");
|
||||
CHECK(push(&assembly, counter + 8u, 2u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 2");
|
||||
CHECK(push(&assembly, counter + 9u, 3u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 3");
|
||||
/* Channel 4 drops before it ever reports. Removing it must complete the
|
||||
* epoch from what has already arrived rather than stall until the
|
||||
* deadline. */
|
||||
CHECK(gps_nav_epoch_assembly_remove_channel(&assembly, 4u),
|
||||
"removing a lost channel did not complete the epoch");
|
||||
CHECK(gps_nav_epoch_assembly_usable(&assembly,
|
||||
GPS_NAV_MIN_WLS_SATELLITES),
|
||||
"four surviving channels were not usable");
|
||||
printf(" a satellite lost mid-epoch releases the epoch immediately\n");
|
||||
|
||||
/* --- losing one too many must not publish a short epoch --- */
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
CHECK(push(&assembly, counter, 0u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"push 0");
|
||||
CHECK(push(&assembly, counter + 5u, 1u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1");
|
||||
CHECK(push(&assembly, counter + 8u, 2u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 2");
|
||||
(void)gps_nav_epoch_assembly_remove_channel(&assembly, 3u);
|
||||
(void)gps_nav_epoch_assembly_remove_channel(&assembly, 4u);
|
||||
CHECK(!gps_nav_epoch_assembly_usable(&assembly,
|
||||
GPS_NAV_MIN_WLS_SATELLITES),
|
||||
"three channels were reported usable for a four-unknown solve");
|
||||
printf(" three surviving channels are never offered as a fix\n");
|
||||
|
||||
/* --- every satellite lost --- */
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
CHECK(push(&assembly, counter, 0u, 0x03u) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"push 0");
|
||||
CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 0u),
|
||||
"removing the only channel reported completion");
|
||||
CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 1u),
|
||||
"removing the last channel reported completion");
|
||||
CHECK(!gps_nav_epoch_assembly_usable(&assembly, 1u),
|
||||
"an emptied assembly was still usable");
|
||||
CHECK(!gps_nav_epoch_assembly_complete(&assembly),
|
||||
"an emptied assembly reported complete");
|
||||
printf(" total loss leaves no assembly behind\n");
|
||||
|
||||
/* --- the newest period supersedes an incomplete older one --- */
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"push 0");
|
||||
CHECK(push(&assembly, counter + 5u, 1u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1");
|
||||
CHECK(push(&assembly, counter + PERIOD_TICKS, 2u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_RESTARTED,
|
||||
"an event from the next period did not restart the epoch");
|
||||
printf(" a new capture period discards the incomplete previous one\n");
|
||||
|
||||
/* --- a straggler from the previous period is not back-filled --- */
|
||||
CHECK(push(&assembly, counter, 3u, 0x0fu) == GPS_NAV_EPOCH_EVENT_STALE,
|
||||
"an event from the previous period was folded into the new epoch");
|
||||
printf(" a late event from the previous period is dropped, not mixed in\n");
|
||||
|
||||
/* --- a coalesced repeat must not be counted twice --- */
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"push 0");
|
||||
CHECK(push(&assembly, counter + 1u, 0u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_DUPLICATE,
|
||||
"a repeated event for one channel was accepted twice");
|
||||
printf(" a coalesced repeat is rejected as a duplicate\n");
|
||||
|
||||
/* --- an event neither in this epoch nor the next is an outlier --- */
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"push 0");
|
||||
CHECK(push(&assembly, counter + WINDOW_TICKS + 1u, 1u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_OUTLIER,
|
||||
"an event outside the coherence window was accepted");
|
||||
printf(" an out-of-window capture is refused, not averaged in\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* Coalescing: the capture mailbox holds only the newest measurement, so when
|
||||
* interrupts are late the receiver has to know how many epochs it skipped and
|
||||
* must never invent the missing ones.
|
||||
*/
|
||||
static void test_coalesced_captures_are_counted_not_invented(void)
|
||||
{
|
||||
uint32_t gap = 0u;
|
||||
CHECK(gps_nav_capture_sequence_consistent(1000u, 5u,
|
||||
1000u + PERIOD_TICKS, 6u,
|
||||
PERIOD_TICKS, &gap),
|
||||
"a single clean epoch was rejected");
|
||||
CHECK(gap == 1u, "single epoch reported a gap");
|
||||
|
||||
CHECK(gps_nav_capture_sequence_consistent(1000u, 5u,
|
||||
1000u + (3u * PERIOD_TICKS), 8u,
|
||||
PERIOD_TICKS, &gap),
|
||||
"three coalesced epochs were rejected");
|
||||
CHECK(gap == 3u, "coalesced gap miscounted");
|
||||
|
||||
/* Counter and sequence disagreeing means the mailbox cannot be trusted. */
|
||||
CHECK(!gps_nav_capture_sequence_consistent(1000u, 5u,
|
||||
1000u + (3u * PERIOD_TICKS), 6u,
|
||||
PERIOD_TICKS, &gap),
|
||||
"a sequence/counter mismatch was accepted");
|
||||
CHECK(!gps_nav_capture_sequence_consistent(1000u, 5u, 1000u, 6u,
|
||||
PERIOD_TICKS, &gap),
|
||||
"a counter that did not advance was accepted");
|
||||
CHECK(!gps_nav_capture_sequence_consistent(0u, 5u,
|
||||
1000u + PERIOD_TICKS, 6u,
|
||||
PERIOD_TICKS, &gap),
|
||||
"an unseeded previous counter was accepted");
|
||||
printf(" coalesced captures are counted exactly, never synthesised\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* Publication is stricter than clustering: every channel in the frozen ready
|
||||
* mask must belong to one coherent epoch, or nothing is published. A partial
|
||||
* cluster would change the satellite geometry according to interrupt timing,
|
||||
* which is exactly what produced large raw-position jumps before.
|
||||
*/
|
||||
static void test_partial_coherence_publishes_nothing(void)
|
||||
{
|
||||
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES];
|
||||
memset(candidates, 0, sizeof(candidates));
|
||||
gps_nav_epoch_selection_t selection;
|
||||
for(unsigned int i = 0u; i < 5u; i++)
|
||||
{
|
||||
candidates[i].week = 2431;
|
||||
candidates[i].tow_sec = 300000.0;
|
||||
candidates[i].raw_sample = 1000000u + (i * 100u);
|
||||
}
|
||||
CHECK(gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW,
|
||||
TOW_TOLERANCE, &selection),
|
||||
"a fully coherent set was rejected");
|
||||
|
||||
/* One channel a full nav period away - a stale measurement that slipped
|
||||
* through. Four good ones remain, which is enough for a solve, and that
|
||||
* is exactly why it must not be published as one. */
|
||||
candidates[4].raw_sample = 1000000u + (uint32_t)(SAMPLES_PER_MS * 200.0);
|
||||
CHECK(!gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW,
|
||||
TOW_TOLERANCE, &selection),
|
||||
"an incoherent epoch was published because a subset was solvable");
|
||||
CHECK(selection.selected_count == 4u,
|
||||
"the diagnostic cluster did not report the coherent subset");
|
||||
|
||||
/* A TOW disagreement is equally disqualifying even when the samples line
|
||||
* up: it means two satellites disagree about what time it is. */
|
||||
candidates[4].raw_sample = 1000000u + 400u;
|
||||
candidates[4].tow_sec = 300000.0 + (10.0 * TOW_TOLERANCE);
|
||||
CHECK(!gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW,
|
||||
TOW_TOLERANCE, &selection),
|
||||
"channels disagreeing about TOW were published as one epoch");
|
||||
printf(" an incoherent epoch publishes nothing, not its solvable subset\n");
|
||||
}
|
||||
|
||||
/*
|
||||
* A non-finite transmit time must never reach the pseudorange arithmetic.
|
||||
* gps_pps_select_reference() is the last gate before it, and it screens the
|
||||
* whole set - not just the entries it happens to walk past before finding the
|
||||
* preferred reference satellite.
|
||||
*/
|
||||
static void test_reference_selection_screens_every_capture(void)
|
||||
{
|
||||
unsigned int ids[4] = {3u, 7u, 11u, 19u};
|
||||
double times[4] = {10.0, 11.0, 12.0, 13.0};
|
||||
unsigned int reference = 99u;
|
||||
|
||||
CHECK(gps_pps_select_reference(ids, times, 4u, true, 7u, &reference),
|
||||
"a clean set was rejected");
|
||||
CHECK(reference == 1u, "the preferred reference satellite was not chosen");
|
||||
|
||||
CHECK(gps_pps_select_reference(ids, times, 4u, false, 0u, &reference),
|
||||
"a clean set without a preference was rejected");
|
||||
CHECK(reference == 0u, "the earliest capture was not chosen");
|
||||
|
||||
/* The corrupt entry sits *after* the preferred satellite, which is the
|
||||
* ordering an early return would walk straight past. */
|
||||
times[3] = NAN;
|
||||
CHECK(!gps_pps_select_reference(ids, times, 4u, true, 7u, &reference),
|
||||
"a non-finite capture time downstream of the preferred reference "
|
||||
"was not detected");
|
||||
CHECK(!gps_pps_select_reference(ids, times, 4u, false, 0u, &reference),
|
||||
"a non-finite capture time was not detected without a preference");
|
||||
printf(" reference selection screens the whole capture set\n");
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
test_cold_start_searches_every_prn();
|
||||
test_stale_ephemeris_cannot_reach_the_solve();
|
||||
test_sample_wrap_does_not_disturb_pseudoranges();
|
||||
test_epoch_assembly_under_connect_and_disconnect();
|
||||
test_coalesced_captures_are_counted_not_invented();
|
||||
test_partial_coherence_publishes_nothing();
|
||||
test_reference_selection_screens_every_capture();
|
||||
|
||||
if(failures != 0)
|
||||
{
|
||||
fprintf(stderr, "%d check(s) failed\n", failures);
|
||||
return 1;
|
||||
}
|
||||
printf("gps pvt consistency tests passed\n");
|
||||
return 0;
|
||||
}
|
||||
@@ -183,8 +183,225 @@ static void fractional_code_phase_corrects_capture_counter(void)
|
||||
"capture correction beyond llround range was accepted");
|
||||
}
|
||||
|
||||
#define PPS_HZ 125000000.0
|
||||
#define BIT_PERIOD_SEC 0.020
|
||||
#define NAV_TICKS 10u
|
||||
#define GUARD_SEC 0.004
|
||||
#define SNAP_SEC 0.050
|
||||
|
||||
/*
|
||||
* These exercise the two functions the firmware actually calls when a returning
|
||||
* PRN is warm-seeded: the phase handed to NAV_SOLVE_PHASE_SEED, and the snap
|
||||
* that turns the first seeded epoch into a time anchor.
|
||||
*
|
||||
* Contract under test (verified against the RTL in
|
||||
* hardware/external/gps/rtl/gps_timer/tb/nav_solve_timer_seed_tb.sv): the seed
|
||||
* is the counter value in effect for the NEXT navigation bit, and an epoch is
|
||||
* raised on absolute bit index b with (b + 1) % NAV_TICKS == 0.
|
||||
*/
|
||||
|
||||
/* Place the receiver a known fraction into a navigation bit so the guard band
|
||||
* is not what is being measured. */
|
||||
static double elapsed_for_outage(double outage_sec, double into_bit_sec)
|
||||
{
|
||||
double bits = floor(outage_sec / BIT_PERIOD_SEC);
|
||||
return (bits * BIT_PERIOD_SEC) + into_bit_sec;
|
||||
}
|
||||
|
||||
static void phase_seed_matches_the_global_bit_grid(void)
|
||||
{
|
||||
const double anchor_tow = 469362.0; /* a HOW boundary: multiple of 6 s */
|
||||
const double outages[] = {0.020, 0.100, 0.200, 1.0, 5.0, 30.0, 60.0,
|
||||
300.0, 900.0};
|
||||
for(unsigned int i = 0u;
|
||||
i < (sizeof(outages) / sizeof(outages[0])); i++)
|
||||
{
|
||||
uint32_t phase = 0u;
|
||||
double elapsed = elapsed_for_outage(outages[i], BIT_PERIOD_SEC / 2.0);
|
||||
if(!gps_nav_bit_phase_from_elapsed(anchor_tow, elapsed,
|
||||
BIT_PERIOD_SEC, NAV_TICKS,
|
||||
GUARD_SEC, &phase))
|
||||
{
|
||||
fprintf(stderr, "FAIL: seed refused for a %.3f s outage\n",
|
||||
outages[i]);
|
||||
failures++;
|
||||
continue;
|
||||
}
|
||||
/* The next bit after the one the receiver is currently inside. */
|
||||
double predicted = anchor_tow + elapsed;
|
||||
uint64_t next_index =
|
||||
(uint64_t)floor(predicted / BIT_PERIOD_SEC) + 1u;
|
||||
CHECK(phase == (uint32_t)(next_index % NAV_TICKS),
|
||||
"seed phase does not match the global navigation-bit grid");
|
||||
/* Applying that phase, the hardware counts up to NAV_TICKS-1; the
|
||||
* epoch therefore lands where the uninterrupted grid would have. */
|
||||
uint64_t epoch_index = next_index + (NAV_TICKS - 1u - phase);
|
||||
CHECK(((epoch_index + 1u) % NAV_TICKS) == 0u,
|
||||
"seeded epoch would land off the global grid");
|
||||
}
|
||||
}
|
||||
|
||||
static void phase_seed_guard_band_rejects_bit_boundaries(void)
|
||||
{
|
||||
const double anchor_tow = 469362.0;
|
||||
uint32_t phase = 0u;
|
||||
/* Just after a boundary. */
|
||||
CHECK(!gps_nav_bit_phase_from_elapsed(
|
||||
anchor_tow, elapsed_for_outage(10.0, GUARD_SEC / 2.0),
|
||||
BIT_PERIOD_SEC, NAV_TICKS, GUARD_SEC, &phase),
|
||||
"arming just after a navigation-bit boundary was allowed");
|
||||
/* Just before the next one. */
|
||||
CHECK(!gps_nav_bit_phase_from_elapsed(
|
||||
anchor_tow,
|
||||
elapsed_for_outage(10.0, BIT_PERIOD_SEC - (GUARD_SEC / 2.0)),
|
||||
BIT_PERIOD_SEC, NAV_TICKS, GUARD_SEC, &phase),
|
||||
"arming just before a navigation-bit boundary was allowed");
|
||||
/* Comfortably inside is fine. */
|
||||
CHECK(gps_nav_bit_phase_from_elapsed(
|
||||
anchor_tow, elapsed_for_outage(10.0, BIT_PERIOD_SEC / 2.0),
|
||||
BIT_PERIOD_SEC, NAV_TICKS, GUARD_SEC, &phase),
|
||||
"arming mid-bit was refused");
|
||||
}
|
||||
|
||||
static void phase_seed_is_always_reduced(void)
|
||||
{
|
||||
/* The tracking IP refuses an unreduced seed, so software must never emit
|
||||
* one for any outage length or anchor. */
|
||||
const double anchors[] = {0.0, 6.0, 302400.0, 604794.0};
|
||||
for(unsigned int a = 0u; a < (sizeof(anchors) / sizeof(anchors[0])); a++)
|
||||
{
|
||||
for(double outage = 0.02; outage < 900.0; outage *= 1.7)
|
||||
{
|
||||
uint32_t phase = 0u;
|
||||
double elapsed =
|
||||
elapsed_for_outage(outage, BIT_PERIOD_SEC / 2.0);
|
||||
if(gps_nav_bit_phase_from_elapsed(anchors[a], elapsed,
|
||||
BIT_PERIOD_SEC, NAV_TICKS,
|
||||
GUARD_SEC, &phase))
|
||||
{
|
||||
CHECK(phase < NAV_TICKS, "emitted seed is not reduced");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void phase_seed_rejects_invalid_requests(void)
|
||||
{
|
||||
uint32_t phase = 0u;
|
||||
CHECK(!gps_nav_bit_phase_from_elapsed(469362.0, -1.0, BIT_PERIOD_SEC,
|
||||
NAV_TICKS, GUARD_SEC, &phase),
|
||||
"negative elapsed time was accepted");
|
||||
CHECK(!gps_nav_bit_phase_from_elapsed(469362.0, 10.0, BIT_PERIOD_SEC,
|
||||
0u, GUARD_SEC, &phase),
|
||||
"a zero nav-solve multiplier was accepted");
|
||||
CHECK(!gps_nav_bit_phase_from_elapsed(-1.0, 10.0, BIT_PERIOD_SEC,
|
||||
NAV_TICKS, GUARD_SEC, &phase),
|
||||
"a negative anchor time of week was accepted");
|
||||
CHECK(!gps_nav_bit_phase_from_elapsed(469362.0, 10.0, BIT_PERIOD_SEC,
|
||||
NAV_TICKS, BIT_PERIOD_SEC, &phase),
|
||||
"a guard band wider than half a bit was accepted");
|
||||
}
|
||||
|
||||
static void epoch_snap_recovers_transmit_time(void)
|
||||
{
|
||||
const double period = (double)NAV_TICKS * BIT_PERIOD_SEC; /* 0.2 s */
|
||||
const double anchor_tow = 469362.0;
|
||||
double snapped = 0.0;
|
||||
|
||||
/* An exact multiple of the epoch period comes back unchanged. */
|
||||
CHECK(gps_nav_epoch_snap_from_elapsed(anchor_tow, 60.0, period,
|
||||
SNAP_SEC, &snapped),
|
||||
"an exact epoch was refused");
|
||||
CHECK(fabs(snapped - (anchor_tow + 60.0)) < 1e-9,
|
||||
"an exact epoch was moved by the snap");
|
||||
|
||||
/* Prediction error inside the tolerance is removed entirely: this is what
|
||||
* turns a drifted prediction back into exact GPS time. */
|
||||
CHECK(gps_nav_epoch_snap_from_elapsed(anchor_tow, 60.0 + 0.03, period,
|
||||
SNAP_SEC, &snapped),
|
||||
"a prediction inside the snap tolerance was refused");
|
||||
CHECK(fabs(snapped - (anchor_tow + 60.0)) < 1e-9,
|
||||
"the snap did not remove the prediction error");
|
||||
CHECK(gps_nav_epoch_snap_from_elapsed(anchor_tow, 60.0 - 0.03, period,
|
||||
SNAP_SEC, &snapped),
|
||||
"a negative prediction error inside tolerance was refused");
|
||||
CHECK(fabs(snapped - (anchor_tow + 60.0)) < 1e-9,
|
||||
"the snap did not remove a negative prediction error");
|
||||
|
||||
/* Beyond the tolerance the context is declared stale rather than snapped
|
||||
* to the wrong epoch. */
|
||||
CHECK(!gps_nav_epoch_snap_from_elapsed(anchor_tow, 60.0 + 0.08, period,
|
||||
SNAP_SEC, &snapped),
|
||||
"a prediction outside the snap tolerance was accepted");
|
||||
|
||||
/* The result may cross a week boundary; the caller normalises. */
|
||||
CHECK(gps_nav_epoch_snap_from_elapsed(604799.8, 0.4, period,
|
||||
SNAP_SEC, &snapped),
|
||||
"an epoch crossing the week boundary was refused");
|
||||
CHECK(snapped > 604800.0,
|
||||
"week crossing was silently wrapped inside the snap");
|
||||
|
||||
CHECK(!gps_nav_epoch_snap_from_elapsed(anchor_tow, 60.0, period,
|
||||
period, &snapped),
|
||||
"a snap tolerance of half a period or more was accepted");
|
||||
}
|
||||
|
||||
/*
|
||||
* Receiver-clock drift is what ultimately bounds warm-seed reuse. The seed
|
||||
* only needs to land in the right 20 ms bit, and the epoch snap only needs to
|
||||
* land in the right 200 ms epoch, so both tolerate far more drift than the
|
||||
* ephemeris fit interval allows.
|
||||
*/
|
||||
static void drift_bounds_are_where_they_are_claimed(void)
|
||||
{
|
||||
const double period = (double)NAV_TICKS * BIT_PERIOD_SEC;
|
||||
const double anchor_tow = 400000.0;
|
||||
const double outage = 900.0; /* the configured reuse limit */
|
||||
uint32_t phase = 0u;
|
||||
double snapped = 0.0;
|
||||
|
||||
/* At 2 ppm over 15 minutes the error is 1.8 ms: both stages cope. */
|
||||
double drift_2ppm = outage * 2e-6;
|
||||
CHECK(gps_nav_epoch_snap_from_elapsed(anchor_tow, outage + drift_2ppm,
|
||||
period, SNAP_SEC, &snapped),
|
||||
"2 ppm over 15 minutes broke the epoch snap");
|
||||
CHECK(fabs(snapped - (anchor_tow + outage)) < 1e-9,
|
||||
"2 ppm drift was not removed by the snap");
|
||||
|
||||
/* The snap fails once drift exceeds half an epoch period; at 15 minutes
|
||||
* that needs well over 50 ppm, far beyond any plausible oscillator. */
|
||||
double drift_100ppm = outage * 100e-6; /* 90 ms */
|
||||
CHECK(!gps_nav_epoch_snap_from_elapsed(anchor_tow, outage + drift_100ppm,
|
||||
period, SNAP_SEC, &snapped),
|
||||
"100 ppm over 15 minutes was accepted by the epoch snap");
|
||||
|
||||
/* The phase seed is the tighter of the two: it must land in the correct
|
||||
* 20 ms bit, so a drift of one whole bit shifts the grid. */
|
||||
double elapsed_ok = elapsed_for_outage(outage, BIT_PERIOD_SEC / 2.0);
|
||||
CHECK(gps_nav_bit_phase_from_elapsed(anchor_tow, elapsed_ok,
|
||||
BIT_PERIOD_SEC, NAV_TICKS,
|
||||
GUARD_SEC, &phase),
|
||||
"the nominal 15 minute seed was refused");
|
||||
uint32_t shifted = 0u;
|
||||
CHECK(gps_nav_bit_phase_from_elapsed(anchor_tow,
|
||||
elapsed_ok + BIT_PERIOD_SEC,
|
||||
BIT_PERIOD_SEC, NAV_TICKS,
|
||||
GUARD_SEC, &shifted),
|
||||
"a one-bit shifted seed was refused");
|
||||
CHECK(shifted == ((phase + 1u) % NAV_TICKS),
|
||||
"a one-bit prediction error does not shift the phase by one");
|
||||
/* That is the failure this cannot detect on its own, which is why the
|
||||
* epoch snap and the hardware's subframe override both exist. */
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
phase_seed_matches_the_global_bit_grid();
|
||||
phase_seed_guard_band_rejects_bit_boundaries();
|
||||
phase_seed_is_always_reduced();
|
||||
phase_seed_rejects_invalid_requests();
|
||||
epoch_snap_recovers_transmit_time();
|
||||
drift_bounds_are_where_they_are_claimed();
|
||||
normal_and_skipped_subframes_are_consistent();
|
||||
week_rollover_is_consistent();
|
||||
temporary_week_can_upgrade_on_first_subframe_one();
|
||||
|
||||
@@ -0,0 +1,736 @@
|
||||
/*
|
||||
* Host model of the warm/hot reacquisition path.
|
||||
*
|
||||
* This closes the gap between the two verification layers that already exist:
|
||||
* Questa proves the RTL honours the seed contract, and the unit tests prove the
|
||||
* firmware maths is self-consistent. Neither shows that the two AGREE. Here a
|
||||
* behavioural model of nav_solve_timer.sv is driven by the real firmware
|
||||
* functions (gps_time_anchor.c, gps_nav_epoch.c), so a disagreement about the
|
||||
* epoch grid fails the build rather than the board.
|
||||
*
|
||||
* The timer model is written from the RTL contract, not copied from the
|
||||
* firmware, so it is an independent check. Every behaviour it implements is
|
||||
* covered by a matching case in
|
||||
* hardware/external/gps/rtl/gps_timer/tb/nav_solve_timer_seed_tb.sv.
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "gps/gps_nav_epoch.h"
|
||||
#include "gps/gps_time_anchor.h"
|
||||
|
||||
#define NAV_TICKS 10u /* the configured multiplier */
|
||||
#define BIT_PERIOD_SEC 0.020
|
||||
#define EPOCH_PERIOD_SEC ((double)NAV_TICKS * BIT_PERIOD_SEC) /* 200 ms */
|
||||
#define GUARD_SEC 0.004
|
||||
#define SNAP_SEC 0.050
|
||||
#define PPS_HZ 125000000.0
|
||||
#define MAX_REUSE_SEC 900.0
|
||||
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
/* ------------------------------------------------------------------ */
|
||||
/* Behavioural model of nav_solve_timer.sv */
|
||||
/* ------------------------------------------------------------------ */
|
||||
|
||||
typedef enum { NST_IDLE = 0, NST_COUNT } nst_state_t;
|
||||
|
||||
typedef struct {
|
||||
nst_state_t state;
|
||||
uint32_t counter;
|
||||
uint32_t max;
|
||||
bool provisional;
|
||||
bool enable;
|
||||
bool busy;
|
||||
} nst_model_t;
|
||||
|
||||
static void nst_init(nst_model_t* m, uint32_t max)
|
||||
{
|
||||
memset(m, 0, sizeof(*m));
|
||||
m->max = max;
|
||||
}
|
||||
|
||||
static void nst_set_busy(nst_model_t* m, bool busy)
|
||||
{
|
||||
m->busy = busy;
|
||||
if(!busy)
|
||||
{
|
||||
/* trck_busy deasserting returns the timer to IDLE and clears the
|
||||
* provisional flag. */
|
||||
m->state = NST_IDLE;
|
||||
m->counter = 0u;
|
||||
m->provisional = false;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* A decoded subframe always wins: it aligns from IDLE, and it also re-aligns a
|
||||
* channel that is counting on an unconfirmed seed. z_count is the HOW Z-count,
|
||||
* so the first bit after the boundary has absolute index z_count * 300.
|
||||
*/
|
||||
static void nst_subframe(nst_model_t* m, uint64_t z_count)
|
||||
{
|
||||
if(!m->busy || !m->enable || (m->max == 0u))
|
||||
{
|
||||
return;
|
||||
}
|
||||
if((m->state == NST_IDLE) || m->provisional)
|
||||
{
|
||||
m->counter = (uint32_t)((z_count * 300u) % m->max);
|
||||
m->provisional = false;
|
||||
m->state = NST_COUNT;
|
||||
}
|
||||
}
|
||||
|
||||
/* Returns true when the seed was taken. The arm is always consumed. */
|
||||
static bool nst_seed(nst_model_t* m, uint32_t phase)
|
||||
{
|
||||
if((m->state != NST_IDLE) || !m->busy || !m->enable ||
|
||||
(m->max == 0u) || (phase >= m->max))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
m->counter = phase;
|
||||
m->provisional = true;
|
||||
m->state = NST_COUNT;
|
||||
return true;
|
||||
}
|
||||
|
||||
/* One navigation bit. Returns true when this bit raises a nav-solve epoch. */
|
||||
static bool nst_bit(nst_model_t* m)
|
||||
{
|
||||
if((m->state != NST_COUNT) || !m->enable || !m->busy)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
bool epoch = (m->counter == (m->max - 1u));
|
||||
m->counter = epoch ? 0u : (m->counter + 1u);
|
||||
return epoch;
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------ */
|
||||
/* Receiver model */
|
||||
/* ------------------------------------------------------------------ */
|
||||
|
||||
typedef struct {
|
||||
double tow_sec; /* transmit time of the last confirmed epoch */
|
||||
uint64_t pps_counter; /* PPS capture at that epoch */
|
||||
bool valid;
|
||||
} prn_context_t;
|
||||
|
||||
/* Absolute navigation-bit index of an exact bit boundary. */
|
||||
static uint64_t bit_index_of(double tow_sec)
|
||||
{
|
||||
return (uint64_t)llround(tow_sec / BIT_PERIOD_SEC);
|
||||
}
|
||||
|
||||
/* Index of the bit the receiver is currently inside, for an arbitrary time. */
|
||||
static uint64_t current_bit_index(double tow_sec)
|
||||
{
|
||||
return (uint64_t)floor(tow_sec / BIT_PERIOD_SEC);
|
||||
}
|
||||
|
||||
/* The grid the receiver must reproduce: epochs on bits where (b+1)%N == 0. */
|
||||
static bool is_epoch_bit(uint64_t bit_index)
|
||||
{
|
||||
return ((bit_index + 1u) % NAV_TICKS) == 0u;
|
||||
}
|
||||
|
||||
/*
|
||||
* A stored PRN context always holds the transmit time of a previous nav-solve
|
||||
* epoch, so it necessarily sits on the epoch grid. That matters: epoch times
|
||||
* are NOT multiples of the epoch period in time of week (with N=10 they fall at
|
||||
* 0.18 s modulo 0.2 s), so anything that recovers an epoch time has to work in
|
||||
* elapsed time relative to another epoch, never in absolute time of week.
|
||||
*/
|
||||
static double epoch_tow_at_or_below(double tow_sec)
|
||||
{
|
||||
uint64_t b = current_bit_index(tow_sec);
|
||||
while(!is_epoch_bit(b))
|
||||
{
|
||||
b--;
|
||||
}
|
||||
return (double)b * BIT_PERIOD_SEC;
|
||||
}
|
||||
|
||||
static uint64_t pps_at(double tow_sec, double tow_origin,
|
||||
uint64_t pps_origin, double clock_ppm)
|
||||
{
|
||||
double elapsed = (tow_sec - tow_origin) * (1.0 + (clock_ppm * 1e-6));
|
||||
return pps_origin + (uint64_t)llround(elapsed * PPS_HZ);
|
||||
}
|
||||
|
||||
/*
|
||||
* Drive one reacquisition: the PRN dropped after an epoch at context->tow_sec,
|
||||
* came back outage_sec later, and firmware seeds the timer. Returns the number
|
||||
* of bits fed after the seed, and asserts that every epoch raised lands on the
|
||||
* uninterrupted global grid.
|
||||
*/
|
||||
static void run_reacquisition(double outage_sec,
|
||||
double clock_ppm,
|
||||
double into_bit_frac,
|
||||
bool expect_seeded)
|
||||
{
|
||||
/* The context is a previous nav-solve epoch, not an arbitrary time. */
|
||||
const double anchor_tow = epoch_tow_at_or_below(469362.0);
|
||||
const uint64_t anchor_pps = 1000000000ull;
|
||||
prn_context_t context = {
|
||||
.tow_sec = anchor_tow, .pps_counter = anchor_pps, .valid = true
|
||||
};
|
||||
|
||||
nst_model_t timer;
|
||||
nst_init(&timer, NAV_TICKS);
|
||||
timer.enable = true;
|
||||
nst_set_busy(&timer, true);
|
||||
|
||||
/* Where the receiver actually is when firmware gets round to arming. */
|
||||
double bits = floor(outage_sec / BIT_PERIOD_SEC);
|
||||
double true_now_tow =
|
||||
anchor_tow + (bits * BIT_PERIOD_SEC) +
|
||||
(into_bit_frac * BIT_PERIOD_SEC);
|
||||
uint64_t now_pps =
|
||||
pps_at(true_now_tow, anchor_tow, anchor_pps, clock_ppm);
|
||||
|
||||
/* Firmware: elapsed from the PPS domain, then the seed. */
|
||||
double elapsed_sec =
|
||||
(double)(now_pps - context.pps_counter) / PPS_HZ;
|
||||
uint32_t phase = 0u;
|
||||
bool armed = gps_nav_bit_phase_from_elapsed(
|
||||
context.tow_sec, elapsed_sec, BIT_PERIOD_SEC, NAV_TICKS,
|
||||
GUARD_SEC, &phase);
|
||||
|
||||
if(!expect_seeded)
|
||||
{
|
||||
CHECK(!armed, "a seed was armed where the guard band should refuse");
|
||||
return;
|
||||
}
|
||||
CHECK(armed, "firmware refused to arm a seed it should have accepted");
|
||||
if(!armed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
CHECK(phase < NAV_TICKS, "firmware emitted an unreduced seed");
|
||||
CHECK(nst_seed(&timer, phase), "the tracking IP refused a valid seed");
|
||||
|
||||
/* Feed real navigation bits and check every epoch against the grid. */
|
||||
uint64_t next_bit = current_bit_index(true_now_tow) + 1u;
|
||||
unsigned int epochs = 0u;
|
||||
bool first_epoch_seen = false;
|
||||
double first_epoch_tow = 0.0;
|
||||
for(unsigned int i = 0u; i < (4u * NAV_TICKS); i++)
|
||||
{
|
||||
uint64_t b = next_bit + i;
|
||||
if(nst_bit(&timer))
|
||||
{
|
||||
epochs++;
|
||||
CHECK(is_epoch_bit(b),
|
||||
"a seeded epoch landed off the global navigation-bit grid");
|
||||
if(!first_epoch_seen)
|
||||
{
|
||||
first_epoch_seen = true;
|
||||
first_epoch_tow = (double)b * BIT_PERIOD_SEC;
|
||||
}
|
||||
}
|
||||
}
|
||||
CHECK(epochs >= 3u, "seeded channel did not produce periodic epochs");
|
||||
CHECK(first_epoch_seen, "seeded channel produced no epoch at all");
|
||||
|
||||
/* Firmware then turns that first epoch into a time anchor via the snap. */
|
||||
if(first_epoch_seen)
|
||||
{
|
||||
uint64_t epoch_pps =
|
||||
pps_at(first_epoch_tow, anchor_tow, anchor_pps, clock_ppm);
|
||||
double epoch_elapsed =
|
||||
(double)(epoch_pps - context.pps_counter) / PPS_HZ;
|
||||
double snapped = 0.0;
|
||||
CHECK(gps_nav_epoch_snap_from_elapsed(
|
||||
context.tow_sec, epoch_elapsed, EPOCH_PERIOD_SEC,
|
||||
SNAP_SEC, &snapped),
|
||||
"the first seeded epoch failed the anchor snap");
|
||||
CHECK(fabs(snapped - first_epoch_tow) < 1e-6,
|
||||
"the anchor snap recovered the wrong transmit time");
|
||||
}
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------------ */
|
||||
|
||||
static void outage_matrix_rejoins_on_the_correct_grid(void)
|
||||
{
|
||||
const double outages[] = {0.020, 0.100, 0.200, 1.0, 5.0, 30.0, 60.0,
|
||||
300.0, 900.0};
|
||||
for(unsigned int i = 0u;
|
||||
i < (sizeof(outages) / sizeof(outages[0])); i++)
|
||||
{
|
||||
run_reacquisition(outages[i], 0.0, 0.5, true);
|
||||
/* A realistic oscillator must not change the outcome. */
|
||||
run_reacquisition(outages[i], 2.0, 0.5, true);
|
||||
run_reacquisition(outages[i], -2.0, 0.5, true);
|
||||
}
|
||||
}
|
||||
|
||||
static void guard_band_refuses_arming_near_a_bit_edge(void)
|
||||
{
|
||||
/* Just inside either edge of the 20 ms bit: firmware must decline and
|
||||
* retry rather than risk a one-bit grid shift. */
|
||||
run_reacquisition(30.0, 0.0, 0.05, false);
|
||||
run_reacquisition(30.0, 0.0, 0.95, false);
|
||||
/* Mid-bit is fine. */
|
||||
run_reacquisition(30.0, 0.0, 0.50, true);
|
||||
}
|
||||
|
||||
static void seeded_grid_equals_the_uninterrupted_grid(void)
|
||||
{
|
||||
/* Run a channel that never dropped and one that dropped and was seeded,
|
||||
* and require identical epoch bits. */
|
||||
const double anchor_tow = epoch_tow_at_or_below(469362.0);
|
||||
nst_model_t reference;
|
||||
nst_init(&reference, NAV_TICKS);
|
||||
reference.enable = true;
|
||||
nst_set_busy(&reference, true);
|
||||
uint64_t z = (uint64_t)(bit_index_of(anchor_tow) / 300u);
|
||||
nst_subframe(&reference, z);
|
||||
uint64_t reference_first_bit = z * 300u;
|
||||
|
||||
nst_model_t seeded;
|
||||
nst_init(&seeded, NAV_TICKS);
|
||||
seeded.enable = true;
|
||||
nst_set_busy(&seeded, true);
|
||||
uint32_t phase = (uint32_t)(reference_first_bit % NAV_TICKS);
|
||||
CHECK(nst_seed(&seeded, phase), "seed refused for the comparison run");
|
||||
|
||||
for(unsigned int i = 0u; i < (5u * NAV_TICKS); i++)
|
||||
{
|
||||
bool a = nst_bit(&reference);
|
||||
bool b = nst_bit(&seeded);
|
||||
CHECK(a == b,
|
||||
"seeded channel epochs diverged from the subframe-aligned channel");
|
||||
if(a)
|
||||
{
|
||||
CHECK(is_epoch_bit(reference_first_bit + i),
|
||||
"reference channel epoch is off the global grid");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void a_wrong_seed_is_corrected_by_the_first_subframe(void)
|
||||
{
|
||||
const double anchor_tow = epoch_tow_at_or_below(469362.0);
|
||||
uint64_t z = (uint64_t)(bit_index_of(anchor_tow) / 300u);
|
||||
uint64_t first_bit = z * 300u;
|
||||
|
||||
nst_model_t timer;
|
||||
nst_init(&timer, NAV_TICKS);
|
||||
timer.enable = true;
|
||||
nst_set_busy(&timer, true);
|
||||
|
||||
/* Seed deliberately three bits wrong, as a stale context would. */
|
||||
uint32_t wrong = (uint32_t)((first_bit + 3u) % NAV_TICKS);
|
||||
CHECK(nst_seed(&timer, wrong), "wrong seed refused");
|
||||
CHECK(timer.provisional, "seeded channel is not marked provisional");
|
||||
|
||||
unsigned int off_grid = 0u;
|
||||
for(unsigned int i = 0u; i < NAV_TICKS; i++)
|
||||
{
|
||||
if(nst_bit(&timer) && !is_epoch_bit(first_bit + i))
|
||||
{
|
||||
off_grid++;
|
||||
}
|
||||
}
|
||||
CHECK(off_grid > 0u,
|
||||
"the wrong-seed scenario did not actually produce a wrong grid");
|
||||
|
||||
/* The first decoded subframe re-aligns and clears provisional. */
|
||||
uint64_t z2 = z + 1u;
|
||||
nst_subframe(&timer, z2);
|
||||
CHECK(!timer.provisional,
|
||||
"provisional flag survived a decoded subframe");
|
||||
uint64_t bit = z2 * 300u;
|
||||
unsigned int epochs = 0u;
|
||||
for(unsigned int i = 0u; i < (3u * NAV_TICKS); i++)
|
||||
{
|
||||
if(nst_bit(&timer))
|
||||
{
|
||||
epochs++;
|
||||
CHECK(is_epoch_bit(bit + i),
|
||||
"epoch still off-grid after the subframe override");
|
||||
}
|
||||
}
|
||||
CHECK(epochs >= 2u, "no epochs after the subframe override");
|
||||
}
|
||||
|
||||
static void a_stale_seed_cannot_survive_a_drop(void)
|
||||
{
|
||||
nst_model_t timer;
|
||||
nst_init(&timer, NAV_TICKS);
|
||||
timer.enable = true;
|
||||
nst_set_busy(&timer, true);
|
||||
CHECK(nst_seed(&timer, 2u), "seed refused");
|
||||
|
||||
/* Arming again while counting must be ignored. */
|
||||
CHECK(!nst_seed(&timer, 5u), "a late seed overwrote a running phase");
|
||||
|
||||
nst_set_busy(&timer, false);
|
||||
CHECK(!timer.provisional, "provisional survived a channel drop");
|
||||
nst_set_busy(&timer, true);
|
||||
unsigned int epochs = 0u;
|
||||
for(unsigned int i = 0u; i < (4u * NAV_TICKS); i++)
|
||||
{
|
||||
if(nst_bit(&timer)) epochs++;
|
||||
}
|
||||
CHECK(epochs == 0u, "a stale seed restarted epochs after a drop");
|
||||
}
|
||||
|
||||
static void stale_context_is_refused_by_the_snap(void)
|
||||
{
|
||||
/* A context so old that the receiver clock has drifted past half an epoch
|
||||
* period must be refused rather than snapped onto the wrong epoch. */
|
||||
const double anchor_tow = epoch_tow_at_or_below(469362.0);
|
||||
double snapped = 0.0;
|
||||
CHECK(!gps_nav_epoch_snap_from_elapsed(
|
||||
anchor_tow, 300.0 + 0.09, EPOCH_PERIOD_SEC,
|
||||
SNAP_SEC, &snapped),
|
||||
"a context drifted most of an epoch was accepted");
|
||||
|
||||
/*
|
||||
* Regression for a real defect: snapping the ABSOLUTE time of week instead
|
||||
* of the elapsed time silently shifted every recovered epoch by up to half
|
||||
* a period. An anchor that is deliberately not a multiple of the epoch
|
||||
* period catches it; one that is cannot.
|
||||
*/
|
||||
CHECK(fmod(anchor_tow, EPOCH_PERIOD_SEC) > 1e-9,
|
||||
"the regression anchor is a period multiple and proves nothing");
|
||||
CHECK(gps_nav_epoch_snap_from_elapsed(anchor_tow, 12.0, EPOCH_PERIOD_SEC,
|
||||
SNAP_SEC, &snapped),
|
||||
"an exact epoch multiple was refused");
|
||||
CHECK(fabs(snapped - (anchor_tow + 12.0)) < 1e-9,
|
||||
"the snap moved an epoch that was already exact");
|
||||
/* And a context beyond the reuse window is refused before that. */
|
||||
double elapsed = MAX_REUSE_SEC + 1.0;
|
||||
CHECK(elapsed > MAX_REUSE_SEC,
|
||||
"reuse window constant is inconsistent with the firmware bound");
|
||||
}
|
||||
|
||||
/*
|
||||
* Epoch coalescing with a channel that joins mid-stream, using the real
|
||||
* gps_nav_epoch.c assembly the firmware runs.
|
||||
*/
|
||||
static void a_seeded_channel_joins_a_live_coalescing_epoch(void)
|
||||
{
|
||||
gps_nav_epoch_assembly_t assembly;
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
|
||||
const uint64_t period = 25000000ull; /* 200 ms at 125 MHz */
|
||||
const uint64_t window = 3000000ull;
|
||||
const uint64_t holdoff = 4000000ull;
|
||||
uint64_t counter = 5000000000ull;
|
||||
uint64_t local = 1000ull;
|
||||
|
||||
/* Four established channels complete an epoch. */
|
||||
uint32_t ready = 0x0fu;
|
||||
gps_nav_epoch_event_result_t r = GPS_NAV_EPOCH_EVENT_INVALID;
|
||||
for(unsigned int ch = 0u; ch < 4u; ch++)
|
||||
{
|
||||
r = gps_nav_epoch_assembly_push(&assembly, counter + ch, local + ch,
|
||||
ch, ready, period, window, holdoff);
|
||||
CHECK((r == GPS_NAV_EPOCH_EVENT_ACCEPTED) ||
|
||||
(r == GPS_NAV_EPOCH_EVENT_COMPLETE),
|
||||
"an established channel was not accepted into the epoch");
|
||||
}
|
||||
CHECK(r == GPS_NAV_EPOCH_EVENT_COMPLETE,
|
||||
"a full expected set did not complete the epoch");
|
||||
|
||||
/* Next epoch: a seeded channel 4 is now in the ready mask. The epoch must
|
||||
* still complete, now expecting five. */
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
counter += period;
|
||||
local += period;
|
||||
ready = 0x1fu;
|
||||
for(unsigned int ch = 0u; ch < 5u; ch++)
|
||||
{
|
||||
r = gps_nav_epoch_assembly_push(&assembly, counter + ch, local + ch,
|
||||
ch, ready, period, window, holdoff);
|
||||
}
|
||||
CHECK(r == GPS_NAV_EPOCH_EVENT_COMPLETE,
|
||||
"the epoch did not complete once a seeded channel joined");
|
||||
|
||||
/* A channel dropping mid-epoch must not deadlock the next one. */
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
counter += period;
|
||||
local += period;
|
||||
for(unsigned int ch = 0u; ch < 3u; ch++)
|
||||
{
|
||||
(void)gps_nav_epoch_assembly_push(&assembly, counter + ch, local + ch,
|
||||
ch, ready, period, window, holdoff);
|
||||
}
|
||||
CHECK(!gps_nav_epoch_assembly_complete(&assembly),
|
||||
"epoch reported complete before its expected set arrived");
|
||||
CHECK(gps_nav_epoch_assembly_remove_channel(&assembly, 3u) ||
|
||||
!gps_nav_epoch_assembly_complete(&assembly),
|
||||
"removing a dropped channel left the epoch inconsistent");
|
||||
(void)gps_nav_epoch_assembly_remove_channel(&assembly, 4u);
|
||||
CHECK(gps_nav_epoch_assembly_usable(&assembly, 3u),
|
||||
"a three-satellite remainder was not usable after two drops");
|
||||
|
||||
/* A late event from the previous epoch must not contaminate this one. */
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
counter += period;
|
||||
local += period;
|
||||
(void)gps_nav_epoch_assembly_push(&assembly, counter, local, 0u, ready,
|
||||
period, window, holdoff);
|
||||
r = gps_nav_epoch_assembly_push(&assembly, counter - period, local + 1u,
|
||||
1u, ready, period, window, holdoff);
|
||||
CHECK(r == GPS_NAV_EPOCH_EVENT_STALE,
|
||||
"a previous-epoch event was not rejected as stale");
|
||||
r = gps_nav_epoch_assembly_push(&assembly, counter, local + 2u, 0u, ready,
|
||||
period, window, holdoff);
|
||||
CHECK(r == GPS_NAV_EPOCH_EVENT_DUPLICATE,
|
||||
"a duplicated channel event was not rejected");
|
||||
}
|
||||
|
||||
/*
|
||||
* Reproduces the failure observed on the board: a channel that is busy, locked
|
||||
* and interrupt-enabled but whose hardware timer is idle emits nothing, yet is
|
||||
* latched into expected_mask when the epoch opens. The epoch can then never
|
||||
* complete and is discarded at its deadline.
|
||||
*
|
||||
* Run twice: with exclusion disabled it must lose every epoch (the defect), and
|
||||
* with exclusion enabled only the first few (the fix). Without the disabled
|
||||
* case this test would pass even if the fix did nothing.
|
||||
*/
|
||||
static void run_silent_channel_epochs(unsigned int limit,
|
||||
unsigned int epochs,
|
||||
unsigned int* completed_out,
|
||||
unsigned int* discarded_out,
|
||||
unsigned int* silent_miss_out)
|
||||
{
|
||||
const uint64_t period = 25000000ull; /* 200 ms at 125 MHz */
|
||||
const uint64_t window = 3000000ull;
|
||||
const uint64_t holdoff = 4000000ull;
|
||||
const unsigned int SILENT = 3u;
|
||||
unsigned int miss[8];
|
||||
memset(miss, 0, sizeof(miss));
|
||||
|
||||
uint64_t counter = 5000000000ull;
|
||||
uint64_t local = 1000ull;
|
||||
unsigned int completed = 0u, discarded = 0u;
|
||||
|
||||
for(unsigned int epoch = 0u; epoch < epochs; epoch++)
|
||||
{
|
||||
gps_nav_epoch_assembly_t assembly;
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
|
||||
uint32_t ready = 0u;
|
||||
for(unsigned int ch = 0u; ch < 4u; ch++)
|
||||
{
|
||||
/* limit == 0 models the original policy: never exclude. */
|
||||
if((limit == 0u) || (miss[ch] < limit)) ready |= (1u << ch);
|
||||
}
|
||||
|
||||
gps_nav_epoch_event_result_t r = GPS_NAV_EPOCH_EVENT_INVALID;
|
||||
for(unsigned int ch = 0u; ch < 4u; ch++)
|
||||
{
|
||||
if(ch == SILENT) continue; /* emits nothing, ever */
|
||||
if((ready & (1u << ch)) == 0u) continue;
|
||||
r = gps_nav_epoch_assembly_push(&assembly, counter + ch,
|
||||
local + ch, ch, ready,
|
||||
period, window, holdoff);
|
||||
}
|
||||
|
||||
if(r == GPS_NAV_EPOCH_EVENT_COMPLETE)
|
||||
{
|
||||
completed++;
|
||||
}
|
||||
else
|
||||
{
|
||||
discarded++;
|
||||
}
|
||||
uint32_t missing = assembly.expected_mask & ~assembly.received_mask;
|
||||
for(unsigned int ch = 0u; ch < 4u; ch++)
|
||||
{
|
||||
if(assembly.received_mask & (1u << ch)) miss[ch] = 0u;
|
||||
else if(missing & (1u << ch)) miss[ch]++;
|
||||
}
|
||||
counter += period;
|
||||
local += period;
|
||||
}
|
||||
*completed_out = completed;
|
||||
*discarded_out = discarded;
|
||||
*silent_miss_out = miss[SILENT];
|
||||
}
|
||||
|
||||
/*
|
||||
* Regression for a defect this suite originally missed: exclusion must be
|
||||
* reversible. An excluded channel is absent from ready_mask, so
|
||||
* gps_nav_epoch_assembly_push() rejects its events as INVALID and it never
|
||||
* enters received_mask. Clearing the miss count from the epoch result alone
|
||||
* therefore pins it as excluded forever - on the board this collapsed the
|
||||
* expected set to three channels and stopped position output permanently.
|
||||
* The miss count must be cleared on the event itself.
|
||||
*/
|
||||
static void an_excluded_channel_is_re_admitted_when_it_emits_again(void)
|
||||
{
|
||||
const uint64_t period = 25000000ull;
|
||||
const uint64_t window = 3000000ull;
|
||||
const uint64_t holdoff = 4000000ull;
|
||||
const unsigned int RECOVERED = 3u;
|
||||
const unsigned int LIMIT = 2u;
|
||||
unsigned int miss[8];
|
||||
memset(miss, 0, sizeof(miss));
|
||||
|
||||
/* Start with the channel already excluded, as after a silent spell. */
|
||||
miss[RECOVERED] = LIMIT;
|
||||
|
||||
uint64_t counter = 5000000000ull;
|
||||
uint64_t local = 1000ull;
|
||||
bool re_admitted = false;
|
||||
|
||||
for(unsigned int epoch = 0u; epoch < 6u; epoch++)
|
||||
{
|
||||
gps_nav_epoch_assembly_t assembly;
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
|
||||
uint32_t ready = 0u;
|
||||
for(unsigned int ch = 0u; ch < 4u; ch++)
|
||||
{
|
||||
if(miss[ch] < LIMIT) ready |= (1u << ch);
|
||||
}
|
||||
if((ready & (1u << RECOVERED)) != 0u) re_admitted = true;
|
||||
|
||||
for(unsigned int ch = 0u; ch < 4u; ch++)
|
||||
{
|
||||
if((ready & (1u << ch)) == 0u)
|
||||
{
|
||||
/* The channel is emitting again even though it is excluded.
|
||||
* This is the moment the miss count must be cleared - modelling
|
||||
* what the firmware now does in the nav-solve event handler. */
|
||||
if(ch == RECOVERED) miss[ch] = 0u;
|
||||
continue;
|
||||
}
|
||||
(void)gps_nav_epoch_assembly_push(&assembly, counter + ch,
|
||||
local + ch, ch, ready,
|
||||
period, window, holdoff);
|
||||
}
|
||||
counter += period;
|
||||
local += period;
|
||||
}
|
||||
|
||||
CHECK(re_admitted,
|
||||
"an excluded channel was never re-admitted after it resumed emitting");
|
||||
CHECK(miss[RECOVERED] == 0u,
|
||||
"a recovered channel kept a non-zero miss count");
|
||||
}
|
||||
|
||||
/*
|
||||
* Regression for the second board failure: excluding a silent channel must
|
||||
* never take the expected set below GPS_NAV_MIN_WLS_SATELLITES. With four
|
||||
* satellites, dropping one left three and made a solution impossible, where
|
||||
* waiting for the channel to recover would have produced a fix.
|
||||
*/
|
||||
static uint32_t ready_mask_with_guard(const unsigned int* miss,
|
||||
unsigned int channels,
|
||||
unsigned int limit)
|
||||
{
|
||||
uint32_t eligible = 0u, active = 0u;
|
||||
unsigned int active_count = 0u;
|
||||
for(unsigned int ch = 0u; ch < channels; ch++)
|
||||
{
|
||||
eligible |= (1u << ch);
|
||||
if(miss[ch] < limit)
|
||||
{
|
||||
active |= (1u << ch);
|
||||
active_count++;
|
||||
}
|
||||
}
|
||||
return (active_count >= GPS_NAV_MIN_WLS_SATELLITES) ? active : eligible;
|
||||
}
|
||||
|
||||
static void exclusion_never_drops_below_the_wls_minimum(void)
|
||||
{
|
||||
const unsigned int LIMIT = 2u;
|
||||
unsigned int miss[8];
|
||||
|
||||
/* Five channels, one silent: excluding it still leaves four. */
|
||||
memset(miss, 0, sizeof(miss));
|
||||
miss[2] = LIMIT;
|
||||
uint32_t mask = ready_mask_with_guard(miss, 5u, LIMIT);
|
||||
CHECK((mask & (1u << 2)) == 0u,
|
||||
"a silent channel was kept when four others remained");
|
||||
CHECK(mask == 0x1bu, "unexpected ready mask with five channels");
|
||||
|
||||
/* Four channels, one silent: excluding it would leave three, below the
|
||||
* minimum, so it must be kept and waited for instead. */
|
||||
memset(miss, 0, sizeof(miss));
|
||||
miss[2] = LIMIT;
|
||||
mask = ready_mask_with_guard(miss, 4u, LIMIT);
|
||||
CHECK((mask & (1u << 2)) != 0u,
|
||||
"exclusion dropped the expected set below the WLS minimum");
|
||||
CHECK(mask == 0x0fu, "guard did not fall back to the eligible set");
|
||||
|
||||
/* Four channels, two silent: same reasoning, keep both. */
|
||||
memset(miss, 0, sizeof(miss));
|
||||
miss[1] = LIMIT;
|
||||
miss[3] = LIMIT;
|
||||
mask = ready_mask_with_guard(miss, 4u, LIMIT);
|
||||
CHECK(mask == 0x0fu, "guard failed with two silent channels");
|
||||
|
||||
/* Six channels, two silent: four remain, so both are excluded. */
|
||||
memset(miss, 0, sizeof(miss));
|
||||
miss[1] = LIMIT;
|
||||
miss[3] = LIMIT;
|
||||
mask = ready_mask_with_guard(miss, 6u, LIMIT);
|
||||
CHECK(mask == 0x35u, "guard excluded too few with six channels");
|
||||
}
|
||||
|
||||
static void a_silent_channel_blocks_every_epoch_until_excluded(void)
|
||||
{
|
||||
const unsigned int EPOCHS = 12u;
|
||||
const unsigned int LIMIT = 2u; /* GPS_NAV_SILENT_CHANNEL_EPOCHS */
|
||||
unsigned int completed = 0u, discarded = 0u, silent_miss = 0u;
|
||||
|
||||
/* The defect: without exclusion, one silent channel costs every epoch. */
|
||||
run_silent_channel_epochs(0u, EPOCHS, &completed, &discarded,
|
||||
&silent_miss);
|
||||
CHECK(discarded == EPOCHS,
|
||||
"the silent-channel defect did not reproduce");
|
||||
CHECK(completed == 0u,
|
||||
"an epoch completed despite a permanently silent channel");
|
||||
|
||||
/* The fix: only the first LIMIT epochs are lost. */
|
||||
run_silent_channel_epochs(LIMIT, EPOCHS, &completed, &discarded,
|
||||
&silent_miss);
|
||||
CHECK(silent_miss >= LIMIT, "the silent channel was never identified");
|
||||
CHECK(discarded == LIMIT,
|
||||
"a silent channel kept discarding epochs after exclusion");
|
||||
CHECK(completed == (EPOCHS - LIMIT),
|
||||
"epochs did not resume once the silent channel was excluded");
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
a_silent_channel_blocks_every_epoch_until_excluded();
|
||||
an_excluded_channel_is_re_admitted_when_it_emits_again();
|
||||
exclusion_never_drops_below_the_wls_minimum();
|
||||
outage_matrix_rejoins_on_the_correct_grid();
|
||||
guard_band_refuses_arming_near_a_bit_edge();
|
||||
seeded_grid_equals_the_uninterrupted_grid();
|
||||
a_wrong_seed_is_corrected_by_the_first_subframe();
|
||||
a_stale_seed_cannot_survive_a_drop();
|
||||
stale_context_is_refused_by_the_snap();
|
||||
a_seeded_channel_joins_a_live_coalescing_epoch();
|
||||
if(failures == 0)
|
||||
{
|
||||
puts("gps warm reacquisition simulation passed");
|
||||
}
|
||||
return failures == 0 ? 0 : 1;
|
||||
}
|
||||
Reference in New Issue
Block a user