433 lines
15 KiB
C
433 lines
15 KiB
C
#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
|
|
#define CAPTURE_PERIOD_TICKS 25000000ULL
|
|
#define CAPTURE_WINDOW_TICKS 3125000ULL
|
|
#define HOLDOFF_LOCAL_TICKS 15000ULL
|
|
|
|
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 bool validate_complete_epoch(
|
|
const gps_nav_epoch_key_t* candidates,
|
|
unsigned int count,
|
|
gps_nav_epoch_selection_t* selection)
|
|
{
|
|
memset(selection, 0, sizeof(*selection));
|
|
return gps_nav_epoch_validate_complete(
|
|
candidates, count, SAMPLE_WINDOW,
|
|
TOW_TOLERANCE, selection);
|
|
}
|
|
|
|
static gps_nav_epoch_event_result_t push_capture(
|
|
gps_nav_epoch_assembly_t* assembly,
|
|
uint64_t capture_counter,
|
|
uint64_t local_ticks,
|
|
unsigned int channel,
|
|
uint32_t ready_mask)
|
|
{
|
|
return gps_nav_epoch_assembly_push(
|
|
assembly, capture_counter, local_ticks, channel, ready_mask,
|
|
CAPTURE_PERIOD_TICKS, CAPTURE_WINDOW_TICKS,
|
|
HOLDOFF_LOCAL_TICKS);
|
|
}
|
|
|
|
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 local_capture_assembly_completes(void)
|
|
{
|
|
gps_nav_epoch_assembly_t assembly;
|
|
gps_nav_epoch_assembly_reset(&assembly);
|
|
const uint32_t ready_mask = 0x7fu;
|
|
const unsigned int order[] = {4u, 0u, 6u, 2u, 5u, 1u, 3u};
|
|
const uint64_t base = 810102900000ULL;
|
|
|
|
for(unsigned int i = 0u ; i < sizeof(order) / sizeof(order[0]) ; i++)
|
|
{
|
|
gps_nav_epoch_event_result_t result =
|
|
push_capture(&assembly, base + (uint64_t)i * 250000u,
|
|
100000u + i, order[i], ready_mask);
|
|
bool last = i + 1u == sizeof(order) / sizeof(order[0]);
|
|
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) == last,
|
|
"coherent local capture group completed at the wrong event");
|
|
}
|
|
CHECK(assembly.expected_mask == ready_mask,
|
|
"assembly changed its frozen ready mask");
|
|
CHECK(assembly.received_mask == ready_mask,
|
|
"assembly lost a ready-channel capture");
|
|
CHECK((assembly.maximum_capture_counter -
|
|
assembly.minimum_capture_counter) <= CAPTURE_WINDOW_TICKS,
|
|
"coherent group exceeded its capture span");
|
|
}
|
|
|
|
static void coalesced_capture_sequence_validation(void)
|
|
{
|
|
uint32_t gap = 0u;
|
|
const uint64_t period = CAPTURE_PERIOD_TICKS;
|
|
const uint64_t base = 5000000000ULL;
|
|
|
|
CHECK(gps_nav_capture_sequence_consistent(
|
|
base, 10u, base + period, 11u, period, &gap) &&
|
|
(gap == 1u),
|
|
"single capture sequence advance was rejected");
|
|
CHECK(gps_nav_capture_sequence_consistent(
|
|
base, 10u, base + 5u * period, 15u, period, &gap) &&
|
|
(gap == 5u),
|
|
"coalesced newest capture did not preserve its real epoch gap");
|
|
CHECK(gps_nav_capture_sequence_consistent(
|
|
base, UINT32_MAX - 1u, base + 3u * period, 1u,
|
|
period, &gap) &&
|
|
(gap == 3u),
|
|
"capture sequence wrap was not handled modulo uint32");
|
|
CHECK(gps_nav_capture_sequence_consistent(
|
|
base, UINT32_MAX, base + period, 0u, period, &gap) &&
|
|
(gap == 1u),
|
|
"capture sequence value zero at wrap was treated as uninitialized");
|
|
CHECK(gps_nav_capture_sequence_consistent(
|
|
base, 0u, base + period, 1u, period, &gap) &&
|
|
(gap == 1u),
|
|
"capture sequence did not continue after wrapping through zero");
|
|
CHECK(gps_nav_capture_sequence_consistent(
|
|
base, 10u, base + period + period / 3u,
|
|
11u, period, &gap),
|
|
"bounded hardware-counter phase jitter was rejected");
|
|
|
|
CHECK(!gps_nav_capture_sequence_consistent(
|
|
base, 10u, base + period, 10u, period, &gap),
|
|
"duplicate capture sequence entered a new epoch");
|
|
CHECK(!gps_nav_capture_sequence_consistent(
|
|
base, 10u, base - 1u, 11u, period, &gap),
|
|
"backward hardware counter was accepted");
|
|
CHECK(!gps_nav_capture_sequence_consistent(
|
|
base, 10u, base + 2u * period, 11u, period, &gap),
|
|
"counter/sequence disagreement was accepted");
|
|
CHECK(!gps_nav_capture_sequence_consistent(
|
|
base, 10u, base + period, 11u, 0u, &gap),
|
|
"zero capture period was accepted");
|
|
}
|
|
|
|
static void missing_channel_expires_as_usable_subset(void)
|
|
{
|
|
gps_nav_epoch_assembly_t assembly;
|
|
gps_nav_epoch_assembly_reset(&assembly);
|
|
const uint32_t ready_mask = 0x3fu;
|
|
const uint64_t base = 900000000ULL;
|
|
|
|
for(unsigned int channel = 0u ; channel < 5u ; channel++)
|
|
{
|
|
CHECK(push_capture(&assembly,
|
|
base + (uint64_t)channel * 100000u,
|
|
200000u + channel,
|
|
channel, ready_mask) ==
|
|
GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
|
"partial epoch rejected a coherent channel");
|
|
}
|
|
CHECK(!gps_nav_epoch_assembly_complete(&assembly),
|
|
"missing ready channel completed before holdoff");
|
|
CHECK(gps_nav_epoch_assembly_usable(
|
|
&assembly, GPS_NAV_MIN_WLS_SATELLITES),
|
|
"five-channel partial epoch was not usable");
|
|
CHECK(!gps_nav_epoch_assembly_expired(
|
|
&assembly, assembly.deadline_local_ticks - 1u),
|
|
"epoch expired before its deadline");
|
|
CHECK(gps_nav_epoch_assembly_expired(
|
|
&assembly, assembly.deadline_local_ticks),
|
|
"epoch did not expire at its deadline");
|
|
}
|
|
|
|
static void incomplete_epoch_never_becomes_complete(void)
|
|
{
|
|
gps_nav_epoch_assembly_t assembly;
|
|
gps_nav_epoch_assembly_reset(&assembly);
|
|
const uint32_t ready_mask = 0x1fu;
|
|
const uint64_t base = 950000000ULL;
|
|
|
|
for(unsigned int channel = 0u ; channel < 4u ; channel++)
|
|
{
|
|
CHECK(push_capture(&assembly,
|
|
base + (uint64_t)channel * 100000u,
|
|
250000u + channel,
|
|
channel, ready_mask) ==
|
|
GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
|
"four-channel partial epoch rejected a coherent channel");
|
|
}
|
|
CHECK(gps_nav_epoch_assembly_usable(
|
|
&assembly, GPS_NAV_MIN_WLS_SATELLITES),
|
|
"four coherent channels must remain algebraically solvable");
|
|
CHECK(!gps_nav_epoch_assembly_complete(&assembly),
|
|
"timed-out subset incorrectly completed the expected epoch");
|
|
CHECK(gps_nav_epoch_assembly_expired(
|
|
&assembly, assembly.deadline_local_ticks),
|
|
"incomplete epoch did not reach its discard deadline");
|
|
}
|
|
|
|
static void local_outlier_cannot_poison_epoch(void)
|
|
{
|
|
gps_nav_epoch_assembly_t assembly;
|
|
gps_nav_epoch_assembly_reset(&assembly);
|
|
const uint32_t ready_mask = 0x1fu;
|
|
const uint64_t base = 1000000000ULL;
|
|
|
|
CHECK(push_capture(&assembly, base, 300000u, 0u, ready_mask) ==
|
|
GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
|
"outlier scenario did not start");
|
|
CHECK(push_capture(&assembly, base + 6000000u, 300001u,
|
|
1u, ready_mask) ==
|
|
GPS_NAV_EPOCH_EVENT_OUTLIER,
|
|
"ambiguous local timestamp entered the active group");
|
|
CHECK((assembly.received_mask == 0x01u) &&
|
|
(assembly.minimum_capture_counter == base) &&
|
|
(assembly.maximum_capture_counter == base),
|
|
"local outlier mutated the active group");
|
|
|
|
CHECK(push_capture(&assembly, base - CAPTURE_PERIOD_TICKS,
|
|
300002u, 2u, ready_mask) ==
|
|
GPS_NAV_EPOCH_EVENT_STALE,
|
|
"delayed previous epoch was not rejected");
|
|
CHECK(push_capture(&assembly, base + CAPTURE_PERIOD_TICKS,
|
|
300003u, 1u, ready_mask) ==
|
|
GPS_NAV_EPOCH_EVENT_RESTARTED,
|
|
"new local epoch did not replace the incomplete group");
|
|
CHECK((assembly.first_capture_counter ==
|
|
base + CAPTURE_PERIOD_TICKS) &&
|
|
(assembly.received_mask == 0x02u),
|
|
"new epoch retained state from the old group");
|
|
}
|
|
|
|
static void duplicate_join_and_disconnect_are_bounded(void)
|
|
{
|
|
gps_nav_epoch_assembly_t assembly;
|
|
gps_nav_epoch_assembly_reset(&assembly);
|
|
const uint64_t base = 1100000000ULL;
|
|
|
|
CHECK(push_capture(&assembly, base, 400000u, 0u, 0x1fu) ==
|
|
GPS_NAV_EPOCH_EVENT_ACCEPTED,
|
|
"membership scenario did not start");
|
|
CHECK(push_capture(&assembly, base, 400001u, 0u, 0x1fu) ==
|
|
GPS_NAV_EPOCH_EVENT_DUPLICATE,
|
|
"duplicate capture was not idempotent");
|
|
CHECK(push_capture(&assembly, base + 100000u, 400002u,
|
|
5u, 0x3fu) ==
|
|
GPS_NAV_EPOCH_EVENT_INVALID,
|
|
"joining channel expanded the active epoch");
|
|
CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 4u),
|
|
"disconnect completed a one-channel group");
|
|
|
|
for(unsigned int channel = 1u ; channel < 4u ; channel++)
|
|
{
|
|
gps_nav_epoch_event_result_t result =
|
|
push_capture(&assembly,
|
|
base + (uint64_t)channel * 100000u,
|
|
400010u + channel, channel, 0x0fu);
|
|
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
|
|
(channel == 3u),
|
|
"remaining frozen membership completed incorrectly");
|
|
}
|
|
}
|
|
|
|
static void randomized_capture_liveness(void)
|
|
{
|
|
uint32_t random_state = 0x6a09e667u;
|
|
uint64_t base = 2000000000ULL;
|
|
for(unsigned int trial = 0u ; trial < 100000u ; trial++)
|
|
{
|
|
gps_nav_epoch_assembly_t assembly;
|
|
gps_nav_epoch_assembly_reset(&assembly);
|
|
unsigned int ready_count =
|
|
4u + (deterministic_random(&random_state) % 6u);
|
|
uint32_t ready_mask = (1u << ready_count) - 1u;
|
|
unsigned int delivered_count =
|
|
4u + (deterministic_random(&random_state) %
|
|
(ready_count - 3u));
|
|
unsigned int order[GPS_NAV_EPOCH_MAX_CANDIDATES];
|
|
for(unsigned int i = 0u ; i < ready_count ; i++)
|
|
{
|
|
order[i] = i;
|
|
}
|
|
for(unsigned int i = ready_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;
|
|
}
|
|
|
|
for(unsigned int i = 0u ; i < delivered_count ; i++)
|
|
{
|
|
uint64_t offset =
|
|
deterministic_random(&random_state) %
|
|
(CAPTURE_WINDOW_TICKS + 1u);
|
|
gps_nav_epoch_event_result_t result =
|
|
push_capture(&assembly, base + offset,
|
|
500000u + i, order[i], ready_mask);
|
|
CHECK((result == GPS_NAV_EPOCH_EVENT_ACCEPTED) ||
|
|
(result == GPS_NAV_EPOCH_EVENT_COMPLETE),
|
|
"random coherent capture was rejected");
|
|
}
|
|
CHECK(gps_nav_epoch_assembly_usable(
|
|
&assembly, GPS_NAV_MIN_WLS_SATELLITES),
|
|
"random stream lost algebraic four-channel liveness");
|
|
CHECK(gps_nav_epoch_assembly_complete(&assembly) ==
|
|
(delivered_count == ready_count),
|
|
"random partial epoch claimed all expected channels");
|
|
if(delivered_count != ready_count)
|
|
{
|
|
CHECK(gps_nav_epoch_assembly_expired(
|
|
&assembly, assembly.deadline_local_ticks),
|
|
"partial random epoch did not expire");
|
|
}
|
|
base += CAPTURE_PERIOD_TICKS;
|
|
}
|
|
}
|
|
|
|
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");
|
|
CHECK(validate_complete_epoch(candidates, 4u, &selection),
|
|
"fully coherent week/sample-wrap epoch was rejected");
|
|
}
|
|
|
|
static void tow_outlier_does_not_block_good_cluster(void)
|
|
{
|
|
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
|
|
{380, 469367.4000, 2982422271u},
|
|
{380, 469367.4001, 2982428475u},
|
|
{381, 28831.6000, 2982490274u},
|
|
{380, 469367.3999, 2982550441u},
|
|
{380, 469367.4002, 2982661405u}
|
|
};
|
|
gps_nav_epoch_selection_t selection = select_epoch(candidates, 5u);
|
|
CHECK(selection.selected_count == 4u,
|
|
"live 45.6-hour TOW outlier blocked the good cluster");
|
|
CHECK(selection.selected_mask == 0x1bu,
|
|
"live TOW outlier was not excluded");
|
|
CHECK(!validate_complete_epoch(candidates, 5u, &selection),
|
|
"TOW outlier incorrectly allowed a partial receiver epoch");
|
|
CHECK(selection.selected_count == 4u,
|
|
"strict rejection lost diagnostic largest-cluster metadata");
|
|
}
|
|
|
|
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");
|
|
CHECK(!validate_complete_epoch(candidates, 7u, &selection),
|
|
"sample outlier incorrectly allowed a partial receiver epoch");
|
|
}
|
|
|
|
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");
|
|
CHECK(!validate_complete_epoch(candidates, 8u, &selection),
|
|
"two complete-looking epoch clusters were mixed");
|
|
}
|
|
|
|
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");
|
|
CHECK(!validate_complete_epoch(candidates, 5u, &selection),
|
|
"nonfinite TOW incorrectly allowed a partial receiver epoch");
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
local_capture_assembly_completes();
|
|
coalesced_capture_sequence_validation();
|
|
missing_channel_expires_as_usable_subset();
|
|
incomplete_epoch_never_becomes_complete();
|
|
local_outlier_cannot_poison_epoch();
|
|
duplicate_join_and_disconnect_are_bounded();
|
|
randomized_capture_liveness();
|
|
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;
|
|
}
|