Make GPS navigation epochs interrupt-coherent
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:
@@ -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 \
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -61,6 +61,7 @@ APP_SRCS := \
|
||||
src/gps/gps_ip.c \
|
||||
src/gps/gps_interrupt_handlers.c \
|
||||
src/gps/gps_time_anchor.c \
|
||||
src/gps/gps_nav_epoch.c \
|
||||
src/gps/gps_pps.c \
|
||||
src/gps/linked_list.c \
|
||||
src/gps/NavEquations.c \
|
||||
|
||||
@@ -638,27 +638,29 @@ GPS_TRK_MEASUREMENT_STALE_TICKS
|
||||
- `NAV_SOLVE_EN=1`
|
||||
- `NAV_SOLVE_INTR_EN=0`
|
||||
- `NAV_SOLVE_FLAG_EN=0`
|
||||
- Firmware once nav bit interrupt'lari ile subframe 1..5 verisini toplar.
|
||||
- En az 4 satellite icin initial frame set hazir olunca nav-bit snapshot'i yalniz ephemeris uygunlugunu ve kullanilacak kanallari belirler; bu snapshot'tan pozisyon hesaplanmaz.
|
||||
- Subframe 1..5 seti hazir olan tracking kanallarinda nav-solve akisi etkinlestirilir:
|
||||
- Firmware nav bit interrupt'lari ile subframe verisini surekli toplar.
|
||||
- Bir tracking kanali ancak IODC/IODE degerleri eslesen subframe 1, 2 ve 3 seti atomik olarak commit edildikten sonra navigation-ready olur. Subframe 4 ve 5 ilk fix'i bloke etmez; page 18 iyonosfer/UTC ve almanak verisi arka planda toplanmaya devam eder.
|
||||
- En az 4 satellite icin tutarli ephemeris commit edilince nav-bit snapshot'i yalniz ephemeris uygunlugunu ve kullanilacak kanallari belirler; bu snapshot'tan pozisyon hesaplanmaz.
|
||||
- Tutarlı ephemerisi hazir olan tracking kanallarinda nav-solve akisi etkinlestirilir:
|
||||
- nav bits interrupt/flag acik kalir,
|
||||
- `NAV_SOLVE_INTR_EN=1`,
|
||||
- `NAV_SOLVE_FLAG_EN=1`.
|
||||
- Ilk pozisyon dahil butun pozisyon hesaplari, kanallardan senkron nav-solve tick ve `ABS_SAMPLE` geldikten sonra olusturulan coherent nav-solve snapshot'i ile yapilir.
|
||||
- Nav-bit ve nav-solve handler'lari ephemeris deposunu ortak kullansa da corrected `ABS_SAMPLE` nav-solve icin node icinde ayri saklanir. Pozisyon snapshot'i bu ayri alani kullanir; araya giren nav-bit interrupt'i pozisyon olcum epoch'unu degistiremez.
|
||||
- Coherent snapshot olusturulurken 32-bit raw `ABS_SAMPLE` degerleri en yeni sample etrafinda modulo fark ile unwrap edilir. Sayac wrap sinirinin iki tarafindaki uydular ayni pseudorange hesabinda kullanilabilir.
|
||||
- Nav bits akisi subframe 1..5 ilk kez alindiktan sonra kapatilmaz. Boylece yeni subframe 1..3 verisi ephemerisi, subframe 4 page 18 verisi iyonosfer/UTC parametrelerini guncel tutar.
|
||||
- Nav bits akisi ilk tutarli subframe 1..3 seti commit edildikten sonra kapatilmaz. Boylece yeni subframe 1..3 verisi ephemerisi, subframe 4 page 18 verisi iyonosfer/UTC parametrelerini guncel tutar.
|
||||
- Nav-solve interrupt'lari acilmadan once TOW, nav-bit HOW alanindan kurulur. Nav-solve acildiktan sonra normal nav-bit HOW gecmisi tutulmaya devam eder fakat canli TOW'a yazmaz. Her nav-solve interrupt'i tam olarak `NAV_SOLVE_TIMER * 20 ms` ilerlemeyi temsil eder ve TOW bu sabit miktarda artirilir.
|
||||
- Subframe 1..3 ham kelimeleri once staging alaninda biriktirilir. IODC'nin alt 8 biti ile iki IODE alani eslesen tam set olusmadan canli ephemeris degistirilmez; yayin guncellemesi sirasinda eski/yeni ephemeris alanlari karismaz.
|
||||
- Subframe 1 `toc` alani parity bitleri disarida birakilarak 16 bit okunur ve `2^4` ile olceklenir.
|
||||
- Subframe 4 page 18 henuz alinmadiysa iyonosfer duzeltmesi uygulanmaz. Page 18 hazir oldugunda her navigation solve tek bir `ionoutc` kopyasi kullanir.
|
||||
- Klobuchar gecikmesi WLS'nin receiver ECEF, azimut ve elevasyon hesabindan uretilir. Ilk unseeded iteration iyonosfersizdir; yaklasik receiver konumu olustuktan sonraki iteration'larda gecikme gozlenen pseudorange'dan cikarilir.
|
||||
- Bundan sonra periyodik pozisyon hesaplamasi nav-solve interrupt'lari ile tetiklenir. RTL `subframe_ready` sonrasinda COUNT durumuna girer ve `NAV_SOLVE_TIMER=N` icin N adet navigation `bit_valid` darbesi sayar. Her navigation biti 20 ms oldugu icin periyot `N * 20 ms`'dir; firmware `GPS_NAV_SOLVE_TIMER_TICKS` build ayarini `gps_startup_initialization()` fonksiyonuna verir.
|
||||
- Bu siralama onemlidir: RTL `nav_solve_timer` sayaci `NAV_SOLVE_EN` ve ilk gecerli `subframe_ready` ile baslar. `NAV_SOLVE_EN` nav-bit asamasinda acik tutulur; subframe 1..5 hazir olana kadar yalniz nav-solve interrupt/flag kapali kalir. Timer COUNT durumuna girdikten sonra sonraki subframe darbeleri sayaci sifirlamaz veya yeniden fazlamaz.
|
||||
- Bu siralama onemlidir: RTL `nav_solve_timer` sayaci `NAV_SOLVE_EN` ve ilk gecerli `subframe_ready` ile baslar. `NAV_SOLVE_EN` nav-bit asamasinda acik tutulur; tutarli subframe 1..3 ephemerisi commit edilene kadar yalniz nav-solve interrupt/flag kapali kalir. Timer COUNT durumuna girdikten sonra sonraki subframe darbeleri sayaci sifirlamaz veya yeniden fazlamaz.
|
||||
- Tracking kanallari farkli zamanlarda lock olabilir ve ilk gecerli subframe'lerini 6 saniyenin farkli katlarinda gorebilir. Timer bu nedenle ilk subframe'in HOW alanindaki 17-bit Z-count degerini kullanarak `Z_COUNT * 300 mod NAV_SOLVE_TIMER` baslangic fazini hesaplar. Hesap ALIGN durumunda 26 PCLK cevriminde ard arda modulo ile yapilir; 20 ms'lik ilk navigation bitinden cok once tamamlanir. RTL ayni GPS haftasi icinde keyfi pozitif `N` degerlerini hizalayabilir; firmware ise hafta rollover'i oncesinden aktif kalan kanallarla rollover sonrasinda katilan kanallarin da ayni fazda kalmasi icin `30240000 mod N == 0` kosulunu compile time'da zorunlu tutar. Ornegin `N=40`, `N=100`, `N=125`, `N=150`, `N=200` ve `N=300` gecerlidir; kosulu saglamayan build acik bir `#error` ile durur.
|
||||
- Initial position sonrasinda subframe'lerini tamamlayan yeni tracking kanallarinda da nav bits acik tutularak nav-solve otomatik etkinlestirilir.
|
||||
- Coherence kontrolu sadece software systick ile yapilmaz; periodic snapshot icin tracking node'da saklanan nav-solve interrupt sonrasi raw `ABS_SAMPLE` kullanilir. `GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS=25` oldugu icin 4 satellite measurement'i 25 ms'den fazla ayriksa navigation solve calistirilmaz. Bu pencere interrupt skew'den cok uydu path delay farkini kapsamak icindir: 20 ms yaklasik 6000 km, 25 ms yaklasik 7500 km pseudorange farkina denk gelir. 25 ms, dusuk-elevation uydu geometrisi ve tracking jitter icin pay birakir ama 50 ms kadar gevsek degildir.
|
||||
- Partial coherent grup sayisi mevcut `ready channels` sayisina ulasmadiysa firmware nav-solve sequence'lerini consume etmez. Coherent snapshot tum ready kanallari kapsasa bile sequence ancak navigation solve basarili olduktan sonra, snapshot aninda kaydedilen sequence degerine kadar consume edilir. Basarisiz solve ayni epoch'u tekrar deneyebilir; daha yeni sequence yanlislikla tuketilmez.
|
||||
- Her nav periyodunda yeni sequence'i bulunan adaylar sabit boyutlu, heap kullanmayan epoch selector'a verilir. Selector TOW ve modulo-32-bit `ABS_SAMPLE` kosullarini ikili olarak saglayan en buyuk coherent alt kumeyi secer; tek bir gecikmis veya bozuk kanal diger en az 4 uyduyu bloke etmez. Esit buyuklukte ayrik kumelerde daha yeni sample epoch'u tercih edilir.
|
||||
- En az 4 uydu secildiginde kume disindaki snapshot sequence'leri emekliye ayrilir. Tuketim snapshot aninda kaydedilen sequence degerine kadar yapildigi icin arada gelen daha yeni interrupt korunur. Tamamlanan donanim epoch'u WLS sonucu basarili veya basarisiz olsa da yalniz bir kez denenir; ayni olcumlerin ilgisiz bir foreground wake-up ile tekrar cozulmesi engellenir.
|
||||
- Navigation snapshot ayni SVID/PRN'i ikinci kez kabul etmez. Ayni uydu iki tracking IP uzerinden ready gorunurse ikinci kopya sessizce snapshot disinda birakilir; ayrinti sadece `DEBUG_TRACE` seviyesinde gorulur.
|
||||
- UART debug satirindaki `coherent_nav=` degeri, o anda ayni nav-solve penceresinde kullanilabilir kanal sayisini gosterir. 600 ms pozisyon gelmiyorsa `busy channels`, `ready channels` ve `coherent_nav` birlikte kontrol edilmelidir.
|
||||
- Tracking ISR event queue dolarsa, her tracking IP icin son event shadow buffer'a alinir. Shadow overwrite sayaci WARN olarak loglanir; bu log gorulurse CPU nav hesaplamasi veya UART debug cikisi tracking interrupt isleme hizini geciktiriyor olabilir.
|
||||
@@ -706,7 +708,8 @@ weight = 1 / variance
|
||||
- `computeRange()` icinde aktif parametreler `psTVLTime`, `sat_no`, `eph`, `clockCorr` ve `satellitePositions`'dir. `rho`, `ionoutc`, `xyz`, `tempPos` ve `velocities` eski genis API'den kalmistir. Ionosphere correction artik receiver azimut/elevasyonu hazir oldugunda WLS icinde uygulanir.
|
||||
- Ephemeris alanlarinin mevcut isimleri ile ICD anlamlari su sekildedir: `omgdot` gercekte `Omega0`, `omg0` gercekte argument of perigee `omega`, `omgkdot` gercekte `OmegaDot` degeridir. `satpos()` bunlari dogru matematiksel yerde kullanir; konu su an hesap hatasi degil, ileride yapilacak kontrollu isim duzeltmesidir.
|
||||
- `POSTPONE_TOW=0.160` donanim/pipeline zaman kalibrasyonunun bir parcasidir; kaynagi dogrulanmadan kaldirilmamalidir.
|
||||
- Tum ready kanallarin coherent nav-solve interrupt'inin beklenmesi bilincli davranistir. Donanimin her aktif tracking kanali icin ilgili interrupt'i uretecegi kabul edilir.
|
||||
- Ilk nav-solve interrupt'i geldiginde o anda allocated, hardware busy ve nav-solve interrupt'i etkin olan kanallar epoch'un sabit bekleme maskesine alinir. WLS ancak bu kanallarin ayni GPS epoch'una ait interrupt'lari tamamlandiginda tetiklenir. Unlock interrupt'i gelen kanal bekleme maskesinden cikarilir; epoch ortasinda navigation-ready olan yeni kanal ise aktif maskeyi buyutmez ve sonraki epoch'tan itibaren beklenir. Timeout yoktur: calisan tracking IP nav-solve interrupt'i, lock kaybeden IP unlock interrupt'i ile ilerleme saglar.
|
||||
- Interrupt bekleme maskesi tamamlandiktan sonra en az dort uyduluk en buyuk ortak TOW/`ABS_SAMPLE` kumesi WLS girdisi olarak kullanilir; kume disi kanallar `epoch_excluded` olarak raporlanir ve sonraki donanim epoch'unda yeniden katilabilir.
|
||||
- Dort uydu 3D konum + receiver clock bias icin minimum ve yeterlidir. Bu durumda
|
||||
`dof=N-4=0` oldugu icin WLS konumu publish edilir fakat residual chi-square
|
||||
integrity bayragi false kalir. Bes veya daha fazla uyduda publish icin mevcut
|
||||
|
||||
@@ -19,7 +19,6 @@
|
||||
// |___|_| |____/|_____|_| |___|_| \_|_____|____/
|
||||
|
||||
#define MIN_NEEDED_SATELLITE 4
|
||||
#define GPS_INITIAL_SUBFRAME_MASK ((1u << 1) | (1u << 2) | (1u << 3) | (1u << 4) | (1u << 5))
|
||||
#define GPS_TRCK_UNLOCK_TIMEOUT_TICKS 500000000u
|
||||
#define GPS_PRN_COUNT 32u
|
||||
#define GPS_PRN_INVALID 0xffu
|
||||
@@ -318,6 +317,7 @@
|
||||
double omgkdot; // from subframe 3
|
||||
double idot; // from subframe 3
|
||||
unsigned char update; // from subframe 3
|
||||
bool ephemeris_valid; // true only after a matching SF1/SF2/SF3 set is committed.
|
||||
unsigned char sf4_data_id; // from subframe 4 word 3.
|
||||
unsigned char sf4_sv_id; // page identifier encoded in subframe 4 word 3.
|
||||
unsigned char sf4_page; // decoded page number, or 0 if not mapped.
|
||||
|
||||
@@ -0,0 +1,70 @@
|
||||
#ifndef GPS_NAV_EPOCH_H
|
||||
#define GPS_NAV_EPOCH_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/*
|
||||
* The current receiver exposes nine tracking channels. Keep the epoch
|
||||
* selector bounded so its exhaustive, allocation-free search has a fixed
|
||||
* worst-case cost on the embedded target.
|
||||
*/
|
||||
#define GPS_NAV_EPOCH_MAX_CANDIDATES 9u
|
||||
|
||||
typedef struct {
|
||||
int week;
|
||||
double tow_sec;
|
||||
uint32_t raw_sample;
|
||||
} gps_nav_epoch_key_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t selected_mask;
|
||||
unsigned int selected_count;
|
||||
unsigned int reference_index;
|
||||
} gps_nav_epoch_selection_t;
|
||||
|
||||
typedef struct {
|
||||
uint64_t epoch_id;
|
||||
uint32_t expected_mask;
|
||||
uint32_t received_mask;
|
||||
bool active;
|
||||
} gps_nav_epoch_assembly_t;
|
||||
|
||||
typedef enum {
|
||||
GPS_NAV_EPOCH_EVENT_INVALID = 0,
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
GPS_NAV_EPOCH_EVENT_DUPLICATE,
|
||||
GPS_NAV_EPOCH_EVENT_STALE,
|
||||
GPS_NAV_EPOCH_EVENT_FUTURE,
|
||||
GPS_NAV_EPOCH_EVENT_COMPLETE
|
||||
} gps_nav_epoch_event_result_t;
|
||||
|
||||
bool gps_nav_epoch_select(
|
||||
const gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES],
|
||||
unsigned int candidate_count,
|
||||
uint32_t sample_window,
|
||||
double tow_tolerance_sec,
|
||||
gps_nav_epoch_selection_t* selection);
|
||||
|
||||
bool gps_nav_epoch_id(
|
||||
int week,
|
||||
double tow_sec,
|
||||
unsigned int period_ms,
|
||||
uint64_t* epoch_id);
|
||||
|
||||
void gps_nav_epoch_assembly_reset(gps_nav_epoch_assembly_t* assembly);
|
||||
|
||||
gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
|
||||
gps_nav_epoch_assembly_t* assembly,
|
||||
uint64_t epoch_id,
|
||||
unsigned int channel,
|
||||
uint32_t ready_mask);
|
||||
|
||||
bool gps_nav_epoch_assembly_remove_channel(
|
||||
gps_nav_epoch_assembly_t* assembly,
|
||||
unsigned int channel);
|
||||
|
||||
bool gps_nav_epoch_assembly_complete(
|
||||
const gps_nav_epoch_assembly_t* assembly);
|
||||
|
||||
#endif
|
||||
@@ -15,6 +15,7 @@ typedef struct gps_ephem_ll {
|
||||
volatile bool gps_how_anchor_valid;
|
||||
volatile uint32_t gps_nav_solve_sequence;
|
||||
volatile uint32_t gps_nav_solve_consumed_sequence;
|
||||
volatile gpstime_t gps_last_nav_solve_tow;
|
||||
volatile uint32_t gps_last_nav_solve_abs_sample;
|
||||
volatile double gps_last_nav_solve_corrected_abs_sample;
|
||||
volatile uint64_t gps_last_nav_solve_local_ticks;
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
#include "gps/linked_list.h"
|
||||
#include "gps/gps_interrupt_handlers.h"
|
||||
#include "gps/Navigation.h"
|
||||
#include "gps/gps_nav_epoch.h"
|
||||
#include "gps/gps_pps.h"
|
||||
#include "gps/gps_time_anchor.h"
|
||||
#include <bsp/irq.h>
|
||||
@@ -71,6 +72,7 @@ static volatile unsigned int gps_plic_repeat_count = 0;
|
||||
static volatile bool gps_acq_schedule_pending = true;
|
||||
static volatile bool gps_timer_maintenance_pending = true;
|
||||
static volatile bool gps_nav_solve_period_pending = false;
|
||||
static gps_nav_epoch_assembly_t gps_nav_epoch_assembly;
|
||||
static volatile gps_runtime_diagnostics_t gps_runtime_diagnostics;
|
||||
static unsigned int gps_acq_round_robin_prn = 0;
|
||||
static unsigned int gps_external_irq_id = 0u;
|
||||
@@ -104,6 +106,24 @@ static double gps_correct_abs_sample(uint32_t abs_sample, uint32_t rem_code_phas
|
||||
return (double)abs_sample - ((double)rem_code_phase / CODE_FREQ_BASIS);
|
||||
}
|
||||
|
||||
static uint32_t gps_nav_ready_channel_mask(void)
|
||||
{
|
||||
uint32_t ready_mask = 0u;
|
||||
for(unsigned int i = 0u ; i < GPS_TRCK_IP_COUNT ; i++)
|
||||
{
|
||||
uint32_t status = trck_ips[i]->STATUS_REG;
|
||||
uint32_t control = trck_ips[i]->CONTROL_REG;
|
||||
bool busy = (status & (1u << GPS_TRCK_BUSY_POS)) != 0u;
|
||||
bool nav_irq_enabled =
|
||||
(control & (1u << GPS_TRCK_NAV_SOLVE_INTR_EN_POS)) != 0u;
|
||||
if(tracking_allocated[i] && busy && nav_irq_enabled)
|
||||
{
|
||||
ready_mask |= 1u << i;
|
||||
}
|
||||
}
|
||||
return ready_mask;
|
||||
}
|
||||
|
||||
static uint64_t gps_systick_interval(void)
|
||||
{
|
||||
uint64_t interval = BSP_CLINT_TIMEBASE_HZ / GPS_SYSTICK_HZ;
|
||||
@@ -238,9 +258,10 @@ bool gps_take_nav_solve_period_ready(void)
|
||||
bool ready = gps_nav_solve_period_pending && pipeline_idle;
|
||||
if(ready)
|
||||
{
|
||||
/* One foreground attempt per hardware NAV period. A failed WLS is not
|
||||
* retried by an unrelated NAV-bit, acquisition or SysTick wake-up. */
|
||||
/* The complete epoch is handed to the foreground exactly once. Events
|
||||
* arriving while WLS runs start the next assembly. */
|
||||
gps_nav_solve_period_pending = false;
|
||||
gps_nav_epoch_assembly_reset(&gps_nav_epoch_assembly);
|
||||
}
|
||||
bsp_irq_restore(irq_state);
|
||||
return ready;
|
||||
@@ -292,6 +313,7 @@ static void gps_runtime_state_init(void)
|
||||
gps_acq_schedule_pending = true;
|
||||
gps_timer_maintenance_pending = true;
|
||||
gps_nav_solve_period_pending = false;
|
||||
gps_nav_epoch_assembly_reset(&gps_nav_epoch_assembly);
|
||||
memset((void*)&gps_runtime_diagnostics, 0, sizeof(gps_runtime_diagnostics));
|
||||
gps_runtime_diagnostics.last_connected_prn = GPS_PRN_INVALID;
|
||||
gps_runtime_diagnostics.last_lost_prn = GPS_PRN_INVALID;
|
||||
@@ -382,6 +404,10 @@ static bool gps_irq_to_tracking_channel(unsigned int irq_id, unsigned char* trck
|
||||
|
||||
void gps_interrupt_init(void)
|
||||
{
|
||||
/* A CPU-only GDB reset leaves the PLIC enable register untouched. Establish
|
||||
* ownership from a clean mask so an earlier IMU-enabled image cannot leave
|
||||
* PMOD IRQ 11 active in a GPS-only image. */
|
||||
bsp_plic_disable_all();
|
||||
gps_runtime_state_init();
|
||||
bsp_set_mtvec(gps_irq_handler);
|
||||
bsp_plic_set_threshold(0);
|
||||
@@ -885,6 +911,12 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event)
|
||||
unsigned char trck_ip_no = event->index;
|
||||
unsigned int dropped_prn = event->prn;
|
||||
|
||||
if(gps_nav_epoch_assembly_remove_channel(
|
||||
&gps_nav_epoch_assembly, trck_ip_no))
|
||||
{
|
||||
gps_nav_solve_period_pending = true;
|
||||
}
|
||||
|
||||
if(gps_prn_valid(dropped_prn))
|
||||
{
|
||||
(void)insert_acq_element(&acquisition_node, dropped_prn);
|
||||
@@ -1059,7 +1091,8 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
|
||||
gps_frame_parsed->gps_ephems.tow.sec = tow_sec;
|
||||
gps_frame_parsed->gps_ephems.abs_sample = event->corrected_abs_sample;
|
||||
node->gps_nav_solve_sequence += nav_solve_event_count;
|
||||
gps_nav_solve_period_pending = true;
|
||||
node->gps_last_nav_solve_tow.week = tow_week;
|
||||
node->gps_last_nav_solve_tow.sec = tow_sec;
|
||||
node->gps_last_nav_solve_abs_sample = event->abs_sample_raw;
|
||||
node->gps_last_nav_solve_corrected_abs_sample = event->corrected_abs_sample;
|
||||
node->gps_last_nav_solve_local_ticks = event->local_timestamp_ticks;
|
||||
@@ -1069,6 +1102,40 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
|
||||
node->gps_last_pps_capture_sequence = event->pps_capture_sequence;
|
||||
}
|
||||
gps_mark_node_fresh(node, event->status, event->tick);
|
||||
|
||||
uint64_t epoch_id;
|
||||
if(!gps_nav_epoch_id(tow_week, tow_sec,
|
||||
GPS_NAV_SOLVE_PERIOD_MS, &epoch_id))
|
||||
{
|
||||
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
|
||||
"nav solve has invalid epoch id: trk=%u week=%d tow=%.6f\r\n",
|
||||
trck_ip_no, tow_week, tow_sec);
|
||||
return;
|
||||
}
|
||||
gps_nav_epoch_event_result_t assembly_result =
|
||||
gps_nav_epoch_assembly_push(
|
||||
&gps_nav_epoch_assembly, epoch_id, trck_ip_no,
|
||||
gps_nav_ready_channel_mask());
|
||||
if(assembly_result == GPS_NAV_EPOCH_EVENT_COMPLETE)
|
||||
{
|
||||
gps_nav_solve_period_pending = true;
|
||||
}
|
||||
else if(assembly_result == GPS_NAV_EPOCH_EVENT_STALE)
|
||||
{
|
||||
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
|
||||
"stale nav epoch interrupt ignored: trk=%u epoch=%lu active=%lu\r\n",
|
||||
trck_ip_no, (unsigned long)epoch_id,
|
||||
(unsigned long)gps_nav_epoch_assembly.epoch_id);
|
||||
}
|
||||
else if(assembly_result == GPS_NAV_EPOCH_EVENT_FUTURE)
|
||||
{
|
||||
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
|
||||
"future nav epoch blocked by incomplete epoch: trk=%u epoch=%lu active=%lu expected=0x%x received=0x%x\r\n",
|
||||
trck_ip_no, (unsigned long)epoch_id,
|
||||
(unsigned long)gps_nav_epoch_assembly.epoch_id,
|
||||
gps_nav_epoch_assembly.expected_mask,
|
||||
gps_nav_epoch_assembly.received_mask);
|
||||
}
|
||||
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_TRK,
|
||||
"nav solve: trk=%u abs_raw=%u rem=%u abs=%.6f timer=%u count=%u dt=%.6f tow=%f seq=%u\r\n",
|
||||
trck_ip_no,
|
||||
|
||||
@@ -8,12 +8,16 @@
|
||||
#include "gps/gps_ip_defines.h"
|
||||
#include "gps/linked_list.h"
|
||||
#include "gps/gps_interrupt_handlers.h"
|
||||
#include "gps/gps_nav_epoch.h"
|
||||
#include "gps/gps_pps.h"
|
||||
#include "gps/defines.h"
|
||||
#include <bsp/irq.h>
|
||||
|
||||
#define GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_SAMPLES ((uint32_t)(SAMPLES_PER_CODE * (double)GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS))
|
||||
|
||||
_Static_assert(GPS_TRCK_IP_COUNT <= GPS_NAV_EPOCH_MAX_CANDIDATES,
|
||||
"GPS nav epoch selector capacity is smaller than the tracking channel count");
|
||||
|
||||
volatile gps_acq_t* const acq0_ip = (volatile gps_acq_t*)ACQ0_IP_BASE_ADDR;
|
||||
volatile gps_acq_t* acq_ips[GPS_ACQ_IP_COUNT];
|
||||
unsigned int acq_ips_intr_lines[GPS_ACQ_IP_COUNT];
|
||||
@@ -98,16 +102,6 @@ bool gps_get_ionoutc(ionoutc_t* ionoutc)
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool gps_abs_sample_after(uint32_t a, uint32_t b)
|
||||
{
|
||||
return ((int32_t)(a - b)) > 0;
|
||||
}
|
||||
|
||||
static uint32_t gps_abs_sample_delta(uint32_t newer, uint32_t older)
|
||||
{
|
||||
return newer - older;
|
||||
}
|
||||
|
||||
static double gps_unwrap_corrected_abs_sample(uint32_t raw_sample,
|
||||
double corrected_sample,
|
||||
uint32_t reference_sample)
|
||||
@@ -133,93 +127,8 @@ static bool gps_snapshot_has_svid(const gps_ephemeris_t snapshot[GPS_TRCK_IP_COU
|
||||
return false;
|
||||
}
|
||||
|
||||
static uint32_t gps_nav_tow_mismatch_total;
|
||||
static uint32_t gps_nav_tow_reject_total;
|
||||
|
||||
static double gps_snapshot_tow_delta_sec(const gpstime_t* first, const gpstime_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 gps_validate_snapshot_tow(const gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
|
||||
unsigned int satellite_count)
|
||||
{
|
||||
if((snapshot == NULL) || (satellite_count < MIN_NEEDED_SATELLITE))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
unsigned int reference_row = 0u;
|
||||
unsigned int reference_cluster_count = 0u;
|
||||
for(unsigned int candidate = 0u ; candidate < satellite_count ; candidate++)
|
||||
{
|
||||
unsigned int cluster_count = 0u;
|
||||
for(unsigned int row = 0u ; row < satellite_count ; row++)
|
||||
{
|
||||
double delta = gps_snapshot_tow_delta_sec(&snapshot[row].tow,
|
||||
&snapshot[candidate].tow);
|
||||
if(fabs(delta) <= GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC)
|
||||
{
|
||||
cluster_count++;
|
||||
}
|
||||
}
|
||||
if(cluster_count > reference_cluster_count)
|
||||
{
|
||||
reference_cluster_count = cluster_count;
|
||||
reference_row = candidate;
|
||||
}
|
||||
}
|
||||
|
||||
if(reference_cluster_count <= (satellite_count / 2u))
|
||||
{
|
||||
gps_nav_tow_reject_total++;
|
||||
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
|
||||
"NAV epoch rejected: no majority TOW cluster sats=%u best_cluster=%u rejects=%u\r\n",
|
||||
satellite_count, reference_cluster_count, gps_nav_tow_reject_total);
|
||||
return false;
|
||||
}
|
||||
|
||||
unsigned int mismatch_count = 0u;
|
||||
double maximum_mismatch_sec = 0.0;
|
||||
unsigned int maximum_mismatch_row = reference_row;
|
||||
for(unsigned int row = 0u ; row < satellite_count ; row++)
|
||||
{
|
||||
double delta = gps_snapshot_tow_delta_sec(&snapshot[row].tow,
|
||||
&snapshot[reference_row].tow);
|
||||
if(fabs(delta) <= GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
mismatch_count++;
|
||||
if(!isfinite(delta) || (fabs(delta) > fabs(maximum_mismatch_sec)))
|
||||
{
|
||||
maximum_mismatch_sec = delta;
|
||||
maximum_mismatch_row = row;
|
||||
}
|
||||
}
|
||||
if(mismatch_count != 0u)
|
||||
{
|
||||
gps_nav_tow_mismatch_total += mismatch_count;
|
||||
gps_nav_tow_reject_total++;
|
||||
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
|
||||
"NAV epoch rejected: TOW mismatch count=%u SVID=%u delta=%.6lf s reference_svid=%u mismatches=%u rejects=%u\r\n",
|
||||
mismatch_count, snapshot[maximum_mismatch_row].SVID,
|
||||
maximum_mismatch_sec, snapshot[reference_row].SVID,
|
||||
gps_nav_tow_mismatch_total, gps_nav_tow_reject_total);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
static uint32_t gps_nav_epoch_excluded_total;
|
||||
static uint32_t gps_nav_epoch_reject_total;
|
||||
|
||||
static int32_t gps_sign_extend(uint32_t value, unsigned int bits)
|
||||
{
|
||||
@@ -497,6 +406,7 @@ static void gps_stage_ephemeris_subframe(const unsigned int* subframe,
|
||||
gps_parse_ephemeris_subframe1(sf1, &frame->gps_ephems);
|
||||
gps_parse_ephemeris_subframe2(sf2, &frame->gps_ephems);
|
||||
gps_parse_ephemeris_subframe3(sf3, &frame->gps_ephems);
|
||||
frame->gps_ephems.ephemeris_valid = true;
|
||||
frame->pending_ephemeris_mask = 0u;
|
||||
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, "consistent ephemeris set committed: iode=%u\r\n", iode2);
|
||||
if(frame->gps_ephems.svhlth != 0u)
|
||||
@@ -591,10 +501,10 @@ static bool gps_tracking_ip_alive(unsigned char trck_ip_no)
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool gps_node_has_initial_frames(volatile gps_ephem_ll_t* node)
|
||||
static bool gps_node_has_committed_ephemeris(volatile gps_ephem_ll_t* node)
|
||||
{
|
||||
return (node != NULL) &&
|
||||
((node->gps_subframe_mask & GPS_INITIAL_SUBFRAME_MASK) == GPS_INITIAL_SUBFRAME_MASK);
|
||||
return (node != NULL) && (node->data_p != NULL) &&
|
||||
node->data_p->ephemeris_valid;
|
||||
}
|
||||
|
||||
static bool gps_node_ready_for_position(volatile gps_ephem_ll_t* node)
|
||||
@@ -604,7 +514,8 @@ static bool gps_node_ready_for_position(volatile gps_ephem_ll_t* node)
|
||||
return false;
|
||||
}
|
||||
|
||||
if(!gps_tracking_ip_alive(node->tracking_ip_no) || !gps_node_has_initial_frames(node))
|
||||
if(!gps_tracking_ip_alive(node->tracking_ip_no) ||
|
||||
!gps_node_has_committed_ephemeris(node))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
@@ -656,11 +567,11 @@ static void gps_enable_nav_solve_for_snapshot_locked(const unsigned char snapsho
|
||||
volatile gps_ephem_ll_t* node = get_element_by_ip_number(&tracking_node, ip_no);
|
||||
if((node != NULL) && gps_node_ready_for_position(node))
|
||||
{
|
||||
if(!gps_channel_nav_solve_enabled(ip_no))
|
||||
{
|
||||
gps_set_channel_nav_solve_mode(ip_no);
|
||||
node->gps_nav_solve_consumed_sequence = node->gps_nav_solve_sequence;
|
||||
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK,
|
||||
if(!gps_channel_nav_solve_enabled(ip_no))
|
||||
{
|
||||
gps_set_channel_nav_solve_mode(ip_no);
|
||||
node->gps_nav_solve_consumed_sequence = node->gps_nav_solve_sequence;
|
||||
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK,
|
||||
"tracking channel %u enabled nav solve; nav bits remain enabled\r\n",
|
||||
ip_no);
|
||||
}
|
||||
@@ -688,38 +599,11 @@ static unsigned int gps_collect_busy_tracking_locked(
|
||||
|
||||
static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
|
||||
unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
|
||||
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT],
|
||||
bool require_nav_solve_mode,
|
||||
bool require_nav_sequence)
|
||||
bool require_nav_sequence)
|
||||
{
|
||||
unsigned int count = 0;
|
||||
uint32_t newest_abs_sample = 0;
|
||||
bool have_newest_abs_sample = false;
|
||||
|
||||
if(require_nav_sequence)
|
||||
{
|
||||
for(volatile gps_ephem_ll_t* epoch_iterator = tracking_node;
|
||||
epoch_iterator != NULL;
|
||||
epoch_iterator = epoch_iterator->next)
|
||||
{
|
||||
unsigned char ip_no = epoch_iterator->tracking_ip_no;
|
||||
if(gps_node_ready_for_position(epoch_iterator) &&
|
||||
(!require_nav_solve_mode || gps_channel_nav_solve_enabled(ip_no)) &&
|
||||
(epoch_iterator->gps_nav_solve_sequence != epoch_iterator->gps_nav_solve_consumed_sequence))
|
||||
{
|
||||
if(!have_newest_abs_sample ||
|
||||
gps_abs_sample_after(epoch_iterator->gps_last_nav_solve_abs_sample, newest_abs_sample))
|
||||
{
|
||||
newest_abs_sample = epoch_iterator->gps_last_nav_solve_abs_sample;
|
||||
have_newest_abs_sample = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(!have_newest_abs_sample)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
for(volatile gps_ephem_ll_t* iterator = tracking_node;
|
||||
iterator != NULL;
|
||||
@@ -729,10 +613,10 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
|
||||
gps_ephemeris_t* eph = (gps_ephemeris_t*)iterator->data_p;
|
||||
if(gps_node_ready_for_position(iterator) &&
|
||||
(!require_nav_solve_mode || gps_channel_nav_solve_enabled(ip_no)) &&
|
||||
(!require_nav_sequence ||
|
||||
((eph != NULL) &&
|
||||
(iterator->gps_nav_solve_sequence != iterator->gps_nav_solve_consumed_sequence) &&
|
||||
(gps_abs_sample_delta(newest_abs_sample, iterator->gps_last_nav_solve_abs_sample) <= GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_SAMPLES))))
|
||||
(!require_nav_sequence ||
|
||||
((eph != NULL) &&
|
||||
(iterator->gps_nav_solve_sequence !=
|
||||
iterator->gps_nav_solve_consumed_sequence))))
|
||||
{
|
||||
if(gps_snapshot_has_svid(snapshot, count, eph->SVID))
|
||||
{
|
||||
@@ -742,17 +626,22 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
|
||||
continue;
|
||||
}
|
||||
snapshot[count] = *eph;
|
||||
if(require_nav_sequence)
|
||||
if(require_nav_sequence)
|
||||
{
|
||||
/* Nav bits may update the shared ephemeris between nav-solve
|
||||
* epochs. Position measurements must use nav-solve timing only. */
|
||||
snapshot[count].abs_sample = gps_unwrap_corrected_abs_sample(
|
||||
iterator->gps_last_nav_solve_abs_sample,
|
||||
iterator->gps_last_nav_solve_corrected_abs_sample,
|
||||
newest_abs_sample);
|
||||
* epochs. Position measurements must use the timing latched by
|
||||
* the nav-solve event, not the shared nav-bit fields. */
|
||||
snapshot[count].tow = iterator->gps_last_nav_solve_tow;
|
||||
snapshot[count].abs_sample =
|
||||
iterator->gps_last_nav_solve_corrected_abs_sample;
|
||||
if(snapshot_raw_samples != NULL)
|
||||
{
|
||||
snapshot_raw_samples[count] =
|
||||
iterator->gps_last_nav_solve_abs_sample;
|
||||
}
|
||||
}
|
||||
snapshot_ips[count] = ip_no;
|
||||
count++;
|
||||
count++;
|
||||
if(count >= GPS_TRCK_IP_COUNT)
|
||||
{
|
||||
break;
|
||||
@@ -762,6 +651,79 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
|
||||
return count;
|
||||
}
|
||||
|
||||
static unsigned int gps_select_coherent_nav_snapshot(
|
||||
gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
|
||||
unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
|
||||
uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
|
||||
uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
|
||||
uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
|
||||
uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
|
||||
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT],
|
||||
unsigned int candidate_count,
|
||||
uint32_t* selected_mask_out)
|
||||
{
|
||||
if((candidate_count == 0u) ||
|
||||
(candidate_count > GPS_NAV_EPOCH_MAX_CANDIDATES))
|
||||
{
|
||||
*selected_mask_out = 0u;
|
||||
return 0u;
|
||||
}
|
||||
|
||||
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES];
|
||||
for(unsigned int i = 0u ; i < candidate_count ; i++)
|
||||
{
|
||||
candidates[i].week = snapshot[i].tow.week;
|
||||
candidates[i].tow_sec = snapshot[i].tow.sec;
|
||||
candidates[i].raw_sample = snapshot_raw_samples[i];
|
||||
}
|
||||
|
||||
gps_nav_epoch_selection_t selection;
|
||||
if(!gps_nav_epoch_select(candidates, candidate_count,
|
||||
GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_SAMPLES,
|
||||
GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC,
|
||||
&selection))
|
||||
{
|
||||
*selected_mask_out = 0u;
|
||||
return 0u;
|
||||
}
|
||||
|
||||
uint32_t reference_sample =
|
||||
snapshot_raw_samples[selection.reference_index];
|
||||
unsigned int destination = 0u;
|
||||
for(unsigned int source = 0u ; source < candidate_count ; source++)
|
||||
{
|
||||
if((selection.selected_mask & (1u << source)) == 0u)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
double corrected_sample = snapshot[source].abs_sample;
|
||||
if(destination != source)
|
||||
{
|
||||
snapshot[destination] = snapshot[source];
|
||||
snapshot_ips[destination] = snapshot_ips[source];
|
||||
snapshot_sequences[destination] = snapshot_sequences[source];
|
||||
snapshot_pps_counters[destination] =
|
||||
snapshot_pps_counters[source];
|
||||
snapshot_pps_sequences[destination] =
|
||||
snapshot_pps_sequences[source];
|
||||
snapshot_local_timestamps[destination] =
|
||||
snapshot_local_timestamps[source];
|
||||
snapshot_raw_samples[destination] =
|
||||
snapshot_raw_samples[source];
|
||||
}
|
||||
snapshot[destination].abs_sample =
|
||||
gps_unwrap_corrected_abs_sample(
|
||||
snapshot_raw_samples[destination],
|
||||
corrected_sample,
|
||||
reference_sample);
|
||||
destination++;
|
||||
}
|
||||
|
||||
*selected_mask_out = selection.selected_mask;
|
||||
return destination;
|
||||
}
|
||||
|
||||
static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
|
||||
uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
|
||||
uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
|
||||
@@ -819,6 +781,32 @@ static void gps_consume_nav_sequences_locked(const unsigned char snapshot_ips[GP
|
||||
}
|
||||
}
|
||||
|
||||
static void gps_consume_rejected_nav_sequences_locked(
|
||||
const unsigned char candidate_ips[GPS_TRCK_IP_COUNT],
|
||||
const uint32_t candidate_sequences[GPS_TRCK_IP_COUNT],
|
||||
unsigned int candidate_count,
|
||||
uint32_t selected_mask)
|
||||
{
|
||||
for(unsigned int i = 0u ; i < candidate_count ; i++)
|
||||
{
|
||||
if((selected_mask & (1u << i)) != 0u)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
volatile gps_ephem_ll_t* node =
|
||||
get_element_by_ip_number(&tracking_node, candidate_ips[i]);
|
||||
if(node != NULL)
|
||||
{
|
||||
/*
|
||||
* Consume exactly the rejected snapshot. If an interrupt has
|
||||
* already produced a newer sequence, it remains different from
|
||||
* this consumed value and is preserved for the next epoch.
|
||||
*/
|
||||
node->gps_nav_solve_consumed_sequence = candidate_sequences[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
|
||||
const uint64_t pps_counters[GPS_TRCK_IP_COUNT],
|
||||
const uint32_t pps_capture_sequences[GPS_TRCK_IP_COUNT],
|
||||
@@ -1104,7 +1092,10 @@ bool calculate_position(gps_nav_solution_t* solution_out,
|
||||
uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT];
|
||||
uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT];
|
||||
uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT];
|
||||
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT];
|
||||
unsigned int satellite_count = 0;
|
||||
unsigned int candidate_count = 0;
|
||||
uint32_t selected_candidate_mask = 0u;
|
||||
bool run_nav = false;
|
||||
bool initial_run = false;
|
||||
unsigned int ready_count;
|
||||
@@ -1124,7 +1115,8 @@ bool calculate_position(gps_nav_solution_t* solution_out,
|
||||
|
||||
uint64_t irq_state = bsp_irq_save();
|
||||
|
||||
ready_count = gps_collect_nav_snapshot_locked(snapshot, snapshot_ips, false, false);
|
||||
ready_count = gps_collect_nav_snapshot_locked(
|
||||
snapshot, snapshot_ips, NULL, false, false);
|
||||
busy_count = gps_collect_busy_tracking_locked(
|
||||
GPS_DEBUG_ENABLED(DEBUG_CHANNELS) ? tracked_prns : NULL);
|
||||
|
||||
@@ -1139,25 +1131,71 @@ bool calculate_position(gps_nav_solution_t* solution_out,
|
||||
gps_enable_nav_solve_for_snapshot_locked(snapshot_ips, ready_count);
|
||||
if(nav_period_ready)
|
||||
{
|
||||
satellite_count = gps_collect_nav_snapshot_locked(snapshot, snapshot_ips, true, true);
|
||||
coherent_nav_count = satellite_count;
|
||||
bool tow_coherent = (satellite_count >= MIN_NEEDED_SATELLITE) &&
|
||||
(satellite_count >= ready_count) &&
|
||||
gps_validate_snapshot_tow(snapshot, satellite_count);
|
||||
run_nav = tow_coherent;
|
||||
candidate_count = gps_collect_nav_snapshot_locked(
|
||||
snapshot, snapshot_ips, snapshot_raw_samples, true, true);
|
||||
gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences,
|
||||
snapshot_pps_counters,
|
||||
snapshot_pps_sequences,
|
||||
snapshot_local_timestamps,
|
||||
candidate_count);
|
||||
initial_run = !gps_initial_position_valid;
|
||||
if(run_nav)
|
||||
{
|
||||
gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences,
|
||||
snapshot_pps_counters, snapshot_pps_sequences,
|
||||
snapshot_local_timestamps,
|
||||
satellite_count);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bsp_irq_restore(irq_state);
|
||||
|
||||
if(nav_period_ready && (candidate_count != 0u))
|
||||
{
|
||||
/*
|
||||
* Preserve the pre-compaction identity of rejected candidates so their
|
||||
* exact sequence can be retired without touching a newer interrupt.
|
||||
*/
|
||||
unsigned char candidate_ips[GPS_TRCK_IP_COUNT];
|
||||
uint32_t candidate_sequences[GPS_TRCK_IP_COUNT];
|
||||
memcpy(candidate_ips, snapshot_ips,
|
||||
candidate_count * sizeof(candidate_ips[0]));
|
||||
memcpy(candidate_sequences, snapshot_sequences,
|
||||
candidate_count * sizeof(candidate_sequences[0]));
|
||||
|
||||
satellite_count = gps_select_coherent_nav_snapshot(
|
||||
snapshot, snapshot_ips, snapshot_sequences,
|
||||
snapshot_pps_counters, snapshot_pps_sequences,
|
||||
snapshot_local_timestamps, snapshot_raw_samples,
|
||||
candidate_count, &selected_candidate_mask);
|
||||
coherent_nav_count = satellite_count;
|
||||
run_nav = satellite_count >= MIN_NEEDED_SATELLITE;
|
||||
|
||||
if(run_nav && (satellite_count < candidate_count))
|
||||
{
|
||||
unsigned int excluded_count = candidate_count - satellite_count;
|
||||
gps_nav_epoch_excluded_total += excluded_count;
|
||||
uint64_t consume_irq_state = bsp_irq_save();
|
||||
gps_consume_rejected_nav_sequences_locked(
|
||||
candidate_ips, candidate_sequences, candidate_count,
|
||||
selected_candidate_mask);
|
||||
bsp_irq_restore(consume_irq_state);
|
||||
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
|
||||
"NAV coherent subset selected: candidates=%u selected=%u excluded=%u excluded_total=%u\r\n",
|
||||
candidate_count, satellite_count, excluded_count,
|
||||
gps_nav_epoch_excluded_total);
|
||||
}
|
||||
else if(!run_nav)
|
||||
{
|
||||
uint64_t consume_irq_state = bsp_irq_save();
|
||||
gps_consume_rejected_nav_sequences_locked(
|
||||
candidate_ips, candidate_sequences, candidate_count, 0u);
|
||||
bsp_irq_restore(consume_irq_state);
|
||||
if(candidate_count >= MIN_NEEDED_SATELLITE)
|
||||
{
|
||||
gps_nav_epoch_reject_total++;
|
||||
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
|
||||
"NAV epoch has no coherent four-satellite subset: candidates=%u best=%u rejects=%u\r\n",
|
||||
candidate_count, satellite_count,
|
||||
gps_nav_epoch_reject_total);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
uint64_t now = gps_get_systick();
|
||||
if((busy_count != last_nav_status_busy_count) ||
|
||||
(ready_count != last_nav_status_ready_count) ||
|
||||
@@ -1202,10 +1240,14 @@ bool calculate_position(gps_nav_solution_t* solution_out,
|
||||
*measurement_timestamp_ticks_out = measurement_timestamp;
|
||||
result_ready = true;
|
||||
}
|
||||
uint64_t consume_irq_state = bsp_irq_save();
|
||||
gps_consume_nav_sequences_locked(snapshot_ips, snapshot_sequences, satellite_count);
|
||||
bsp_irq_restore(consume_irq_state);
|
||||
}
|
||||
/* A completed hardware epoch is attempted once. Retrying the same
|
||||
* measurements after a WLS failure would couple solution cadence to an
|
||||
* unrelated foreground wake-up. */
|
||||
uint64_t consume_irq_state = bsp_irq_save();
|
||||
gps_consume_nav_sequences_locked(
|
||||
snapshot_ips, snapshot_sequences, satellite_count);
|
||||
bsp_irq_restore(consume_irq_state);
|
||||
uint64_t foreground_cycles = gps_read_cycle_counter() - nav_period_start_cycles;
|
||||
uint64_t period_budget_cycles = (GPS_PROFILE_CPU_HZ * GPS_NAV_SOLVE_PERIOD_MS) / 1000u;
|
||||
bool period_overrun = foreground_cycles >= period_budget_cycles;
|
||||
@@ -1218,7 +1260,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
|
||||
"fde=%u valid=%u budget_cycles=%lu overrun=%u math_corr=%.6lf "
|
||||
"math_clock_corr=%.6lf "
|
||||
"isr_overflow=%u plic_repeat=%u drain_limit=%u coalesced=%u lost=%u "
|
||||
"tow_mismatch=%u tow_rejected=%u\r\n",
|
||||
"epoch_excluded=%u epoch_rejected=%u\r\n",
|
||||
GPS_NAV_SOLVE_PERIOD_MS,
|
||||
(unsigned long)GPS_PROFILE_CPU_HZ,
|
||||
(unsigned long)foreground_cycles,
|
||||
@@ -1241,8 +1283,8 @@ bool calculate_position(gps_nav_solution_t* solution_out,
|
||||
runtime_diagnostics.plic_drain_limit_total,
|
||||
runtime_diagnostics.nav_epoch_coalesced_total,
|
||||
runtime_diagnostics.satellite_lost_total,
|
||||
gps_nav_tow_mismatch_total,
|
||||
gps_nav_tow_reject_total);
|
||||
gps_nav_epoch_excluded_total,
|
||||
gps_nav_epoch_reject_total);
|
||||
(void)foreground_cycles;
|
||||
(void)nav_software_cycles;
|
||||
(void)period_budget_cycles;
|
||||
@@ -1384,8 +1426,8 @@ void gps_startup_initialization(unsigned int nsp_tick)
|
||||
gps_pps_init(nsp_tick);
|
||||
gps_ionoutc_valid = false;
|
||||
gps_last_ionoutc = (ionoutc_t){0};
|
||||
gps_nav_tow_mismatch_total = 0u;
|
||||
gps_nav_tow_reject_total = 0u;
|
||||
gps_nav_epoch_excluded_total = 0u;
|
||||
gps_nav_epoch_reject_total = 0u;
|
||||
for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
|
||||
{
|
||||
acq_ips[i]->CONTROL_REG = (1u << GPS_ACQ_INTPRT_CLR);
|
||||
|
||||
@@ -0,0 +1,301 @@
|
||||
#include "gps/gps_nav_epoch.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#define GPS_WEEK_SECONDS 604800.0
|
||||
#define GPS_HALF_WEEK_SECONDS 302400.0
|
||||
#define GPS_WEEK_MILLISECONDS 604800000ULL
|
||||
|
||||
static double gps_nav_epoch_tow_delta(
|
||||
const gps_nav_epoch_key_t* first,
|
||||
const gps_nav_epoch_key_t* second)
|
||||
{
|
||||
double delta = (((double)first->week - (double)second->week) *
|
||||
GPS_WEEK_SECONDS) +
|
||||
first->tow_sec - second->tow_sec;
|
||||
if(delta > GPS_HALF_WEEK_SECONDS)
|
||||
{
|
||||
delta -= GPS_WEEK_SECONDS;
|
||||
}
|
||||
else if(delta < -GPS_HALF_WEEK_SECONDS)
|
||||
{
|
||||
delta += GPS_WEEK_SECONDS;
|
||||
}
|
||||
return delta;
|
||||
}
|
||||
|
||||
static uint32_t gps_nav_epoch_sample_distance(uint32_t first, uint32_t second)
|
||||
{
|
||||
uint32_t forward = first - second;
|
||||
uint32_t reverse = second - first;
|
||||
return (forward < reverse) ? forward : reverse;
|
||||
}
|
||||
|
||||
static bool gps_nav_epoch_sample_after(uint32_t first, uint32_t second)
|
||||
{
|
||||
uint32_t delta = first - second;
|
||||
return (delta != 0u) && (delta < 0x80000000u);
|
||||
}
|
||||
|
||||
static unsigned int gps_nav_epoch_mask_count(uint32_t mask)
|
||||
{
|
||||
unsigned int count = 0u;
|
||||
while(mask != 0u)
|
||||
{
|
||||
count += mask & 1u;
|
||||
mask >>= 1u;
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
static bool gps_nav_epoch_candidate_valid(const gps_nav_epoch_key_t* candidate)
|
||||
{
|
||||
return (candidate != NULL) &&
|
||||
isfinite(candidate->tow_sec) &&
|
||||
(candidate->tow_sec >= 0.0) &&
|
||||
(candidate->tow_sec < GPS_WEEK_SECONDS);
|
||||
}
|
||||
|
||||
static bool gps_nav_epoch_subset_coherent(
|
||||
const gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES],
|
||||
unsigned int candidate_count,
|
||||
uint32_t subset_mask,
|
||||
uint32_t sample_window,
|
||||
double tow_tolerance_sec,
|
||||
unsigned int* reference_index)
|
||||
{
|
||||
unsigned int reference = GPS_NAV_EPOCH_MAX_CANDIDATES;
|
||||
for(unsigned int first = 0u ; first < candidate_count ; first++)
|
||||
{
|
||||
if((subset_mask & (1u << first)) == 0u)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
if(!gps_nav_epoch_candidate_valid(&candidates[first]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if((reference == GPS_NAV_EPOCH_MAX_CANDIDATES) ||
|
||||
gps_nav_epoch_sample_after(candidates[first].raw_sample,
|
||||
candidates[reference].raw_sample))
|
||||
{
|
||||
reference = first;
|
||||
}
|
||||
|
||||
for(unsigned int second = first + 1u ; second < candidate_count ; second++)
|
||||
{
|
||||
if((subset_mask & (1u << second)) == 0u)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
double tow_delta = gps_nav_epoch_tow_delta(
|
||||
&candidates[first], &candidates[second]);
|
||||
if(!isfinite(tow_delta) ||
|
||||
(fabs(tow_delta) > tow_tolerance_sec) ||
|
||||
(gps_nav_epoch_sample_distance(candidates[first].raw_sample,
|
||||
candidates[second].raw_sample) >
|
||||
sample_window))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(reference == GPS_NAV_EPOCH_MAX_CANDIDATES)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
*reference_index = reference;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool gps_nav_epoch_select(
|
||||
const gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES],
|
||||
unsigned int candidate_count,
|
||||
uint32_t sample_window,
|
||||
double tow_tolerance_sec,
|
||||
gps_nav_epoch_selection_t* selection)
|
||||
{
|
||||
if((candidates == NULL) || (selection == NULL) ||
|
||||
(candidate_count == 0u) ||
|
||||
(candidate_count > GPS_NAV_EPOCH_MAX_CANDIDATES) ||
|
||||
(sample_window == 0u) ||
|
||||
!isfinite(tow_tolerance_sec) ||
|
||||
(tow_tolerance_sec < 0.0))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
selection->selected_mask = 0u;
|
||||
selection->selected_count = 0u;
|
||||
selection->reference_index = 0u;
|
||||
|
||||
uint32_t subset_limit = 1u << candidate_count;
|
||||
for(uint32_t subset = 1u ; subset < subset_limit ; subset++)
|
||||
{
|
||||
unsigned int subset_count = gps_nav_epoch_mask_count(subset);
|
||||
if(subset_count < selection->selected_count)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
unsigned int reference;
|
||||
if(!gps_nav_epoch_subset_coherent(candidates, candidate_count, subset,
|
||||
sample_window, tow_tolerance_sec,
|
||||
&reference))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
bool select_subset = subset_count > selection->selected_count;
|
||||
if(!select_subset && (selection->selected_count != 0u))
|
||||
{
|
||||
/*
|
||||
* Equal-sized disjoint clusters represent different nav epochs.
|
||||
* Prefer the newer raw-sample epoch. Overlapping equal-sized
|
||||
* subsets have the same/new-near reference and retain deterministic
|
||||
* mask order.
|
||||
*/
|
||||
uint32_t reference_delta =
|
||||
candidates[reference].raw_sample -
|
||||
candidates[selection->reference_index].raw_sample;
|
||||
if(gps_nav_epoch_sample_after(
|
||||
candidates[reference].raw_sample,
|
||||
candidates[selection->reference_index].raw_sample) &&
|
||||
(reference_delta > sample_window))
|
||||
{
|
||||
select_subset = true;
|
||||
}
|
||||
}
|
||||
|
||||
if(select_subset)
|
||||
{
|
||||
selection->selected_mask = subset;
|
||||
selection->selected_count = subset_count;
|
||||
selection->reference_index = reference;
|
||||
}
|
||||
}
|
||||
|
||||
return selection->selected_count != 0u;
|
||||
}
|
||||
|
||||
bool gps_nav_epoch_id(
|
||||
int week,
|
||||
double tow_sec,
|
||||
unsigned int period_ms,
|
||||
uint64_t* epoch_id)
|
||||
{
|
||||
if((epoch_id == NULL) || (week < 0) || (period_ms == 0u) ||
|
||||
((GPS_WEEK_MILLISECONDS % period_ms) != 0u) ||
|
||||
!isfinite(tow_sec) || (tow_sec < 0.0) ||
|
||||
(tow_sec >= GPS_WEEK_SECONDS))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
uint64_t epochs_per_week = GPS_WEEK_MILLISECONDS / period_ms;
|
||||
double epoch_in_week = (tow_sec * 1000.0) / (double)period_ms;
|
||||
uint64_t rounded_epoch = (uint64_t)llround(epoch_in_week);
|
||||
if(rounded_epoch == epochs_per_week)
|
||||
{
|
||||
week++;
|
||||
rounded_epoch = 0u;
|
||||
}
|
||||
*epoch_id = ((uint64_t)(unsigned int)week * epochs_per_week) +
|
||||
rounded_epoch;
|
||||
return true;
|
||||
}
|
||||
|
||||
void gps_nav_epoch_assembly_reset(gps_nav_epoch_assembly_t* assembly)
|
||||
{
|
||||
if(assembly == NULL)
|
||||
{
|
||||
return;
|
||||
}
|
||||
assembly->epoch_id = 0u;
|
||||
assembly->expected_mask = 0u;
|
||||
assembly->received_mask = 0u;
|
||||
assembly->active = false;
|
||||
}
|
||||
|
||||
bool gps_nav_epoch_assembly_complete(
|
||||
const gps_nav_epoch_assembly_t* assembly)
|
||||
{
|
||||
return (assembly != NULL) && assembly->active &&
|
||||
(assembly->expected_mask != 0u) &&
|
||||
((assembly->received_mask & assembly->expected_mask) ==
|
||||
assembly->expected_mask);
|
||||
}
|
||||
|
||||
gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
|
||||
gps_nav_epoch_assembly_t* assembly,
|
||||
uint64_t epoch_id,
|
||||
unsigned int channel,
|
||||
uint32_t ready_mask)
|
||||
{
|
||||
if((assembly == NULL) ||
|
||||
(channel >= GPS_NAV_EPOCH_MAX_CANDIDATES))
|
||||
{
|
||||
return GPS_NAV_EPOCH_EVENT_INVALID;
|
||||
}
|
||||
|
||||
uint32_t channel_mask = 1u << channel;
|
||||
if(!assembly->active)
|
||||
{
|
||||
uint32_t valid_mask =
|
||||
(1u << GPS_NAV_EPOCH_MAX_CANDIDATES) - 1u;
|
||||
ready_mask &= valid_mask;
|
||||
if((ready_mask & channel_mask) == 0u)
|
||||
{
|
||||
return GPS_NAV_EPOCH_EVENT_INVALID;
|
||||
}
|
||||
assembly->epoch_id = epoch_id;
|
||||
assembly->expected_mask = ready_mask;
|
||||
assembly->received_mask = 0u;
|
||||
assembly->active = true;
|
||||
}
|
||||
|
||||
if(epoch_id < assembly->epoch_id)
|
||||
{
|
||||
return GPS_NAV_EPOCH_EVENT_STALE;
|
||||
}
|
||||
if(epoch_id > assembly->epoch_id)
|
||||
{
|
||||
return GPS_NAV_EPOCH_EVENT_FUTURE;
|
||||
}
|
||||
if((assembly->expected_mask & channel_mask) == 0u)
|
||||
{
|
||||
return GPS_NAV_EPOCH_EVENT_INVALID;
|
||||
}
|
||||
if((assembly->received_mask & channel_mask) != 0u)
|
||||
{
|
||||
return GPS_NAV_EPOCH_EVENT_DUPLICATE;
|
||||
}
|
||||
|
||||
assembly->received_mask |= channel_mask;
|
||||
return gps_nav_epoch_assembly_complete(assembly) ?
|
||||
GPS_NAV_EPOCH_EVENT_COMPLETE :
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED;
|
||||
}
|
||||
|
||||
bool gps_nav_epoch_assembly_remove_channel(
|
||||
gps_nav_epoch_assembly_t* assembly,
|
||||
unsigned int channel)
|
||||
{
|
||||
if((assembly == NULL) || !assembly->active ||
|
||||
(channel >= GPS_NAV_EPOCH_MAX_CANDIDATES))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t channel_mask = 1u << channel;
|
||||
assembly->expected_mask &= ~channel_mask;
|
||||
assembly->received_mask &= assembly->expected_mask;
|
||||
if(assembly->expected_mask == 0u)
|
||||
{
|
||||
gps_nav_epoch_assembly_reset(assembly);
|
||||
return false;
|
||||
}
|
||||
return gps_nav_epoch_assembly_complete(assembly);
|
||||
}
|
||||
@@ -68,6 +68,8 @@ bool insert_element(volatile gps_ephem_ll_t** node,
|
||||
new_node->gps_how_anchor_valid = false;
|
||||
new_node->gps_nav_solve_sequence = 0u;
|
||||
new_node->gps_nav_solve_consumed_sequence = 0u;
|
||||
new_node->gps_last_nav_solve_tow.week = 0;
|
||||
new_node->gps_last_nav_solve_tow.sec = 0.0;
|
||||
new_node->gps_last_nav_solve_abs_sample = 0u;
|
||||
new_node->gps_last_nav_solve_corrected_abs_sample = 0.0;
|
||||
new_node->gps_last_nav_solve_local_ticks = 0u;
|
||||
|
||||
@@ -2,17 +2,25 @@ CC ?= gcc
|
||||
CFLAGS ?= -O2 -std=c11 -Wall -Wextra -Werror
|
||||
|
||||
ROOT := ..
|
||||
TARGET := test_imu_calibration
|
||||
TARGETS := test_imu_calibration test_gps_nav_epoch test_plic_warm_reset
|
||||
|
||||
.PHONY: all run clean
|
||||
|
||||
all: $(TARGET)
|
||||
all: $(TARGETS)
|
||||
|
||||
$(TARGET): test_imu_calibration.c $(ROOT)/src/imu/imu_calibration.c
|
||||
test_imu_calibration: test_imu_calibration.c $(ROOT)/src/imu/imu_calibration.c
|
||||
$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
run: $(TARGET)
|
||||
./$(TARGET)
|
||||
test_gps_nav_epoch: test_gps_nav_epoch.c $(ROOT)/src/gps/gps_nav_epoch.c
|
||||
$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
|
||||
|
||||
test_plic_warm_reset: test_plic_warm_reset.c $(ROOT)/../bsp/drivers/plic.c
|
||||
$(CC) $(CFLAGS) -Ifake_bsp -I$(ROOT)/../bsp/include $^ -o $@
|
||||
|
||||
run: $(TARGETS)
|
||||
./test_imu_calibration
|
||||
./test_gps_nav_epoch
|
||||
./test_plic_warm_reset
|
||||
|
||||
clean:
|
||||
rm -f $(TARGET)
|
||||
rm -f $(TARGETS)
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
#ifndef TEST_BSP_MMIO_H
|
||||
#define TEST_BSP_MMIO_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
uint32_t test_bsp_read32(uintptr_t address);
|
||||
void test_bsp_write32(uintptr_t address, uint32_t value);
|
||||
|
||||
static inline uint32_t bsp_read32(uintptr_t address)
|
||||
{
|
||||
return test_bsp_read32(address);
|
||||
}
|
||||
|
||||
static inline void bsp_write32(uintptr_t address, uint32_t value)
|
||||
{
|
||||
test_bsp_write32(address, value);
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,6 @@
|
||||
#ifndef TEST_BSP_SOC_H
|
||||
#define TEST_BSP_SOC_H
|
||||
|
||||
#define BSP_PLIC_BASE 0x0c000000UL
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,475 @@
|
||||
#include "gps/gps_nav_epoch.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#define SAMPLE_WINDOW 409200u
|
||||
#define TOW_TOLERANCE 0.001
|
||||
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
static gps_nav_epoch_selection_t select_epoch(
|
||||
const gps_nav_epoch_key_t* candidates,
|
||||
unsigned int count)
|
||||
{
|
||||
gps_nav_epoch_selection_t selection;
|
||||
memset(&selection, 0, sizeof(selection));
|
||||
CHECK(gps_nav_epoch_select(candidates, count, SAMPLE_WINDOW,
|
||||
TOW_TOLERANCE, &selection),
|
||||
"epoch selection call failed");
|
||||
return selection;
|
||||
}
|
||||
|
||||
static unsigned int mask_count(uint32_t mask)
|
||||
{
|
||||
unsigned int count = 0u;
|
||||
while(mask != 0u)
|
||||
{
|
||||
count += mask & 1u;
|
||||
mask >>= 1u;
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
static void epoch_id_rounding_and_week_wrap(void)
|
||||
{
|
||||
uint64_t before_wrap;
|
||||
uint64_t after_wrap;
|
||||
uint64_t same_epoch_a;
|
||||
uint64_t same_epoch_b;
|
||||
CHECK(gps_nav_epoch_id(2300, 604799.9999, 200u, &before_wrap),
|
||||
"epoch id rejected a valid pre-wrap TOW");
|
||||
CHECK(gps_nav_epoch_id(2301, 0.0001, 200u, &after_wrap),
|
||||
"epoch id rejected a valid post-wrap TOW");
|
||||
CHECK(before_wrap == after_wrap,
|
||||
"week rollover split one rounded navigation epoch");
|
||||
CHECK(gps_nav_epoch_id(2301, 123.3997, 200u, &same_epoch_a) &&
|
||||
gps_nav_epoch_id(2301, 123.4003, 200u, &same_epoch_b) &&
|
||||
(same_epoch_a == same_epoch_b),
|
||||
"sub-millisecond TOW skew split one navigation epoch");
|
||||
CHECK(!gps_nav_epoch_id(2301, NAN, 200u, &same_epoch_a),
|
||||
"non-finite TOW produced an epoch id");
|
||||
CHECK(!gps_nav_epoch_id(2301, 1.0, 333u, &same_epoch_a),
|
||||
"non-integral epochs per GPS week were accepted");
|
||||
}
|
||||
|
||||
static void assembly_waits_for_every_ready_channel(void)
|
||||
{
|
||||
gps_nav_epoch_assembly_t assembly;
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
const uint32_t ready_mask = 0xafu;
|
||||
const unsigned int order[] = {7u, 0u, 5u, 3u, 2u, 1u};
|
||||
for(unsigned int i = 0u ; i < sizeof(order) / sizeof(order[0]) ; i++)
|
||||
{
|
||||
gps_nav_epoch_event_result_t result =
|
||||
gps_nav_epoch_assembly_push(
|
||||
&assembly, 1000u, order[i], ready_mask);
|
||||
bool last = i + 1u == sizeof(order) / sizeof(order[0]);
|
||||
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) == last,
|
||||
"assembly completed before every ready channel arrived");
|
||||
}
|
||||
CHECK(assembly.expected_mask == ready_mask,
|
||||
"assembly changed its frozen ready mask");
|
||||
CHECK(assembly.received_mask == ready_mask,
|
||||
"assembly lost a ready-channel interrupt");
|
||||
}
|
||||
|
||||
static void assembly_handles_duplicates_and_epoch_order(void)
|
||||
{
|
||||
gps_nav_epoch_assembly_t assembly;
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
CHECK(gps_nav_epoch_assembly_push(&assembly, 2000u, 0u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"first channel did not start assembly");
|
||||
CHECK(gps_nav_epoch_assembly_push(&assembly, 2000u, 0u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_DUPLICATE,
|
||||
"duplicate channel interrupt was not idempotent");
|
||||
CHECK(gps_nav_epoch_assembly_push(&assembly, 1999u, 1u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_STALE,
|
||||
"stale epoch interrupt was not rejected");
|
||||
CHECK(gps_nav_epoch_assembly_push(&assembly, 2001u, 1u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_FUTURE,
|
||||
"future epoch was mixed into an incomplete epoch");
|
||||
CHECK(assembly.received_mask == 0x01u,
|
||||
"rejected interrupts changed the received mask");
|
||||
}
|
||||
|
||||
static void assembly_disconnect_and_reconnect(void)
|
||||
{
|
||||
gps_nav_epoch_assembly_t assembly;
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 0u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"disconnect scenario did not start");
|
||||
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 1u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"second interrupt unexpectedly completed");
|
||||
CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 4u),
|
||||
"pending-channel disconnect completed too early");
|
||||
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 2u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"post-disconnect interrupt was rejected");
|
||||
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 3u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_COMPLETE,
|
||||
"unlock did not release the removed channel from the epoch");
|
||||
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
CHECK(gps_nav_epoch_assembly_push(&assembly, 3001u, 0u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"reconnect epoch did not start");
|
||||
for(unsigned int channel = 1u ; channel < 5u ; channel++)
|
||||
{
|
||||
gps_nav_epoch_event_result_t result =
|
||||
gps_nav_epoch_assembly_push(
|
||||
&assembly, 3001u, channel, 0x1fu);
|
||||
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
|
||||
(channel == 4u),
|
||||
"reconnected channel was not required by the next epoch");
|
||||
}
|
||||
}
|
||||
|
||||
static void channel_join_does_not_expand_active_epoch(void)
|
||||
{
|
||||
gps_nav_epoch_assembly_t assembly;
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
CHECK(gps_nav_epoch_assembly_push(&assembly, 4000u, 0u, 0x0fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
||||
"join scenario did not start");
|
||||
CHECK(gps_nav_epoch_assembly_push(&assembly, 4000u, 4u, 0x1fu) ==
|
||||
GPS_NAV_EPOCH_EVENT_INVALID,
|
||||
"joining channel changed the active epoch");
|
||||
CHECK(assembly.expected_mask == 0x0fu,
|
||||
"active expected mask expanded after a channel joined");
|
||||
for(unsigned int channel = 1u ; channel < 4u ; channel++)
|
||||
{
|
||||
gps_nav_epoch_event_result_t result =
|
||||
gps_nav_epoch_assembly_push(
|
||||
&assembly, 4000u, channel, 0x1fu);
|
||||
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
|
||||
(channel == 3u),
|
||||
"original ready set did not complete after a join");
|
||||
}
|
||||
}
|
||||
|
||||
static uint32_t deterministic_random(uint32_t* state)
|
||||
{
|
||||
uint32_t value = *state;
|
||||
value ^= value << 13;
|
||||
value ^= value >> 17;
|
||||
value ^= value << 5;
|
||||
*state = value;
|
||||
return value;
|
||||
}
|
||||
|
||||
static void randomized_interrupt_and_unlock_model(void)
|
||||
{
|
||||
uint32_t random_state = 0x6a09e667u;
|
||||
for(unsigned int trial = 0u ; trial < 10000u ; trial++)
|
||||
{
|
||||
unsigned int channel_count =
|
||||
4u + (deterministic_random(&random_state) % 6u);
|
||||
uint32_t ready_mask = (1u << channel_count) - 1u;
|
||||
unsigned int order[GPS_NAV_EPOCH_MAX_CANDIDATES];
|
||||
for(unsigned int i = 0u ; i < channel_count ; i++)
|
||||
{
|
||||
order[i] = i;
|
||||
}
|
||||
for(unsigned int i = channel_count ; i > 1u ; i--)
|
||||
{
|
||||
unsigned int swap =
|
||||
deterministic_random(&random_state) % i;
|
||||
unsigned int temporary = order[i - 1u];
|
||||
order[i - 1u] = order[swap];
|
||||
order[swap] = temporary;
|
||||
}
|
||||
|
||||
bool disconnect =
|
||||
(deterministic_random(&random_state) & 3u) == 0u;
|
||||
unsigned int dropped_channel = disconnect ?
|
||||
(deterministic_random(&random_state) % channel_count) :
|
||||
GPS_NAV_EPOCH_MAX_CANDIDATES;
|
||||
uint32_t final_mask = ready_mask;
|
||||
if(disconnect)
|
||||
{
|
||||
final_mask &= ~(1u << dropped_channel);
|
||||
}
|
||||
|
||||
gps_nav_epoch_assembly_t assembly;
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
bool complete = false;
|
||||
for(unsigned int i = 0u ; i < channel_count ; i++)
|
||||
{
|
||||
unsigned int channel = order[i];
|
||||
if(channel == dropped_channel)
|
||||
{
|
||||
complete = gps_nav_epoch_assembly_remove_channel(
|
||||
&assembly, channel);
|
||||
continue;
|
||||
}
|
||||
gps_nav_epoch_event_result_t result =
|
||||
gps_nav_epoch_assembly_push(
|
||||
&assembly, 5000u + trial, channel, ready_mask);
|
||||
if((deterministic_random(&random_state) & 7u) == 0u)
|
||||
{
|
||||
CHECK(gps_nav_epoch_assembly_push(
|
||||
&assembly, 5000u + trial, channel,
|
||||
ready_mask) ==
|
||||
GPS_NAV_EPOCH_EVENT_DUPLICATE,
|
||||
"random duplicate interrupt was not idempotent");
|
||||
}
|
||||
complete = result == GPS_NAV_EPOCH_EVENT_COMPLETE;
|
||||
}
|
||||
if(disconnect &&
|
||||
((assembly.expected_mask & (1u << dropped_channel)) != 0u))
|
||||
{
|
||||
complete = gps_nav_epoch_assembly_remove_channel(
|
||||
&assembly, dropped_channel);
|
||||
}
|
||||
CHECK(mask_count(final_mask) < 4u ||
|
||||
(complete && gps_nav_epoch_assembly_complete(&assembly)),
|
||||
"random interrupt/unlock sequence did not complete");
|
||||
CHECK(assembly.expected_mask == final_mask,
|
||||
"random unlock changed the wrong expected channel");
|
||||
CHECK(assembly.received_mask == final_mask,
|
||||
"random model lost or invented an interrupt");
|
||||
}
|
||||
}
|
||||
|
||||
static void multi_epoch_hardware_interrupt_model(void)
|
||||
{
|
||||
gps_nav_epoch_assembly_t assembly;
|
||||
uint32_t random_state = 0xbb67ae85u;
|
||||
uint32_t ready_mask = 0x7fu;
|
||||
unsigned int completed_epochs = 0u;
|
||||
unsigned int dropped_epochs = 0u;
|
||||
unsigned int joined_epochs = 0u;
|
||||
|
||||
for(unsigned int epoch = 0u ; epoch < 2000u ; epoch++)
|
||||
{
|
||||
bool drop_before_interrupt = (epoch == 250u);
|
||||
bool drop_after_interrupt = (epoch == 900u);
|
||||
bool join_during_epoch = (epoch == 500u) || (epoch == 1200u);
|
||||
unsigned int changing_channel =
|
||||
(epoch == 1200u) ? 6u : 5u;
|
||||
|
||||
if(epoch == 400u)
|
||||
{
|
||||
ready_mask |= 1u << 2;
|
||||
}
|
||||
if(epoch == 1050u)
|
||||
{
|
||||
ready_mask |= 1u << 4;
|
||||
}
|
||||
if(join_during_epoch)
|
||||
{
|
||||
ready_mask &= ~(1u << changing_channel);
|
||||
}
|
||||
|
||||
uint32_t epoch_ready_mask = ready_mask;
|
||||
unsigned int order[GPS_NAV_EPOCH_MAX_CANDIDATES];
|
||||
unsigned int order_count = 0u;
|
||||
for(unsigned int channel = 0u ;
|
||||
channel < GPS_NAV_EPOCH_MAX_CANDIDATES ; channel++)
|
||||
{
|
||||
if((epoch_ready_mask & (1u << channel)) != 0u)
|
||||
{
|
||||
order[order_count++] = channel;
|
||||
}
|
||||
}
|
||||
for(unsigned int i = order_count ; i > 1u ; i--)
|
||||
{
|
||||
unsigned int swap =
|
||||
deterministic_random(&random_state) % i;
|
||||
unsigned int temporary = order[i - 1u];
|
||||
order[i - 1u] = order[swap];
|
||||
order[swap] = temporary;
|
||||
}
|
||||
|
||||
gps_nav_epoch_assembly_reset(&assembly);
|
||||
bool completed = false;
|
||||
for(unsigned int event_index = 0u ;
|
||||
event_index < order_count ; event_index++)
|
||||
{
|
||||
unsigned int channel = order[event_index];
|
||||
if(drop_before_interrupt && (channel == 2u))
|
||||
{
|
||||
ready_mask &= ~(1u << channel);
|
||||
(void)gps_nav_epoch_assembly_remove_channel(
|
||||
&assembly, channel);
|
||||
dropped_epochs++;
|
||||
continue;
|
||||
}
|
||||
|
||||
gps_nav_epoch_event_result_t result =
|
||||
gps_nav_epoch_assembly_push(
|
||||
&assembly, 100000u + epoch, channel,
|
||||
epoch_ready_mask);
|
||||
if(result == GPS_NAV_EPOCH_EVENT_COMPLETE)
|
||||
{
|
||||
completed = true;
|
||||
}
|
||||
|
||||
if(drop_after_interrupt && (channel == 4u))
|
||||
{
|
||||
ready_mask &= ~(1u << channel);
|
||||
if(gps_nav_epoch_assembly_remove_channel(
|
||||
&assembly, channel))
|
||||
{
|
||||
completed = true;
|
||||
}
|
||||
dropped_epochs++;
|
||||
}
|
||||
|
||||
if(join_during_epoch && (event_index == 0u))
|
||||
{
|
||||
ready_mask |= 1u << changing_channel;
|
||||
CHECK(gps_nav_epoch_assembly_push(
|
||||
&assembly, 100000u + epoch,
|
||||
changing_channel, ready_mask) ==
|
||||
GPS_NAV_EPOCH_EVENT_INVALID,
|
||||
"mid-epoch connection expanded the frozen mask");
|
||||
joined_epochs++;
|
||||
}
|
||||
|
||||
if((event_index == 0u) && (epoch != 0u))
|
||||
{
|
||||
CHECK(gps_nav_epoch_assembly_push(
|
||||
&assembly, 99999u + epoch,
|
||||
channel, epoch_ready_mask) ==
|
||||
GPS_NAV_EPOCH_EVENT_STALE,
|
||||
"delayed previous-epoch interrupt was accepted");
|
||||
}
|
||||
}
|
||||
|
||||
CHECK(completed &&
|
||||
gps_nav_epoch_assembly_complete(&assembly),
|
||||
"hardware stream did not complete a live epoch");
|
||||
CHECK(assembly.received_mask == assembly.expected_mask,
|
||||
"hardware stream completed with a missing interrupt");
|
||||
completed_epochs++;
|
||||
}
|
||||
|
||||
CHECK(completed_epochs == 2000u,
|
||||
"hardware stream produced more or fewer than one completion per epoch");
|
||||
CHECK(dropped_epochs == 2u,
|
||||
"hardware stream did not exercise both unlock positions");
|
||||
CHECK(joined_epochs == 2u,
|
||||
"hardware stream did not exercise both reconnect positions");
|
||||
}
|
||||
|
||||
static void all_coherent_across_sample_wrap(void)
|
||||
{
|
||||
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
|
||||
{2300, 604799.9998, UINT32_MAX - 100u},
|
||||
{2301, 0.0001, 50u},
|
||||
{2301, 0.0000, 1200u},
|
||||
{2300, 604799.9999, UINT32_MAX - 2000u}
|
||||
};
|
||||
gps_nav_epoch_selection_t selection = select_epoch(candidates, 4u);
|
||||
CHECK(selection.selected_count == 4u,
|
||||
"sample/week wrap split a coherent epoch");
|
||||
CHECK(selection.selected_mask == 0x0fu,
|
||||
"sample/week wrap selected the wrong rows");
|
||||
}
|
||||
|
||||
static void tow_outlier_does_not_block_good_cluster(void)
|
||||
{
|
||||
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
|
||||
{2300, 100.0000, 1000000u},
|
||||
{2300, 100.0001, 1010000u},
|
||||
{2300, 99.9999, 1020000u},
|
||||
{2300, 100.0002, 1030000u},
|
||||
{2300, 100.0000, 1040000u},
|
||||
{2300, 100.0001, 1050000u},
|
||||
{2300, 100.2000, 1060000u}
|
||||
};
|
||||
gps_nav_epoch_selection_t selection = select_epoch(candidates, 7u);
|
||||
CHECK(selection.selected_count == 6u,
|
||||
"single TOW outlier blocked the six-satellite cluster");
|
||||
CHECK(selection.selected_mask == 0x3fu,
|
||||
"TOW outlier was not excluded");
|
||||
}
|
||||
|
||||
static void newest_sample_outlier_does_not_define_the_cluster(void)
|
||||
{
|
||||
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
|
||||
{2300, 200.0000, 2000000u},
|
||||
{2300, 200.0000, 2010000u},
|
||||
{2300, 200.0000, 2020000u},
|
||||
{2300, 200.0000, 2030000u},
|
||||
{2300, 200.0000, 2040000u},
|
||||
{2300, 200.0000, 2050000u},
|
||||
{2300, 200.0000, 3000000u}
|
||||
};
|
||||
gps_nav_epoch_selection_t selection = select_epoch(candidates, 7u);
|
||||
CHECK(selection.selected_count == 6u,
|
||||
"newest raw-sample outlier displaced the largest cluster");
|
||||
CHECK(selection.selected_mask == 0x3fu,
|
||||
"newest raw-sample outlier was not excluded");
|
||||
}
|
||||
|
||||
static void equal_clusters_prefer_newer_epoch(void)
|
||||
{
|
||||
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
|
||||
{2300, 300.0000, 1000000u},
|
||||
{2300, 300.0000, 1010000u},
|
||||
{2300, 300.0000, 1020000u},
|
||||
{2300, 300.0000, 1030000u},
|
||||
{2300, 300.2000, 5000000u},
|
||||
{2300, 300.2000, 5010000u},
|
||||
{2300, 300.2000, 5020000u},
|
||||
{2300, 300.2000, 5030000u}
|
||||
};
|
||||
gps_nav_epoch_selection_t selection = select_epoch(candidates, 8u);
|
||||
CHECK(selection.selected_count == 4u,
|
||||
"equal epoch clusters returned the wrong size");
|
||||
CHECK(selection.selected_mask == 0xf0u,
|
||||
"equal epoch clusters did not prefer the newer epoch");
|
||||
}
|
||||
|
||||
static void nonfinite_candidate_is_excluded(void)
|
||||
{
|
||||
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
|
||||
{2300, 400.0000, 1000000u},
|
||||
{2300, 400.0000, 1010000u},
|
||||
{2300, 400.0000, 1020000u},
|
||||
{2300, 400.0000, 1030000u},
|
||||
{2300, NAN, 1040000u}
|
||||
};
|
||||
gps_nav_epoch_selection_t selection = select_epoch(candidates, 5u);
|
||||
CHECK(selection.selected_count == 4u,
|
||||
"invalid TOW candidate blocked valid measurements");
|
||||
CHECK(selection.selected_mask == 0x0fu,
|
||||
"invalid TOW candidate was selected");
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
epoch_id_rounding_and_week_wrap();
|
||||
assembly_waits_for_every_ready_channel();
|
||||
assembly_handles_duplicates_and_epoch_order();
|
||||
assembly_disconnect_and_reconnect();
|
||||
channel_join_does_not_expand_active_epoch();
|
||||
randomized_interrupt_and_unlock_model();
|
||||
multi_epoch_hardware_interrupt_model();
|
||||
all_coherent_across_sample_wrap();
|
||||
tow_outlier_does_not_block_good_cluster();
|
||||
newest_sample_outlier_does_not_define_the_cluster();
|
||||
equal_clusters_prefer_newer_epoch();
|
||||
nonfinite_candidate_is_excluded();
|
||||
|
||||
if(failures == 0)
|
||||
{
|
||||
puts("gps_nav_epoch tests passed");
|
||||
}
|
||||
return failures == 0 ? 0 : 1;
|
||||
}
|
||||
@@ -0,0 +1,97 @@
|
||||
#include "bsp/plic.h"
|
||||
#include "bsp/soc.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
#define PLIC_WORDS ((0x200008u / sizeof(uint32_t)) + 1u)
|
||||
#define PLIC_ENABLE_OFFSET 0x2000u
|
||||
|
||||
static uint32_t plic_words[PLIC_WORDS];
|
||||
static int failures;
|
||||
|
||||
#define CHECK(condition, message) do { \
|
||||
if(!(condition)) { \
|
||||
fprintf(stderr, "FAIL: %s\n", message); \
|
||||
failures++; \
|
||||
} \
|
||||
} while(0)
|
||||
|
||||
uint32_t test_bsp_read32(uintptr_t address)
|
||||
{
|
||||
if((address < BSP_PLIC_BASE) ||
|
||||
(address >= BSP_PLIC_BASE + sizeof(plic_words)))
|
||||
{
|
||||
failures++;
|
||||
return 0u;
|
||||
}
|
||||
return plic_words[(address - BSP_PLIC_BASE) / sizeof(uint32_t)];
|
||||
}
|
||||
|
||||
void test_bsp_write32(uintptr_t address, uint32_t value)
|
||||
{
|
||||
if((address < BSP_PLIC_BASE) ||
|
||||
(address >= BSP_PLIC_BASE + sizeof(plic_words)))
|
||||
{
|
||||
failures++;
|
||||
return;
|
||||
}
|
||||
plic_words[(address - BSP_PLIC_BASE) / sizeof(uint32_t)] = value;
|
||||
}
|
||||
|
||||
static uint32_t enabled_sources(void)
|
||||
{
|
||||
return plic_words[PLIC_ENABLE_OFFSET / sizeof(uint32_t)];
|
||||
}
|
||||
|
||||
static void initialize_gps_owned_sources(void)
|
||||
{
|
||||
bsp_plic_disable_all();
|
||||
bsp_plic_set_threshold(0u);
|
||||
for(uint32_t source = 1u ; source <= 10u ; source++)
|
||||
{
|
||||
bsp_plic_set_priority(source, 1u);
|
||||
bsp_plic_enable(source);
|
||||
}
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
memset(plic_words, 0, sizeof(plic_words));
|
||||
|
||||
/* Model the previous fused image: GPS sources 1..10 plus PMOD/IMU 11. */
|
||||
for(uint32_t source = 1u ; source <= 11u ; source++)
|
||||
{
|
||||
bsp_plic_enable(source);
|
||||
}
|
||||
CHECK(enabled_sources() == 0x0ffeu,
|
||||
"fused-image source mask was not established");
|
||||
|
||||
/* CPU-only warm reset must remove every source not owned by GPS. */
|
||||
initialize_gps_owned_sources();
|
||||
CHECK(enabled_sources() == 0x07feu,
|
||||
"warm GPS initialization retained the previous IMU source");
|
||||
for(uint32_t source = 1u ; source <= 10u ; source++)
|
||||
{
|
||||
CHECK(plic_words[source] == 1u,
|
||||
"GPS source priority was not re-established");
|
||||
}
|
||||
|
||||
/* A later IMU start may explicitly reclaim source 11. */
|
||||
bsp_plic_set_priority(11u, 1u);
|
||||
bsp_plic_enable(11u);
|
||||
CHECK(enabled_sources() == 0x0ffeu,
|
||||
"explicit IMU start did not add source 11");
|
||||
|
||||
/* A second warm GPS-only initialization must be idempotent. */
|
||||
initialize_gps_owned_sources();
|
||||
CHECK(enabled_sources() == 0x07feu,
|
||||
"repeated warm initialization was not idempotent");
|
||||
|
||||
if(failures == 0)
|
||||
{
|
||||
puts("plic warm-reset tests passed");
|
||||
}
|
||||
return failures == 0 ? 0 : 1;
|
||||
}
|
||||
Reference in New Issue
Block a user