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

Make GPS navigation epochs interrupt-coherent

Browse Source 
Wait for every navigation-ready tracking channel in a GPS epoch, remove unlocked channels without a timeout, and keep late joins for the next epoch. Reset PLIC ownership during warm initialization and extend Linux synthetic, integrity, and fake-MMIO regression coverage.
This commit is contained in:
onur
2026-07-23 19:20:06 +03:00
parent 7cc3eb2a54
commit 516016efb9
16 changed files with 1780 additions and 246 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/fusion/tests/Makefile
+13 -2
View File
@@ -4,14 +4,15 @@ CFLAGS += -I../../gps_system/header
LDLIBS ?= -lm
GPS_TIME_SOURCE := ../../gps_system/src/gps/gps_time_anchor.c
GPS_EPOCH_SOURCE := ../../gps_system/src/gps/gps_nav_epoch.c
ESKF_SOURCES := ../../gps_system/src/fusion/eskf.c ../../gps_system/src/fusion/fusion_motion.c ../../gps_system/src/fusion/fusion_rest_detector.c ../../gps_system/src/fusion/fusion_engine.c ../../gps_system/src/fusion/fusion_gps_adapter.c ../../gps_system/src/fusion/fusion_bus.c
IMU_PROFILE_SOURCE := ../../gps_system/src/adapters/imu_fusion_profile.c
.PHONY: all test scenario-logs sanitize profile benchmark clean
.PHONY: all test gps-system-test scenario-logs sanitize profile benchmark clean
all: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test
standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE)
standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE) $(GPS_EPOCH_SOURCE)
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS)
eskf_motion_test: eskf_motion_test.c $(ESKF_SOURCES)
@@ -49,6 +50,9 @@ test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test f
./gps_reacquisition_test
./platform_scenario_matrix_test
gps-system-test: standalone_gps_integrity_test
./standalone_gps_integrity_test
scenario-logs: platform_scenario_matrix_test
mkdir -p output/scenarios
@for scenario in stationary pedestrian car aircraft; do \
@@ -62,6 +66,13 @@ scenario-logs: platform_scenario_matrix_test
@echo "Wrote 48 replay logs under output/scenarios/"
sanitize:
$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
-fsanitize=address,undefined -fno-omit-frame-pointer \
-I../../gps_system/header standalone_gps_integrity_test.c \
$(GPS_TIME_SOURCE) $(GPS_EPOCH_SOURCE) \
-o standalone_gps_integrity_test_san $(LDLIBS)
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 ./standalone_gps_integrity_test_san
$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
-fsanitize=address,undefined -fno-omit-frame-pointer \
-I../../gps_system/header fusion_rest_detector_test.c \
soc/software/fusion/tests/standalone_gps_integrity_test.c
+490 -59
View File
@@ -9,11 +9,15 @@
#include <string.h>
#include <time.h>
#include "gps/gps_nav_epoch.h"
#include "gps/gps_time_anchor.h"
#define MAX_SATELLITES 9
#define STATE_DIM 4
#define PI 3.14159265358979323846
#define RECEIVER_SAMPLES_PER_SECOND 16368000.0
#define EPOCH_SAMPLE_WINDOW 409200u
#define EPOCH_TOW_TOLERANCE_SEC 0.001
typedef struct {
unsigned prn;
@@ -49,11 +53,6 @@ typedef enum {
SOLVE_UNIFORM_WEIGHTS
} solve_weight_mode_t;
typedef struct {
int week;
double sec;
} test_tow_t;
static int failures;
static volatile double benchmark_sink;
@@ -69,38 +68,6 @@ static double square(double x)
return x * x;
}
static double test_tow_delta_sec(const test_tow_t* first, const test_tow_t* second)
{
double delta = (double)(first->week - second->week) * 604800.0 +
first->sec - second->sec;
if (delta > 302400.0) {
delta -= 604800.0;
} else if (delta < -302400.0) {
delta += 604800.0;
}
return delta;
}
static bool test_validate_tow(const test_tow_t tow[MAX_SATELLITES], unsigned count)
{
unsigned best_cluster = 0;
for (unsigned candidate = 0; candidate < count; candidate++) {
unsigned cluster = 0;
for (unsigned row = 0; row < count; row++) {
if (fabs(test_tow_delta_sec(&tow[row], &tow[candidate])) <= 0.001) {
cluster++;
}
}
if (cluster > best_cluster) {
best_cluster = cluster;
}
}
if (best_cluster <= count / 2u) {
return false;
}
return best_cluster == count;
}
static double vector_norm3(const double v[3])
{
return sqrt(square(v[0]) + square(v[1]) + square(v[2]));
@@ -424,7 +391,9 @@ static bool solve_subset(const measurement_t measurements[MAX_SATELLITES],
output->chi_square_ratio = INFINITY;
output->quality_score = 0.0;
}
output->solution_valid = output->residual_test_valid && output->chi_square_pass;
output->solution_valid =
((output->used_count == STATE_DIM) && (output->dof == 0u)) ||
(output->residual_test_valid && output->chi_square_pass);
double covariance_ecef[STATE_DIM][STATE_DIM];
if (!inverse4(absolute_normal, covariance_ecef)) {
@@ -595,6 +564,451 @@ static int run_benchmark(const char* scenario,
return 0;
}
typedef struct {
uint64_t state;
} synthetic_rng_t;
typedef struct {
bool have_sf[3];
unsigned char iod[3];
bool ephemeris_valid;
} linux_nav_channel_t;
typedef struct {
const char* name;
unsigned epochs;
unsigned candidate_total;
unsigned selected_total;
unsigned fixes;
unsigned fde_fixes;
unsigned epoch_exclusions;
double east_sum_m;
double north_sum_m;
double up_sum_m;
double east_square_sum_m2;
double north_square_sum_m2;
double up_square_sum_m2;
double horizontal_max_m;
double vertical_max_m;
double horizontal_errors_m[128];
unsigned error_count;
} synthetic_stage_metrics_t;
enum {
STAGE_BASELINE = 0,
STAGE_ONE_DROPOUT,
STAGE_THREE_DROPOUTS,
STAGE_MINIMUM_FOUR,
STAGE_OUTAGE_THREE,
STAGE_REACQUIRE,
STAGE_DELAYED_CHANNEL,
STAGE_EPHEMERIS_JOIN,
STAGE_PSEUDORANGE_FAULT,
SYNTHETIC_STAGE_COUNT
};
static uint64_t synthetic_rng_next(synthetic_rng_t* rng)
{
uint64_t value = rng->state;
value ^= value >> 12;
value ^= value << 25;
value ^= value >> 27;
rng->state = value;
return value * UINT64_C(2685821657736338717);
}
static double synthetic_rng_uniform(synthetic_rng_t* rng)
{
return ((double)(synthetic_rng_next(rng) >> 11) + 0.5) /
9007199254740992.0;
}
static double synthetic_rng_gaussian(synthetic_rng_t* rng)
{
double u1 = synthetic_rng_uniform(rng);
double u2 = synthetic_rng_uniform(rng);
return sqrt(-2.0 * log(u1)) * cos(2.0 * PI * u2);
}
static void linux_nav_receive_ephemeris_subframe(linux_nav_channel_t* channel,
unsigned subframe_id,
unsigned char iod)
{
if (channel == NULL || subframe_id < 1u || subframe_id > 3u) {
return;
}
unsigned slot = subframe_id - 1u;
channel->have_sf[slot] = true;
channel->iod[slot] = iod;
if (channel->have_sf[0] && channel->have_sf[1] && channel->have_sf[2] &&
channel->iod[0] == channel->iod[1] &&
channel->iod[1] == channel->iod[2]) {
channel->ephemeris_valid = true;
}
}
static unsigned synthetic_stage_for_epoch(unsigned epoch)
{
static const unsigned ends[SYNTHETIC_STAGE_COUNT] =
{100u, 160u, 220u, 260u, 300u, 360u, 420u, 480u, 540u};
for (unsigned stage = 0u; stage < SYNTHETIC_STAGE_COUNT; stage++) {
if (epoch < ends[stage]) {
return stage;
}
}
return SYNTHETIC_STAGE_COUNT - 1u;
}
static uint32_t synthetic_active_mask(unsigned stage)
{
static const uint32_t masks[SYNTHETIC_STAGE_COUNT] = {
0x0ffu, /* eight healthy channels */
0x07fu, /* one channel lost */
0x01fu, /* three channels lost */
0x00fu, /* minimum 3-D fix plus clock */
0x007u, /* outage: below the four-channel minimum */
0x07fu, /* reacquisition */
0x0ffu, /* one of eight arrives in the wrong epoch */
0x1ffu, /* ninth channel joins after SF1/SF2/SF3 commit */
0x1ffu /* one coherent 100 m pseudorange fault */
};
return masks[stage];
}
static void synthetic_epoch_time(unsigned epoch, int* week, double* tow_sec)
{
double absolute_tow = 604790.0 + 0.2 * (double)epoch;
*week = 2300 + (int)floor(absolute_tow / 604800.0);
*tow_sec = fmod(absolute_tow, 604800.0);
}
static void synthetic_shift_time(int* week, double* tow_sec, double delta_sec)
{
*tow_sec += delta_sec;
while (*tow_sec >= 604800.0) {
*tow_sec -= 604800.0;
(*week)++;
}
while (*tow_sec < 0.0) {
*tow_sec += 604800.0;
(*week)--;
}
}
static int compare_double(const void* left, const void* right)
{
double lhs = *(const double*)left;
double rhs = *(const double*)right;
return (lhs > rhs) - (lhs < rhs);
}
static void synthetic_record_error(synthetic_stage_metrics_t* metrics,
const solution_t* solution,
const double receiver_truth[3],
double axes[3][3])
{
double ecef_error[3] = {
solution->state[0] - receiver_truth[0],
solution->state[1] - receiver_truth[1],
solution->state[2] - receiver_truth[2]
};
double enu_error[3] = {0.0, 0.0, 0.0};
for (unsigned enu = 0u; enu < 3u; enu++) {
for (unsigned ecef = 0u; ecef < 3u; ecef++) {
enu_error[enu] += axes[enu][ecef] * ecef_error[ecef];
}
}
double horizontal_error = hypot(enu_error[0], enu_error[1]);
double vertical_error = fabs(enu_error[2]);
metrics->east_sum_m += enu_error[0];
metrics->north_sum_m += enu_error[1];
metrics->up_sum_m += enu_error[2];
metrics->east_square_sum_m2 += square(enu_error[0]);
metrics->north_square_sum_m2 += square(enu_error[1]);
metrics->up_square_sum_m2 += square(enu_error[2]);
if (horizontal_error > metrics->horizontal_max_m) {
metrics->horizontal_max_m = horizontal_error;
}
if (vertical_error > metrics->vertical_max_m) {
metrics->vertical_max_m = vertical_error;
}
if (metrics->error_count <
sizeof(metrics->horizontal_errors_m) /
sizeof(metrics->horizontal_errors_m[0])) {
metrics->horizontal_errors_m[metrics->error_count++] = horizontal_error;
}
}
static void synthetic_print_metrics(synthetic_stage_metrics_t metrics)
{
double count = (double)metrics.fixes;
double horizontal_rmse = count > 0.0
? sqrt((metrics.east_square_sum_m2 + metrics.north_square_sum_m2) / count)
: 0.0;
double vertical_rmse = count > 0.0
? sqrt(metrics.up_square_sum_m2 / count) : 0.0;
double horizontal_spread = 0.0;
double vertical_spread = 0.0;
if (count > 0.0) {
double east_mean = metrics.east_sum_m / count;
double north_mean = metrics.north_sum_m / count;
double up_mean = metrics.up_sum_m / count;
double east_variance = metrics.east_square_sum_m2 / count - square(east_mean);
double north_variance = metrics.north_square_sum_m2 / count - square(north_mean);
double up_variance = metrics.up_square_sum_m2 / count - square(up_mean);
horizontal_spread = sqrt(fmax(0.0, east_variance) +
fmax(0.0, north_variance));
vertical_spread = sqrt(fmax(0.0, up_variance));
}
double p95 = 0.0;
if (metrics.error_count != 0u) {
qsort(metrics.horizontal_errors_m, metrics.error_count,
sizeof(metrics.horizontal_errors_m[0]), compare_double);
unsigned p95_index = (95u * metrics.error_count + 99u) / 100u;
if (p95_index != 0u) {
p95_index--;
}
p95 = metrics.horizontal_errors_m[p95_index];
}
printf("%-21s epochs=%3u fixes=%3u avg=%4.1f->%4.1f "
"epoch_excl=%3u fde=%3u hRMSE=%6.2f hSTD=%6.2f "
"hP95=%6.2f hMAX=%6.2f vRMSE=%6.2f vSTD=%6.2f\n",
metrics.name, metrics.epochs, metrics.fixes,
metrics.epochs != 0u
? (double)metrics.candidate_total / (double)metrics.epochs : 0.0,
metrics.epochs != 0u
? (double)metrics.selected_total / (double)metrics.epochs : 0.0,
metrics.epoch_exclusions, metrics.fde_fixes,
horizontal_rmse, horizontal_spread, p95, metrics.horizontal_max_m,
vertical_rmse, vertical_spread);
}
static void run_general_synthetic_scenario(const double receiver_truth[3],
double latitude_rad,
double longitude_rad,
double clock_bias_m)
{
static const char* names[SYNTHETIC_STAGE_COUNT] = {
"baseline-8",
"one-dropout-7",
"three-dropouts-5",
"minimum-fix-4",
"outage-3",
"reacquire-7",
"delayed-channel",
"sf123-join",
"100m-fault-FDE"
};
synthetic_stage_metrics_t metrics[SYNTHETIC_STAGE_COUNT];
memset(metrics, 0, sizeof(metrics));
for (unsigned stage = 0u; stage < SYNTHETIC_STAGE_COUNT; stage++) {
metrics[stage].name = names[stage];
}
linux_nav_channel_t channels[MAX_SATELLITES];
memset(channels, 0, sizeof(channels));
for (unsigned satellite = 0u; satellite < 8u; satellite++) {
channels[satellite].ephemeris_valid = true;
}
linux_nav_channel_t iod_regression = {0};
linux_nav_receive_ephemeris_subframe(&iod_regression, 1u, 17u);
linux_nav_receive_ephemeris_subframe(&iod_regression, 2u, 17u);
linux_nav_receive_ephemeris_subframe(&iod_regression, 3u, 18u);
CHECK(!iod_regression.ephemeris_valid,
"mismatched SF1/SF2/SF3 IOD must not make a channel ready");
linux_nav_receive_ephemeris_subframe(&iod_regression, 3u, 17u);
CHECK(iod_regression.ephemeris_valid,
"matching SF1/SF2/SF3 must make a channel ready without SF4/SF5");
double axes[3][3];
enu_axes(latitude_rad, longitude_rad, axes);
synthetic_rng_t rng = {UINT64_C(0x91e10da5c79e7b1d)};
double seed[STATE_DIM] = {
receiver_truth[0] + 120.0,
receiver_truth[1] - 90.0,
receiver_truth[2] + 60.0,
clock_bias_m - 200.0
};
unsigned delayed_selection_failures = 0u;
unsigned below_minimum_fixes = 0u;
unsigned precommit_join_count_failures = 0u;
unsigned postcommit_join_count_failures = 0u;
unsigned fde_wrong_exclusion = 0u;
bool saw_week_rollover = false;
bool saw_sample_rollover = false;
uint32_t previous_raw_sample = 0u;
for (unsigned epoch = 0u; epoch < 540u; epoch++) {
unsigned stage = synthetic_stage_for_epoch(epoch);
synthetic_stage_metrics_t* stage_metrics = &metrics[stage];
stage_metrics->epochs++;
if (stage == STAGE_EPHEMERIS_JOIN && epoch == 420u) {
linux_nav_receive_ephemeris_subframe(&channels[8], 1u, 29u);
linux_nav_receive_ephemeris_subframe(&channels[8], 2u, 29u);
}
if (stage == STAGE_EPHEMERIS_JOIN && epoch == 450u) {
linux_nav_receive_ephemeris_subframe(&channels[8], 3u, 29u);
}
const double zero_noise[MAX_SATELLITES] = {0.0};
measurement_t master[MAX_SATELLITES];
create_measurements(master, MAX_SATELLITES, receiver_truth,
latitude_rad, longitude_rad, clock_bias_m,
zero_noise);
for (unsigned satellite = 0u; satellite < MAX_SATELLITES; satellite++) {
master[satellite].pseudorange_m +=
0.35 * master[satellite].sigma_m *
synthetic_rng_gaussian(&rng);
}
if (stage == STAGE_PSEUDORANGE_FAULT) {
master[2].pseudorange_m += 100.0;
}
int week;
double tow_sec;
synthetic_epoch_time(epoch, &week, &tow_sec);
if (week > 2300) {
saw_week_rollover = true;
}
uint32_t raw_sample =
(UINT32_MAX - 100000u) + epoch * 3273600u;
if (epoch != 0u && raw_sample < previous_raw_sample) {
saw_sample_rollover = true;
}
previous_raw_sample = raw_sample;
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES];
unsigned candidate_to_satellite[GPS_NAV_EPOCH_MAX_CANDIDATES];
unsigned candidate_count = 0u;
uint32_t active_mask = synthetic_active_mask(stage);
for (unsigned satellite = 0u; satellite < MAX_SATELLITES; satellite++) {
if ((active_mask & (1u << satellite)) == 0u ||
!channels[satellite].ephemeris_valid) {
continue;
}
candidates[candidate_count].week = week;
candidates[candidate_count].tow_sec = tow_sec;
candidates[candidate_count].raw_sample =
raw_sample + satellite * 5000u;
if (stage == STAGE_DELAYED_CHANNEL && satellite == 7u) {
double offset_sec = (epoch & 1u) != 0u ? 0.4 : -0.4;
synthetic_shift_time(&candidates[candidate_count].week,
&candidates[candidate_count].tow_sec,
offset_sec);
candidates[candidate_count].raw_sample +=
(epoch & 1u) != 0u ? 6547200u : (uint32_t)-6547200;
}
candidate_to_satellite[candidate_count] = satellite;
candidate_count++;
}
stage_metrics->candidate_total += candidate_count;
gps_nav_epoch_selection_t selection;
bool selected = gps_nav_epoch_select(
candidates, candidate_count, EPOCH_SAMPLE_WINDOW,
EPOCH_TOW_TOLERANCE_SEC, &selection);
unsigned selected_count = selected ? selection.selected_count : 0u;
stage_metrics->selected_total += selected_count;
stage_metrics->epoch_exclusions += candidate_count - selected_count;
if (stage == STAGE_DELAYED_CHANNEL &&
(selected_count != 7u ||
(selection.selected_mask & (1u << 7)) != 0u)) {
delayed_selection_failures++;
}
if (stage == STAGE_EPHEMERIS_JOIN && epoch < 450u &&
selected_count != 8u) {
precommit_join_count_failures++;
}
if (stage == STAGE_EPHEMERIS_JOIN && epoch >= 450u &&
selected_count != 9u) {
postcommit_join_count_failures++;
}
if (selected_count < STATE_DIM) {
continue;
}
measurement_t selected_measurements[MAX_SATELLITES];
unsigned destination = 0u;
for (unsigned candidate = 0u; candidate < candidate_count; candidate++) {
if ((selection.selected_mask & (1u << candidate)) != 0u) {
selected_measurements[destination++] =
master[candidate_to_satellite[candidate]];
}
}
solution_t solution;
solution_t full_solution;
if (!solve_with_fde(selected_measurements, selected_count, seed,
latitude_rad, longitude_rad,
&solution, &full_solution) ||
!solution.solution_valid) {
continue;
}
stage_metrics->fixes++;
if (solution.fde_applied) {
stage_metrics->fde_fixes++;
}
if (stage == STAGE_OUTAGE_THREE) {
below_minimum_fixes++;
}
if (stage == STAGE_PSEUDORANGE_FAULT &&
(!solution.fde_applied ||
(solution.excluded_mask & (1u << 2)) == 0u)) {
fde_wrong_exclusion++;
}
synthetic_record_error(stage_metrics, &solution, receiver_truth, axes);
memcpy(seed, solution.state, sizeof(seed));
}
puts("\nGeneral synthetic GPS channel/epoch scenario");
puts("stage epochs/fixes candidates->selected and position scatter");
for (unsigned stage = 0u; stage < SYNTHETIC_STAGE_COUNT; stage++) {
synthetic_print_metrics(metrics[stage]);
}
CHECK(saw_week_rollover, "general scenario must cross a GPS week boundary");
CHECK(saw_sample_rollover, "general scenario must cross uint32 ABS_SAMPLE rollover");
CHECK(metrics[STAGE_BASELINE].fixes >= 98u,
"baseline must produce stable fixes across time/sample rollover");
CHECK(metrics[STAGE_ONE_DROPOUT].fixes >= 58u,
"one satellite dropout must not interrupt fixes");
CHECK(metrics[STAGE_THREE_DROPOUTS].fixes >= 58u,
"five remaining satellites must continue producing fixes");
CHECK(metrics[STAGE_MINIMUM_FOUR].fixes == 40u,
"four satellites must remain a publishable minimum fix");
CHECK(metrics[STAGE_OUTAGE_THREE].fixes == 0u && below_minimum_fixes == 0u,
"three satellites must suppress the position solution");
CHECK(metrics[STAGE_REACQUIRE].fixes >= 58u,
"position must recover after satellite reacquisition");
CHECK(delayed_selection_failures == 0u &&
metrics[STAGE_DELAYED_CHANNEL].fixes >= 58u,
"delayed/newest outlier channel must be excluded without blocking fixes");
CHECK(precommit_join_count_failures == 0u,
"joining channel must remain excluded before matching SF1/SF2/SF3");
CHECK(postcommit_join_count_failures == 0u,
"joining channel must enter immediately after SF1/SF2/SF3 commit");
CHECK(metrics[STAGE_EPHEMERIS_JOIN].fixes >= 58u,
"ephemeris-ready channel join must preserve position continuity");
CHECK(metrics[STAGE_PSEUDORANGE_FAULT].fixes >= 58u &&
metrics[STAGE_PSEUDORANGE_FAULT].fde_fixes >= 58u &&
fde_wrong_exclusion == 0u,
"single 100 m pseudorange fault must be isolated by FDE");
CHECK(metrics[STAGE_BASELINE].horizontal_max_m < 25.0 &&
metrics[STAGE_REACQUIRE].horizontal_max_m < 25.0 &&
metrics[STAGE_DELAYED_CHANNEL].horizontal_max_m < 25.0,
"nominal/recovery/delay position error must stay bounded");
CHECK(metrics[STAGE_MINIMUM_FOUR].horizontal_max_m < 120.0,
"minimum four-satellite geometry must stay within the synthetic bound");
}
int main(int argc, char** argv)
{
const unsigned satellite_count = 7;
@@ -628,7 +1042,7 @@ int main(int argc, char** argv)
puts("Synthetic standalone GPS integrity/FDE comparison");
puts("quality = 100/(1 + chi_square/threshold); 50 is the 99% reject boundary\n");
const double receiver_samples_per_second = 16368000.0;
const double receiver_samples_per_second = RECEIVER_SAMPLES_PER_SECOND;
int anchored_week = 0;
double anchored_tow = 0.0;
double anchor_age = 0.0;
@@ -703,30 +1117,44 @@ int main(int argc, char** argv)
&anchored_week, &anchored_tow, NULL),
"stale HOW anchor must be rejected");
test_tow_t mismatched_tow[MAX_SATELLITES] = {
{2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8},
{2300, 46386.8}, {2300, 46386.8}, {2300, 46386.6},
{2300, 46386.8}
gps_nav_epoch_selection_t epoch_selection;
gps_nav_epoch_key_t mismatched_epoch[MAX_SATELLITES] = {
{2300, 46386.8, 1000000u}, {2300, 46386.8, 1010000u},
{2300, 46386.8, 1020000u}, {2300, 46386.8, 1030000u},
{2300, 46386.8, 1040000u}, {2300, 46386.6, 5000000u},
{2300, 46386.8, 1050000u}
};
CHECK(!test_validate_tow(mismatched_tow, 7u),
"200 ms TOW mismatch must reject instead of copying the majority TOW");
test_tow_t coherent_tow[MAX_SATELLITES] = {
{2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8},
{2300, 46386.8}, {2300, 46386.8}, {2300, 46386.8},
{2300, 46386.8}
CHECK(gps_nav_epoch_select(mismatched_epoch, 7u, EPOCH_SAMPLE_WINDOW,
EPOCH_TOW_TOLERANCE_SEC, &epoch_selection) &&
epoch_selection.selected_count == 6u &&
(epoch_selection.selected_mask & (1u << 5)) == 0u,
"200 ms TOW outlier must be excluded without blocking six good channels");
gps_nav_epoch_key_t coherent_epoch[MAX_SATELLITES] = {
{2300, 46386.8, 1000000u}, {2300, 46386.8, 1010000u},
{2300, 46386.8, 1020000u}, {2300, 46386.8, 1030000u},
{2300, 46386.8, 1040000u}, {2300, 46386.8, 1050000u},
{2300, 46386.8, 1060000u}
};
CHECK(test_validate_tow(coherent_tow, 7u),
"fully coherent TOW snapshot should pass");
test_tow_t ambiguous_tow[MAX_SATELLITES] = {
{2300, 100.0}, {2300, 100.0}, {2300, 101.0}, {2300, 101.0}
CHECK(gps_nav_epoch_select(coherent_epoch, 7u, EPOCH_SAMPLE_WINDOW,
EPOCH_TOW_TOLERANCE_SEC, &epoch_selection) &&
epoch_selection.selected_count == 7u,
"fully coherent epoch must retain all channels");
gps_nav_epoch_key_t ambiguous_epoch[MAX_SATELLITES] = {
{2300, 100.0, 1000000u}, {2300, 100.0, 1010000u},
{2300, 101.0, 5000000u}, {2300, 101.0, 5010000u}
};
CHECK(!test_validate_tow(ambiguous_tow, 4u),
"two-versus-two TOW split must reject the navigation epoch");
test_tow_t excessive_tow[MAX_SATELLITES] = {
{2300, 100.0}, {2300, 100.0}, {2300, 100.0}, {2300, 96.0}
CHECK(gps_nav_epoch_select(ambiguous_epoch, 4u, EPOCH_SAMPLE_WINDOW,
EPOCH_TOW_TOLERANCE_SEC, &epoch_selection) &&
epoch_selection.selected_count == 2u,
"two-versus-two epoch split must stay below the four-channel minimum");
gps_nav_epoch_key_t excessive_epoch[MAX_SATELLITES] = {
{2300, 100.0, 1000000u}, {2300, 100.0, 1010000u},
{2300, 100.0, 1020000u}, {2300, 96.0, 5000000u}
};
CHECK(!test_validate_tow(excessive_tow, 4u),
"large TOW disagreement must not be silently corrected");
CHECK(gps_nav_epoch_select(excessive_epoch, 4u, EPOCH_SAMPLE_WINDOW,
EPOCH_TOW_TOLERANCE_SEC, &epoch_selection) &&
epoch_selection.selected_count == 3u,
"three-versus-one disagreement must stay below the navigation minimum");
solution_t absolute_solution;
solution_t normalized_solution;
@@ -855,6 +1283,9 @@ int main(int argc, char** argv)
normalized_solution.covariance_enu_m2[2][2] > 0.0,
"ENU covariance diagonal must be positive");
run_general_synthetic_scenario(receiver_truth, latitude_rad,
longitude_rad, clock_bias_m);
if (failures == 0) {
puts("\nPASS: all synthetic integrity checks passed");
return 0;