gps/soc/software/gps_system/tests/test_gps_pvt_consistency.c

/*
 * PVT consistency under the things that actually happen on the board:
 * satellites appearing and disappearing mid-epoch, a 32-bit sample counter
 * that wraps every 262 seconds, coalesced interrupts, and a cold start whose
 * flash data is older than the receiver has any way to know.
 *
 * The question each test answers is not "does it compute a position" but "can
 * this produce a position that is wrong without saying so".
 */

#include <assert.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "gps/gps_ip_defines.h"
#include "gps/gps_acq_manager.h"
#include "gps/gps_almanac.h"
#include "gps/gps_ephemeris_cache.h"
#include "gps/gps_ephemeris_validity.h"
#include "gps/gps_nav_epoch.h"
#include "gps/gps_pps.h"
#include "gps/gps_pps_time_filter.h"
#include "gps/Navigation.h"

static int failures;

#define CHECK(condition, message) do { \
	if(!(condition)) { \
		fprintf(stderr, "FAIL: %s\n", message); \
		failures++; \
	} \
} while(0)

/* The receiver's own constants, so the test cannot drift from the firmware. */
#define SAMPLE_RATE_HZ   GPS_RF_SAMPLE_RATE_HZ
#define SAMPLES_PER_MS   (SAMPLE_RATE_HZ / 1000.0)
#define SYNC_WINDOW      ((uint32_t)(SAMPLES_PER_MS * \
                          (double)GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS))
#define TOW_TOLERANCE    GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC

/* ================================================================== */
/* 1. Cold start: what does the acquisition engine actually search?    */
/* ================================================================== */

/*
 * At power-on there is no receiver time - it arrives only once a satellite is
 * tracked and its HOW decoded - so the almanac cannot be evaluated and the
 * visibility model must stand aside completely.  Anything less would mean a
 * cold start searching a subset chosen by data it cannot yet validate.
 */
static void test_cold_start_searches_every_prn(void)
{
	gps_acq_manager_t manager;
	gps_acq_manager_config_t config = {
		.retry_delay_ticks = GPS_ACQ_RETRY_DELAY_TICKS,
		.hot_retry_delay_ticks = GPS_ACQ_HOT_RETRY_DELAY_TICKS,
		.hot_window_ticks = GPS_ACQ_REACQ_PRIORITY_TICKS,
		.max_hot_streak = GPS_ACQ_MAX_HOT_STREAK,
		.hot_fast_attempts = GPS_ACQ_HOT_FAST_ATTEMPTS,
		.unlikely_retry_delay_ticks = GPS_ACQ_UNLIKELY_RETRY_DELAY_TICKS
	};
	gps_acq_manager_init(&manager, &config);
	CHECK(gps_acq_manager_search_mask(&manager) == 0xffffffffu,
	      "acquisition did not start with every PRN searchable");

	for(unsigned int prn = 0u; prn < GPS_ACQ_MANAGER_PRN_COUNT; prn++)
	{
		CHECK(gps_acq_manager_enable_cold(&manager, prn), "enable cold");
	}

	/*
	 * gps_refresh_acq_visibility() forces the mask wide whenever receiver time
	 * is unknown.  Reproduce that and confirm one full sweep really does reach
	 * all 32 PRNs before any of them is retried.
	 */
	gps_acq_manager_set_search_mask(&manager, 0xffffffffu);
	uint32_t swept = 0u;
	uint64_t now = 1u;
	for(unsigned int i = 0u; i < GPS_ACQ_MANAGER_PRN_COUNT; i++)
	{
		unsigned int prn = 0u;
		bool hot = false;
		CHECK(gps_acq_manager_start_next(&manager, now, &prn, &hot),
		      "cold start had nothing to search");
		CHECK((swept & (1u << prn)) == 0u,
		      "a PRN was searched twice before the sweep completed");
		swept |= 1u << prn;
		gps_acq_manager_mark_failed(&manager, prn, now);
		now += GPS_ACQ_RETRY_DELAY_TICKS;
	}
	CHECK(swept == 0xffffffffu, "the cold-start sweep missed a PRN");
	printf("  cold start with no time: all 32 PRNs searched, none twice\n");

	/*
	 * And an empty visibility model - which is what a corrupt or absent flash
	 * almanac produces - must degrade to the same blind sweep, never to
	 * silence.
	 */
	gps_acq_manager_set_search_mask(&manager, 0u);
	CHECK(gps_acq_manager_search_mask(&manager) == 0xffffffffu,
	      "an empty visibility mask silenced acquisition");
	printf("  empty visibility model degrades to a blind sweep\n");
}

/*
 * The other half of the cold-start question: a flash ephemeris is installed
 * while time is unknown, so what stops a months-old orbit reaching the
 * position solve?
 *
 * gps_collect_completed_epoch_locked() qualifies every channel with
 * time_known = true against the epoch's own transmit time, which comes from
 * the decoded HOW and not from anything persisted.  So the age check is
 * unavoidable by the time a measurement can influence a fix, however the
 * ephemeris got into the cache.
 */
static void test_stale_ephemeris_cannot_reach_the_solve(void)
{
	gps_ephemeris_t eph;
	memset(&eph, 0, sizeof(eph));
	eph.ephemeris_valid = true;
	eph.SVID = 5u;
	eph.iode = 0x30u;
	eph.iodc = 0x30u;
	eph.toe.sec = 100000.0;
	eph.toc.sec = 100000.0;
	eph.toe.week = 2431;
	eph.toe_week_valid = true;
	eph.A = 26560000.0;
	eph.sqrta = 5153.6;
	eph.ecc = 0.006;
	eph.sva = 2u;
	eph.fit_flag = false;   /* nominal four hour curve fit */

	/* While time is unknown only the structural rules can apply - this is the
	 * cold-start window, and it is why the epoch gate exists. */
	CHECK(gps_ephemeris_check(&eph, 5u, false, 0, 0.0) == GPS_EPHEMERIS_OK,
	      "structural check rejected a well-formed record");

	/* Inside the fit interval: usable. */
	CHECK(gps_ephemeris_check(&eph, 5u, true, 2431, 103000.0) ==
	      GPS_EPHEMERIS_OK,
	      "a record 50 minutes old was rejected");

	/* Past it: refused, whatever it claims about itself. */
	CHECK(gps_ephemeris_check(&eph, 5u, true, 2431, 100000.0 + 8000.0) ==
	      GPS_EPHEMERIS_REJECT_AGE,
	      "a record past its curve fit interval was accepted");
	/* A week later, expressed as a wrapped second-of-week - the shape a
	 * flash record takes after a long power-off. */
	CHECK(gps_ephemeris_check(&eph, 5u, true, 2432, 100000.0) ==
	      GPS_EPHEMERIS_REJECT_AGE,
	      "a week-old record was accepted");
	printf("  restored ephemeris is age-checked before it can reach a fix\n");
}

/* ================================================================== */
/* 2. The 32-bit sample counter wraps every 262 s.                     */
/* ================================================================== */

/*
 * Pseudoranges are differences of corrected sample counts against one
 * reference, scaled to metres.  The counter is 32 bits at 16.368 MHz, so it
 * wraps mid-flight roughly every four and a half minutes.  If the wrap leaked
 * into the differences it would put one satellite 2^31 samples - about 39
 * million kilometres - away from the rest, and the resulting position would be
 * wrong with a perfectly ordinary-looking residual on the other channels.
 *
 * Build one epoch twice, once away from the wrap and once straddling it, and
 * require the metre-domain pseudoranges to be identical.
 */
static void test_sample_wrap_does_not_disturb_pseudoranges(void)
{
	/* Six channels spread over 20 ms of capture skew, with sub-sample code
	 * phase corrections, which is the realistic shape of one epoch. */
	const int32_t offsets[6] = {0, -12345, 48000, -230000, 91000, 150000};
	const double code_phase[6] = {0.31, 0.77, 0.02, 0.55, 0.94, 0.18};

	double away[6];
	double across[6];

	for(unsigned int pass = 0u; pass < 2u; pass++)
	{
		/* Pass 0 sits mid-range; pass 1 puts the reference just past the
		 * wrap so half the channels are still near 0xFFFFFFFF. */
		uint32_t base = (pass == 0u) ? 0x40000000u : 0x00000064u;
		uint32_t reference = base;
		double lifted[6];
		for(unsigned int i = 0u; i < 6u; i++)
		{
			uint32_t raw = (uint32_t)((int64_t)base + offsets[i]);
			double corrected = (double)raw - code_phase[i];
			lifted[i] = gps_nav_epoch_unwrap_sample(raw, corrected,
			                                        reference);
		}
		/* Exactly what gps_ip.c forms: milliseconds relative to the
		 * reference, then metres. */
		for(unsigned int i = 0u; i < 6u; i++)
		{
			double delta_ms = (lifted[i] / SAMPLES_PER_MS) -
			                  (lifted[0] / SAMPLES_PER_MS);
			double metres = delta_ms * (SPEED_OF_LIGHT / 1000.0);
			if(pass == 0u) { away[i] = metres; } else { across[i] = metres; }
		}
	}

	double worst = 0.0;
	for(unsigned int i = 0u; i < 6u; i++)
	{
		double difference = fabs(away[i] - across[i]);
		if(difference > worst)
		{
			worst = difference;
		}
	}
	printf("  sample counter wrap: worst pseudorange difference %.9f m\n",
	       worst);
	CHECK(worst < 1.0e-3, "the 32-bit sample wrap moved a pseudorange");

	/* The epoch selector's own comparisons must agree across the wrap too,
	 * otherwise a coherent epoch would be rejected once every 262 s. */
	/* Sized to the API's declared parameter, not to the count in use: the
	 * signature promises GPS_NAV_EPOCH_MAX_CANDIDATES entries even though it
	 * only reads candidate_count of them. */
	gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES];
	memset(candidates, 0, sizeof(candidates));
	gps_nav_epoch_selection_t selection;
	for(unsigned int i = 0u; i < 6u; i++)
	{
		candidates[i].week = 2431;
		candidates[i].tow_sec = 300000.0;
		candidates[i].raw_sample =
			(uint32_t)((int64_t)0x00000064 + offsets[i]);
	}
	CHECK(gps_nav_epoch_validate_complete(candidates, 6u, SYNC_WINDOW,
	                                      TOW_TOLERANCE, &selection),
	      "a coherent epoch straddling the counter wrap was rejected");
	CHECK(selection.selected_count == 6u, "epoch lost a channel at the wrap");
	printf("  epoch selection is wrap-safe\n");
}

/* ================================================================== */
/* 3. Satellites connecting and disconnecting.                         */
/* ================================================================== */

#define PERIOD_TICKS  ((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_SOLVE_PERIOD_MS / 1000u)
#define WINDOW_TICKS  (((uint64_t)GPS_PPS_CLOCK_HZ * \
                        GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS) / 1000u)
#define HOLDOFF_TICKS ((uint64_t)1000000)

static gps_nav_epoch_event_result_t push(gps_nav_epoch_assembly_t* assembly,
                                         uint64_t counter,
                                         unsigned int channel,
                                         uint32_t ready_mask)
{
	return gps_nav_epoch_assembly_push(assembly, counter, counter, channel,
	                                   ready_mask, PERIOD_TICKS,
	                                   WINDOW_TICKS, HOLDOFF_TICKS);
}

/*
 * An epoch is an all-or-nothing hardware measurement set: the expected channel
 * set is frozen when the first event of a period arrives, so the satellite
 * geometry of a published epoch cannot depend on interrupt timing.  These are
 * the transitions that have to hold that line.
 */
static void test_epoch_assembly_under_connect_and_disconnect(void)
{
	gps_nav_epoch_assembly_t assembly;
	uint64_t counter = 1000000u;

	/* --- a satellite acquired mid-epoch must wait for the next one --- */
	gps_nav_epoch_assembly_reset(&assembly);
	CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "first event rejected");
	CHECK(push(&assembly, counter + 10u, 1u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED, "second event rejected");
	/* Channel 4 has just locked, so the live ready mask has grown. */
	CHECK(push(&assembly, counter + 20u, 4u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_INVALID,
	      "a channel that joined mid-epoch was admitted to it");
	CHECK(push(&assembly, counter + 30u, 2u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED, "third event rejected");
	CHECK(push(&assembly, counter + 40u, 3u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_COMPLETE,
	      "the frozen expected set did not complete");
	printf("  a satellite acquired mid-epoch joins the next epoch, not this one\n");

	/* --- a satellite lost mid-epoch must release the epoch --- */
	gps_nav_epoch_assembly_reset(&assembly);
	CHECK(push(&assembly, counter, 0u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "push 0");
	CHECK(push(&assembly, counter + 5u, 1u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1");
	CHECK(push(&assembly, counter + 8u, 2u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 2");
	CHECK(push(&assembly, counter + 9u, 3u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 3");
	/* Channel 4 drops before it ever reports. Removing it must complete the
	 * epoch from what has already arrived rather than stall until the
	 * deadline. */
	CHECK(gps_nav_epoch_assembly_remove_channel(&assembly, 4u),
	      "removing a lost channel did not complete the epoch");
	CHECK(gps_nav_epoch_assembly_usable(&assembly,
	                                    GPS_NAV_MIN_WLS_SATELLITES),
	      "four surviving channels were not usable");
	printf("  a satellite lost mid-epoch releases the epoch immediately\n");

	/* --- losing one too many must not publish a short epoch --- */
	gps_nav_epoch_assembly_reset(&assembly);
	CHECK(push(&assembly, counter, 0u, 0x1fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "push 0");
	CHECK(push(&assembly, counter + 5u, 1u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1");
	CHECK(push(&assembly, counter + 8u, 2u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 2");
	(void)gps_nav_epoch_assembly_remove_channel(&assembly, 3u);
	(void)gps_nav_epoch_assembly_remove_channel(&assembly, 4u);
	CHECK(!gps_nav_epoch_assembly_usable(&assembly,
	                                     GPS_NAV_MIN_WLS_SATELLITES),
	      "three channels were reported usable for a four-unknown solve");
	printf("  three surviving channels are never offered as a fix\n");

	/* --- every satellite lost --- */
	gps_nav_epoch_assembly_reset(&assembly);
	CHECK(push(&assembly, counter, 0u, 0x03u) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "push 0");
	CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 0u),
	      "removing the only channel reported completion");
	CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 1u),
	      "removing the last channel reported completion");
	CHECK(!gps_nav_epoch_assembly_usable(&assembly, 1u),
	      "an emptied assembly was still usable");
	CHECK(!gps_nav_epoch_assembly_complete(&assembly),
	      "an emptied assembly reported complete");
	printf("  total loss leaves no assembly behind\n");

	/* --- the newest period supersedes an incomplete older one --- */
	gps_nav_epoch_assembly_reset(&assembly);
	CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "push 0");
	CHECK(push(&assembly, counter + 5u, 1u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED, "push 1");
	CHECK(push(&assembly, counter + PERIOD_TICKS, 2u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_RESTARTED,
	      "an event from the next period did not restart the epoch");
	printf("  a new capture period discards the incomplete previous one\n");

	/* --- a straggler from the previous period is not back-filled --- */
	CHECK(push(&assembly, counter, 3u, 0x0fu) == GPS_NAV_EPOCH_EVENT_STALE,
	      "an event from the previous period was folded into the new epoch");
	printf("  a late event from the previous period is dropped, not mixed in\n");

	/* --- a coalesced repeat must not be counted twice --- */
	gps_nav_epoch_assembly_reset(&assembly);
	CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "push 0");
	CHECK(push(&assembly, counter + 1u, 0u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_DUPLICATE,
	      "a repeated event for one channel was accepted twice");
	printf("  a coalesced repeat is rejected as a duplicate\n");

	/* --- an event neither in this epoch nor the next is an outlier --- */
	gps_nav_epoch_assembly_reset(&assembly);
	CHECK(push(&assembly, counter, 0u, 0x0fu) == GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "push 0");
	CHECK(push(&assembly, counter + WINDOW_TICKS + 1u, 1u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_OUTLIER,
	      "an event outside the coherence window was accepted");
	printf("  an out-of-window capture is refused, not averaged in\n");
}

/*
 * Coalescing: the capture mailbox holds only the newest measurement, so when
 * interrupts are late the receiver has to know how many epochs it skipped and
 * must never invent the missing ones.
 */
static void test_coalesced_captures_are_counted_not_invented(void)
{
	uint32_t gap = 0u;
	CHECK(gps_nav_capture_sequence_consistent(1000u, 5u,
	                                          1000u + PERIOD_TICKS, 6u,
	                                          PERIOD_TICKS, &gap),
	      "a single clean epoch was rejected");
	CHECK(gap == 1u, "single epoch reported a gap");

	CHECK(gps_nav_capture_sequence_consistent(1000u, 5u,
	                                          1000u + (3u * PERIOD_TICKS), 8u,
	                                          PERIOD_TICKS, &gap),
	      "three coalesced epochs were rejected");
	CHECK(gap == 3u, "coalesced gap miscounted");

	/* Counter and sequence disagreeing means the mailbox cannot be trusted. */
	CHECK(!gps_nav_capture_sequence_consistent(1000u, 5u,
	                                           1000u + (3u * PERIOD_TICKS), 6u,
	                                           PERIOD_TICKS, &gap),
	      "a sequence/counter mismatch was accepted");
	CHECK(!gps_nav_capture_sequence_consistent(1000u, 5u, 1000u, 6u,
	                                           PERIOD_TICKS, &gap),
	      "a counter that did not advance was accepted");
	CHECK(!gps_nav_capture_sequence_consistent(0u, 5u,
	                                           1000u + PERIOD_TICKS, 6u,
	                                           PERIOD_TICKS, &gap),
	      "an unseeded previous counter was accepted");
	printf("  coalesced captures are counted exactly, never synthesised\n");
}

/*
 * Publication is stricter than clustering: every channel in the frozen ready
 * mask must belong to one coherent epoch, or nothing is published.  A partial
 * cluster would change the satellite geometry according to interrupt timing,
 * which is exactly what produced large raw-position jumps before.
 */
static void test_partial_coherence_publishes_nothing(void)
{
	gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES];
	memset(candidates, 0, sizeof(candidates));
	gps_nav_epoch_selection_t selection;
	for(unsigned int i = 0u; i < 5u; i++)
	{
		candidates[i].week = 2431;
		candidates[i].tow_sec = 300000.0;
		candidates[i].raw_sample = 1000000u + (i * 100u);
	}
	CHECK(gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW,
	                                      TOW_TOLERANCE, &selection),
	      "a fully coherent set was rejected");

	/* One channel a full nav period away - a stale measurement that slipped
	 * through.  Four good ones remain, which is enough for a solve, and that
	 * is exactly why it must not be published as one. */
	candidates[4].raw_sample = 1000000u + (uint32_t)(SAMPLES_PER_MS * 200.0);
	CHECK(!gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW,
	                                       TOW_TOLERANCE, &selection),
	      "an incoherent epoch was published because a subset was solvable");
	CHECK(selection.selected_count == 4u,
	      "the diagnostic cluster did not report the coherent subset");

	/* A TOW disagreement is equally disqualifying even when the samples line
	 * up: it means two satellites disagree about what time it is. */
	candidates[4].raw_sample = 1000000u + 400u;
	candidates[4].tow_sec = 300000.0 + (10.0 * TOW_TOLERANCE);
	CHECK(!gps_nav_epoch_validate_complete(candidates, 5u, SYNC_WINDOW,
	                                       TOW_TOLERANCE, &selection),
	      "channels disagreeing about TOW were published as one epoch");
	printf("  an incoherent epoch publishes nothing, not its solvable subset\n");
}

/*
 * A non-finite transmit time must never reach the pseudorange arithmetic.
 * gps_pps_select_reference() is the last gate before it, and it screens the
 * whole set - not just the entries it happens to walk past before finding the
 * preferred reference satellite.
 */
static void test_reference_selection_screens_every_capture(void)
{
	unsigned int ids[4] = {3u, 7u, 11u, 19u};
	double times[4] = {10.0, 11.0, 12.0, 13.0};
	unsigned int reference = 99u;

	CHECK(gps_pps_select_reference(ids, times, 4u, true, 7u, &reference),
	      "a clean set was rejected");
	CHECK(reference == 1u, "the preferred reference satellite was not chosen");

	CHECK(gps_pps_select_reference(ids, times, 4u, false, 0u, &reference),
	      "a clean set without a preference was rejected");
	CHECK(reference == 0u, "the earliest capture was not chosen");

	/* The corrupt entry sits *after* the preferred satellite, which is the
	 * ordering an early return would walk straight past. */
	times[3] = NAN;
	CHECK(!gps_pps_select_reference(ids, times, 4u, true, 7u, &reference),
	      "a non-finite capture time downstream of the preferred reference "
	      "was not detected");
	CHECK(!gps_pps_select_reference(ids, times, 4u, false, 0u, &reference),
	      "a non-finite capture time was not detected without a preference");
	printf("  reference selection screens the whole capture set\n");
}

int main(void)
{
	test_cold_start_searches_every_prn();
	test_stale_ephemeris_cannot_reach_the_solve();
	test_sample_wrap_does_not_disturb_pseudoranges();
	test_epoch_assembly_under_connect_and_disconnect();
	test_coalesced_captures_are_counted_not_invented();
	test_partial_coherence_publishes_nothing();
	test_reference_selection_screens_every_capture();

	if(failures != 0)
	{
		fprintf(stderr, "%d check(s) failed\n", failures);
		return 1;
	}
	printf("gps pvt consistency tests passed\n");
	return 0;
}