/* * PVT consistency under the things that actually happen on the board: * satellites appearing and disappearing mid-epoch, a 32-bit sample counter * that wraps every 262 seconds, coalesced interrupts, and a cold start whose * flash data is older than the receiver has any way to know. * * The question each test answers is not "does it compute a position" but "can * this produce a position that is wrong without saying so". */ #include #include #include #include #include #include #include "gps/gps_ip_defines.h" #include "gps/gps_acq_manager.h" #include "gps/gps_almanac.h" #include "gps/gps_ephemeris_cache.h" #include "gps/gps_ephemeris_validity.h" #include "gps/gps_nav_epoch.h" #include "gps/gps_pps.h" #include "gps/gps_pps_time_filter.h" #include "gps/Navigation.h" static int failures; #define CHECK(condition, message) do { \ if(!(condition)) { \ fprintf(stderr, "FAIL: %s\n", message); \ failures++; \ } \ } while(0) /* The receiver's own constants, so the test cannot drift from the firmware. */ #define SAMPLE_RATE_HZ GPS_RF_SAMPLE_RATE_HZ #define SAMPLES_PER_MS (SAMPLE_RATE_HZ / 1000.0) #define SYNC_WINDOW ((uint32_t)(SAMPLES_PER_MS * \ (double)GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS)) #define TOW_TOLERANCE GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC /* ================================================================== */ /* 1. Cold start: what does the acquisition engine actually search? */ /* ================================================================== */ /* * At power-on there is no receiver time - it arrives only once a satellite is * tracked and its HOW decoded - so the almanac cannot be evaluated and the * visibility model must stand aside completely. Anything less would mean a * cold start searching a subset chosen by data it cannot yet validate. */ static void test_cold_start_searches_every_prn(void) { gps_acq_manager_t manager; gps_acq_manager_config_t config = { .retry_delay_ticks = GPS_ACQ_RETRY_DELAY_TICKS, .hot_retry_delay_ticks = GPS_ACQ_HOT_RETRY_DELAY_TICKS, .hot_window_ticks = GPS_ACQ_REACQ_PRIORITY_TICKS, .max_hot_streak = GPS_ACQ_MAX_HOT_STREAK, .hot_fast_attempts = GPS_ACQ_HOT_FAST_ATTEMPTS, .unlikely_retry_delay_ticks = GPS_ACQ_UNLIKELY_RETRY_DELAY_TICKS }; gps_acq_manager_init(&manager, &config); CHECK(gps_acq_manager_search_mask(&manager) == 0xffffffffu, "acquisition did not start with every PRN searchable"); for(unsigned int prn = 0u; prn < GPS_ACQ_MANAGER_PRN_COUNT; prn++) { CHECK(gps_acq_manager_enable_cold(&manager, prn), "enable cold"); } /* * gps_refresh_acq_visibility() forces the mask wide whenever receiver time * is unknown. Reproduce that and confirm one full sweep really does reach * all 32 PRNs before any of them is retried. */ gps_acq_manager_set_search_mask(&manager, 0xffffffffu); uint32_t swept = 0u; uint64_t now = 1u; for(unsigned int i = 0u; i < GPS_ACQ_MANAGER_PRN_COUNT; i++) { unsigned int prn = 0u; bool hot = false; CHECK(gps_acq_manager_start_next(&manager, now, &prn, &hot), "cold start had nothing to search"); CHECK((swept & (1u << prn)) == 0u, "a PRN was searched twice before the sweep completed"); swept |= 1u << prn; gps_acq_manager_mark_failed(&manager, prn, now); now += GPS_ACQ_RETRY_DELAY_TICKS; } CHECK(swept == 0xffffffffu, "the cold-start sweep missed a PRN"); printf(" cold start with no time: all 32 PRNs searched, none twice\n"); /* * And an empty visibility model - which is what a corrupt or absent flash * almanac produces - must degrade to the same blind sweep, never to * silence. */ gps_acq_manager_set_search_mask(&manager, 0u); CHECK(gps_acq_manager_search_mask(&manager) == 0xffffffffu, "an empty visibility mask silenced acquisition"); printf(" empty visibility model degrades to a blind sweep\n"); } /* * The other half of the cold-start question: a flash ephemeris is installed * while time is unknown, so what stops a months-old orbit reaching the * position solve? * * gps_collect_completed_epoch_locked() qualifies every channel with * time_known = true against the epoch's own transmit time, which comes from * the decoded HOW and not from anything persisted. So the age check is * unavoidable by the time a measurement can influence a fix, however the * ephemeris got into the cache. */ static void test_stale_ephemeris_cannot_reach_the_solve(void) { gps_ephemeris_t eph; memset(&eph, 0, sizeof(eph)); eph.ephemeris_valid = true; eph.SVID = 5u; eph.iode = 0x30u; eph.iodc = 0x30u; eph.toe.sec = 100000.0; eph.toc.sec = 100000.0; eph.toe.week = 2431; eph.toe_week_valid = true; eph.A = 26560000.0; eph.sqrta = 5153.6; eph.ecc = 0.006; eph.sva = 2u; eph.fit_flag = false; /* nominal four hour curve fit */ /* While time is unknown only the structural rules can apply - this is the * cold-start window, and it is why the epoch gate exists. */ CHECK(gps_ephemeris_check(&eph, 5u, false, 0, 0.0) == GPS_EPHEMERIS_OK, "structural check rejected a well-formed record"); /* Inside the fit interval: usable. */ CHECK(gps_ephemeris_check(&eph, 5u, true, 2431, 103000.0) == GPS_EPHEMERIS_OK, "a record 50 minutes old was rejected"); /* Past it: refused, whatever it claims about itself. */ CHECK(gps_ephemeris_check(&eph, 5u, true, 2431, 100000.0 + 8000.0) == GPS_EPHEMERIS_REJECT_AGE, "a record past its curve fit interval was accepted"); /* A week later, expressed as a wrapped second-of-week - the shape a * flash record takes after a long power-off. */ CHECK(gps_ephemeris_check(&eph, 5u, true, 2432, 100000.0) == GPS_EPHEMERIS_REJECT_AGE, "a week-old record was accepted"); printf(" restored ephemeris is age-checked before it can reach a fix\n"); } /* ================================================================== */ /* 2. The 32-bit sample counter wraps every 262 s. */ /* ================================================================== */ /* * Pseudoranges are differences of corrected sample counts against one * reference, scaled to metres. The counter is 32 bits at 16.368 MHz, so it * wraps mid-flight roughly every four and a half minutes. If the wrap leaked * into the differences it would put one satellite 2^31 samples - about 39 * million kilometres - away from the rest, and the resulting position would be * wrong with a perfectly ordinary-looking residual on the other channels. * * Build one epoch twice, once away from the wrap and once straddling it, and * require the metre-domain pseudoranges to be identical. */ static void test_sample_wrap_does_not_disturb_pseudoranges(void) { /* Six channels spread over 20 ms of capture skew, with sub-sample code * phase corrections, which is the realistic shape of one epoch. */ const int32_t offsets[6] = {0, -12345, 48000, -230000, 91000, 150000}; const double code_phase[6] = {0.31, 0.77, 0.02, 0.55, 0.94, 0.18}; double away[6]; double across[6]; for(unsigned int pass = 0u; pass < 2u; pass++) { /* Pass 0 sits mid-range; pass 1 puts the reference just past the * wrap so half the channels are still near 0xFFFFFFFF. */ uint32_t base = (pass == 0u) ? 0x40000000u : 0x00000064u; uint32_t reference = base; double lifted[6]; for(unsigned int i = 0u; i < 6u; i++) { uint32_t raw = (uint32_t)((int64_t)base + offsets[i]); double corrected = (double)raw - code_phase[i]; lifted[i] = gps_nav_epoch_unwrap_sample(raw, corrected, reference); } /* Exactly what gps_ip.c forms: milliseconds relative to the * reference, then metres. */ for(unsigned int i = 0u; i < 6u; i++) { double delta_ms = (lifted[i] / SAMPLES_PER_MS) - (lifted[0] / SAMPLES_PER_MS); double metres = delta_ms * (SPEED_OF_LIGHT / 1000.0); if(pass == 0u) { away[i] = metres; } else { across[i] = metres; } } } double worst = 0.0; for(unsigned int i = 0u; i < 6u; i++) { double difference = fabs(away[i] - across[i]); if(difference > worst) { worst = difference; } } printf(" sample counter wrap: worst pseudorange difference %.9f m\n", worst); CHECK(worst < 1.0e-3, "the 32-bit sample wrap moved a pseudorange"); /* The epoch selector's own comparisons must agree across the wrap too, * otherwise a coherent epoch would be rejected once every 262 s. */ /* Sized to the API's declared parameter, not to the count in use: the * signature promises GPS_NAV_EPOCH_MAX_CANDIDATES entries even though it * only reads candidate_count of them. */ gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES]; memset(candidates, 0, sizeof(candidates)); gps_nav_epoch_selection_t selection; for(unsigned int i = 0u; i < 6u; i++) { candidates[i].week = 2431; candidates[i].tow_sec = 300000.0; candidates[i].raw_sample = (uint32_t)((int64_t)0x00000064 + offsets[i]); } CHECK(gps_nav_epoch_validate_complete(candidates, 6u, SYNC_WINDOW, TOW_TOLERANCE, &selection), "a coherent epoch straddling the counter wrap was rejected"); CHECK(selection.selected_count == 6u, "epoch lost a channel at the wrap"); printf(" epoch selection is wrap-safe\n"); } /* ================================================================== */ /* 3. Satellites connecting and disconnecting. */ /* ================================================================== */ #define PERIOD_TICKS ((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_SOLVE_PERIOD_MS / 1000u) #define WINDOW_TICKS (((uint64_t)GPS_PPS_CLOCK_HZ * \ GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS) / 1000u) #define HOLDOFF_TICKS ((uint64_t)1000000) static gps_nav_epoch_event_result_t push(gps_nav_epoch_assembly_t* assembly, uint64_t counter, unsigned int channel, uint32_t ready_mask) { return gps_nav_epoch_assembly_push(assembly, counter, counter, channel, ready_mask, PERIOD_TICKS, WINDOW_TICKS, HOLDOFF_TICKS); } /* * An epoch is an all-or-nothing hardware measurement set: the expected channel * set is frozen when the first event of a period arrives, so the satellite * geometry of a published epoch cannot depend on interrupt timing. These are * the transitions that have to hold that line. */ static void test_epoch_assembly_under_connect_and_disconnect(void) { gps_nav_epoch_assembly_t assembly; uint64_t counter = 1000000u; /* --- a satellite acquired mid-epoch must wait for the next one --- */ gps_nav_epoch_assembly_reset(&assembly); CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "first event rejected"); CHECK(push(&assembly, counter + 10u, 1u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "second event rejected"); /* Channel 4 has just locked, so the live ready mask has grown. */ CHECK(push(&assembly, counter + 20u, 4u, 0x1fu) == GPS_NAV_EPOCH_EVENT_INVALID, "a channel that joined mid-epoch was admitted to it"); CHECK(push(&assembly, counter + 30u, 2u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "third event rejected"); CHECK(push(&assembly, counter + 40u, 3u, 0x1fu) == GPS_NAV_EPOCH_EVENT_COMPLETE, "the frozen expected set did not complete"); printf(" a satellite acquired mid-epoch joins the next epoch, not this one\n"); /* --- a satellite lost mid-epoch must release the epoch --- */ gps_nav_epoch_assembly_reset(&assembly); CHECK(push(&assembly, counter, 0u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 0"); CHECK(push(&assembly, counter + 5u, 1u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1"); CHECK(push(&assembly, counter + 8u, 2u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 2"); CHECK(push(&assembly, counter + 9u, 3u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 3"); /* Channel 4 drops before it ever reports. Removing it must complete the * epoch from what has already arrived rather than stall until the * deadline. */ CHECK(gps_nav_epoch_assembly_remove_channel(&assembly, 4u), "removing a lost channel did not complete the epoch"); CHECK(gps_nav_epoch_assembly_usable(&assembly, GPS_NAV_MIN_WLS_SATELLITES), "four surviving channels were not usable"); printf(" a satellite lost mid-epoch releases the epoch immediately\n"); /* --- losing one too many must not publish a short epoch --- */ gps_nav_epoch_assembly_reset(&assembly); CHECK(push(&assembly, counter, 0u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 0"); CHECK(push(&assembly, counter + 5u, 1u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1"); CHECK(push(&assembly, counter + 8u, 2u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 2"); (void)gps_nav_epoch_assembly_remove_channel(&assembly, 3u); (void)gps_nav_epoch_assembly_remove_channel(&assembly, 4u); CHECK(!gps_nav_epoch_assembly_usable(&assembly, GPS_NAV_MIN_WLS_SATELLITES), "three channels were reported usable for a four-unknown solve"); printf(" three surviving channels are never offered as a fix\n"); /* --- every satellite lost --- */ gps_nav_epoch_assembly_reset(&assembly); CHECK(push(&assembly, counter, 0u, 0x03u) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 0"); CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 0u), "removing the only channel reported completion"); CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 1u), "removing the last channel reported completion"); CHECK(!gps_nav_epoch_assembly_usable(&assembly, 1u), "an emptied assembly was still usable"); CHECK(!gps_nav_epoch_assembly_complete(&assembly), "an emptied assembly reported complete"); printf(" total loss leaves no assembly behind\n"); /* --- the newest period supersedes an incomplete older one --- */ gps_nav_epoch_assembly_reset(&assembly); CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 0"); CHECK(push(&assembly, counter + 5u, 1u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1"); CHECK(push(&assembly, counter + PERIOD_TICKS, 2u, 0x0fu) == GPS_NAV_EPOCH_EVENT_RESTARTED, "an event from the next period did not restart the epoch"); printf(" a new capture period discards the incomplete previous one\n"); /* --- a straggler from the previous period is not back-filled --- */ CHECK(push(&assembly, counter, 3u, 0x0fu) == GPS_NAV_EPOCH_EVENT_STALE, "an event from the previous period was folded into the new epoch"); printf(" a late event from the previous period is dropped, not mixed in\n"); /* --- a coalesced repeat must not be counted twice --- */ gps_nav_epoch_assembly_reset(&assembly); CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 0"); CHECK(push(&assembly, counter + 1u, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_DUPLICATE, "a repeated event for one channel was accepted twice"); printf(" a coalesced repeat is rejected as a duplicate\n"); /* --- an event neither in this epoch nor the next is an outlier --- */ gps_nav_epoch_assembly_reset(&assembly); CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 0"); CHECK(push(&assembly, counter + WINDOW_TICKS + 1u, 1u, 0x0fu) == GPS_NAV_EPOCH_EVENT_OUTLIER, "an event outside the coherence window was accepted"); printf(" an out-of-window capture is refused, not averaged in\n"); } /* * Coalescing: the capture mailbox holds only the newest measurement, so when * interrupts are late the receiver has to know how many epochs it skipped and * must never invent the missing ones. */ static void test_coalesced_captures_are_counted_not_invented(void) { uint32_t gap = 0u; CHECK(gps_nav_capture_sequence_consistent(1000u, 5u, 1000u + PERIOD_TICKS, 6u, PERIOD_TICKS, &gap), "a single clean epoch was rejected"); CHECK(gap == 1u, "single epoch reported a gap"); CHECK(gps_nav_capture_sequence_consistent(1000u, 5u, 1000u + (3u * PERIOD_TICKS), 8u, PERIOD_TICKS, &gap), "three coalesced epochs were rejected"); CHECK(gap == 3u, "coalesced gap miscounted"); /* Counter and sequence disagreeing means the mailbox cannot be trusted. */ CHECK(!gps_nav_capture_sequence_consistent(1000u, 5u, 1000u + (3u * PERIOD_TICKS), 6u, PERIOD_TICKS, &gap), "a sequence/counter mismatch was accepted"); CHECK(!gps_nav_capture_sequence_consistent(1000u, 5u, 1000u, 6u, PERIOD_TICKS, &gap), "a counter that did not advance was accepted"); CHECK(!gps_nav_capture_sequence_consistent(0u, 5u, 1000u + PERIOD_TICKS, 6u, PERIOD_TICKS, &gap), "an unseeded previous counter was accepted"); printf(" coalesced captures are counted exactly, never synthesised\n"); } /* * Publication is stricter than clustering: every channel in the frozen ready * mask must belong to one coherent epoch, or nothing is published. A partial * cluster would change the satellite geometry according to interrupt timing, * which is exactly what produced large raw-position jumps before. */ static void test_partial_coherence_publishes_nothing(void) { gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES]; memset(candidates, 0, sizeof(candidates)); gps_nav_epoch_selection_t selection; for(unsigned int i = 0u; i < 5u; i++) { candidates[i].week = 2431; candidates[i].tow_sec = 300000.0; candidates[i].raw_sample = 1000000u + (i * 100u); } CHECK(gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW, TOW_TOLERANCE, &selection), "a fully coherent set was rejected"); /* One channel a full nav period away - a stale measurement that slipped * through. Four good ones remain, which is enough for a solve, and that * is exactly why it must not be published as one. */ candidates[4].raw_sample = 1000000u + (uint32_t)(SAMPLES_PER_MS * 200.0); CHECK(!gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW, TOW_TOLERANCE, &selection), "an incoherent epoch was published because a subset was solvable"); CHECK(selection.selected_count == 4u, "the diagnostic cluster did not report the coherent subset"); /* A TOW disagreement is equally disqualifying even when the samples line * up: it means two satellites disagree about what time it is. */ candidates[4].raw_sample = 1000000u + 400u; candidates[4].tow_sec = 300000.0 + (10.0 * TOW_TOLERANCE); CHECK(!gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW, TOW_TOLERANCE, &selection), "channels disagreeing about TOW were published as one epoch"); printf(" an incoherent epoch publishes nothing, not its solvable subset\n"); } /* * A non-finite transmit time must never reach the pseudorange arithmetic. * gps_pps_select_reference() is the last gate before it, and it screens the * whole set - not just the entries it happens to walk past before finding the * preferred reference satellite. */ static void test_reference_selection_screens_every_capture(void) { unsigned int ids[4] = {3u, 7u, 11u, 19u}; double times[4] = {10.0, 11.0, 12.0, 13.0}; unsigned int reference = 99u; CHECK(gps_pps_select_reference(ids, times, 4u, true, 7u, &reference), "a clean set was rejected"); CHECK(reference == 1u, "the preferred reference satellite was not chosen"); CHECK(gps_pps_select_reference(ids, times, 4u, false, 0u, &reference), "a clean set without a preference was rejected"); CHECK(reference == 0u, "the earliest capture was not chosen"); /* The corrupt entry sits *after* the preferred satellite, which is the * ordering an early return would walk straight past. */ times[3] = NAN; CHECK(!gps_pps_select_reference(ids, times, 4u, true, 7u, &reference), "a non-finite capture time downstream of the preferred reference " "was not detected"); CHECK(!gps_pps_select_reference(ids, times, 4u, false, 0u, &reference), "a non-finite capture time was not detected without a preference"); printf(" reference selection screens the whole capture set\n"); } int main(void) { test_cold_start_searches_every_prn(); test_stale_ephemeris_cannot_reach_the_solve(); test_sample_wrap_does_not_disturb_pseudoranges(); test_epoch_assembly_under_connect_and_disconnect(); test_coalesced_captures_are_counted_not_invented(); test_partial_coherence_publishes_nothing(); test_reference_selection_screens_every_capture(); if(failures != 0) { fprintf(stderr, "%d check(s) failed\n", failures); return 1; } printf("gps pvt consistency tests passed\n"); return 0; }