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

/*
 * Almanac decoder, orbital propagation and visibility-mask tests.
 *
 * The reference vector is a real broadcast almanac (PRN 1, GPS week 2431)
 * quantised to the LNAV representation, with the expected ECEF position
 * produced by an independent Python implementation of the IS-GPS-200
 * user algorithm.  That checks the bit layout and the propagation against
 * something other than this code's own arithmetic.
 */

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

#include "gps/gps_almanac.h"

/* -std=c11 does not expose TEST_PI. */
#define TEST_PI 3.1415926535897932

/*
 * Place a field into a 30-bit LNAV word.  first_bit is 1-based from the MSB
 * of the word and the low six bits are parity, matching IS-GPS-200 and the
 * decoder under test.
 */
static void put_field(unsigned int* word, unsigned int first_bit,
                      unsigned int length, uint32_t value)
{
	unsigned int shift = 30u - (first_bit + length - 1u);
	uint32_t mask = (length >= 32u) ? 0xffffffffu : ((1u << length) - 1u);
	*word &= ~(mask << shift);
	*word |= (value & mask) << shift;
}

static void build_almanac_page(unsigned int subframe[10],
                               unsigned char subframe_id,
                               unsigned char sv_id,
                               uint32_t ecc, uint32_t toa, int32_t delta_i,
                               int32_t omega_dot, uint32_t health,
                               uint32_t sqrt_a, int32_t omega0,
                               int32_t omega, int32_t m0,
                               int32_t af0, int32_t af1)
{
	memset(subframe, 0, sizeof(unsigned int) * 10u);
	/* Word 2 carries the subframe ID at bits 20..22. */
	put_field(&subframe[1], 20u, 3u, subframe_id);
	put_field(&subframe[2], 1u, 2u, 1u); /* data ID = 1 (LNAV) */
	put_field(&subframe[2], 3u, 6u, sv_id);
	put_field(&subframe[2], 9u, 16u, ecc);
	put_field(&subframe[3], 1u, 8u, toa);
	put_field(&subframe[3], 9u, 16u, (uint32_t)delta_i);
	put_field(&subframe[4], 1u, 16u, (uint32_t)omega_dot);
	put_field(&subframe[4], 17u, 8u, health);
	put_field(&subframe[5], 1u, 24u, sqrt_a);
	put_field(&subframe[6], 1u, 24u, (uint32_t)omega0);
	put_field(&subframe[7], 1u, 24u, (uint32_t)omega);
	put_field(&subframe[8], 1u, 24u, (uint32_t)m0);
	/* af0 is split: bits 1..8 are the MSBs, bits 20..22 the LSBs. */
	put_field(&subframe[9], 1u, 8u, ((uint32_t)af0 >> 3) & 0xffu);
	put_field(&subframe[9], 9u, 11u, (uint32_t)af1);
	put_field(&subframe[9], 20u, 3u, (uint32_t)af0 & 0x7u);
}

/* Real broadcast almanac for PRN 1, quantised to the LNAV fields. */
#define REF_ECC       3902
#define REF_TOA       36
#define REF_DELTA_I   2439
#define REF_OMEGA_DOT (-691)
#define REF_SQRT_A    10554475
#define REF_OMEGA0    677285
#define REF_OMEGA     459593
#define REF_M0        301402
#define REF_WEEK      2431u
#define REF_TOW       550000.0

static void build_reference_page(unsigned int subframe[10], unsigned char sv_id,
                                 uint32_t health)
{
	build_almanac_page(subframe, 5u, sv_id, REF_ECC, REF_TOA, REF_DELTA_I,
	                   REF_OMEGA_DOT, health, REF_SQRT_A, REF_OMEGA0,
	                   REF_OMEGA, REF_M0, -5, 3);
}

static void test_field_decoding(void)
{
	unsigned int subframe[10];
	gps_almanac_t almanac;
	gps_almanac_reset(&almanac);
	build_reference_page(subframe, 1u, 0u);

	assert(gps_almanac_store_page(&almanac, subframe, 5u));
	const gps_almanac_entry_t* e = &almanac.sv[0];
	assert(e->valid);
	assert(e->prn == 1u);
	assert(e->health == 0u);

	assert(fabs(e->ecc - (REF_ECC * ldexp(1.0, -21))) < 1e-15);
	assert(fabs(e->toa_sec - (REF_TOA * 4096.0)) < 1e-9);
	assert(fabs(e->sqrt_a - (REF_SQRT_A * ldexp(1.0, -11))) < 1e-9);
	/* Negative OMEGADOT must survive sign extension of a 16-bit field. */
	assert(e->omega_dot_rps < 0.0);
	assert(fabs(e->omega_dot_rps -
	            (REF_OMEGA_DOT * ldexp(1.0, -38) * TEST_PI)) < 1e-18);
	/* Inclination is an offset from 0.30 semicircles, not an absolute value. */
	assert(fabs(e->i0_rad -
	            ((0.30 + (REF_DELTA_I * ldexp(1.0, -19))) * TEST_PI)) < 1e-12);
	/* af0 is reassembled from two separated bit groups. */
	assert(fabs(e->af0_sec - (-5.0 * ldexp(1.0, -20))) < 1e-18);
	assert(fabs(e->af1_sec_per_sec - (3.0 * ldexp(1.0, -38))) < 1e-24);
	assert(gps_almanac_valid_count(&almanac) == 1u);
	printf("  field decoding ok\n");
}

static void test_propagation_against_reference(void)
{
	unsigned int subframe[10];
	gps_almanac_t almanac;
	gps_almanac_reset(&almanac);
	build_reference_page(subframe, 1u, 0u);
	assert(gps_almanac_store_page(&almanac, subframe, 5u));

	double ecef[3];
	assert(gps_almanac_satellite_ecef(&almanac.sv[0], REF_WEEK, REF_WEEK,
	                                  REF_TOW, ecef));

	/*
	 * Independent Python implementation of the same user algorithm, with tk as
	 * the actual elapsed time since toa - here 402544 s, or 4.66 days.
	 *
	 * This vector replaces an earlier one generated with tk wrapped into the
	 * half week.  That wrap was a misreading of IS-GPS-200 20.3.3.5.2.3, which
	 * is a week-crossover correction rather than a general reduction, and it
	 * put this satellite 5161 km from where it actually is - evaluating the
	 * orbit 2.3 days *before* toa instead of 4.7 days after it.
	 */
	const double expected[3] = {19464181.4246, 11780273.3193, 13764877.9908};
	for(unsigned int i = 0u; i < 3u; i++)
	{
		double error = fabs(ecef[i] - expected[i]);
		if(error > 1.0)
		{
			printf("  axis %u: got %.4f expected %.4f (error %.4f m)\n",
			       i, ecef[i], expected[i], error);
		}
		assert(error <= 1.0);
	}

	double radius = sqrt((ecef[0] * ecef[0]) + (ecef[1] * ecef[1]) +
	                     (ecef[2] * ecef[2]));
	assert(fabs(radius - 26591.4e3) < 5.0e3);
	printf("  propagation matches reference within 1 m\n");
}

static void test_week_rollover_is_symmetric(void)
{
	unsigned int subframe[10];
	gps_almanac_t almanac;
	gps_almanac_reset(&almanac);
	build_reference_page(subframe, 1u, 0u);
	assert(gps_almanac_store_page(&almanac, subframe, 5u));

	/*
	 * The same instant expressed as (week, tow) or (week+1, tow-604800) must
	 * propagate to the same place, otherwise a week boundary would step the
	 * predicted satellite position and briefly corrupt the visible set.
	 */
	double a[3];
	double b[3];
	assert(gps_almanac_satellite_ecef(&almanac.sv[0], REF_WEEK, REF_WEEK,
	                                  600000.0, a));
	assert(gps_almanac_satellite_ecef(&almanac.sv[0], REF_WEEK, REF_WEEK + 1u,
	                                  600000.0 - 604800.0, b));
	for(unsigned int i = 0u; i < 3u; i++)
	{
		assert(fabs(a[i] - b[i]) < 1.0);
	}
	printf("  week rollover consistent\n");
}

static void test_page_rejection(void)
{
	unsigned int subframe[10];
	gps_almanac_t almanac;
	gps_almanac_reset(&almanac);

	/* SV ID 1..24 belongs to subframe 5; the same page on subframe 4 is not
	 * an almanac page and must not be stored. */
	build_reference_page(subframe, 1u, 0u);
	assert(!gps_almanac_store_page(&almanac, subframe, 4u));

	/* SV ID 56 on subframe 4 is the ionosphere page, not an almanac. */
	build_almanac_page(subframe, 4u, 56u, REF_ECC, REF_TOA, REF_DELTA_I,
	                   REF_OMEGA_DOT, 0u, REF_SQRT_A, REF_OMEGA0,
	                   REF_OMEGA, REF_M0, 0, 0);
	assert(!gps_almanac_store_page(&almanac, subframe, 4u));

	/* An all-zero (never uploaded) page has sqrt_a = 0 and must be rejected
	 * rather than stored as an entry that divides by zero when propagated. */
	build_almanac_page(subframe, 5u, 7u, 0u, 0u, 0, 0, 0u, 0u, 0, 0, 0, 0, 0);
	assert(!gps_almanac_store_page(&almanac, subframe, 5u));

	assert(gps_almanac_valid_count(&almanac) == 0u);
	printf("  page rejection ok\n");
}

/* Receiver position used for the visibility tests (Bolu, Turkey). */
static void receiver_ecef(double out[3])
{
	const double lat = 40.735696 * TEST_PI / 180.0;
	const double lon = 31.598649 * TEST_PI / 180.0;
	const double h = 803.0;
	const double a = 6378137.0;
	const double f = 1.0 / 298.257223563;
	const double e2 = f * (2.0 - f);
	double n = a / sqrt(1.0 - (e2 * sin(lat) * sin(lat)));
	out[0] = (n + h) * cos(lat) * cos(lon);
	out[1] = (n + h) * cos(lat) * sin(lon);
	out[2] = ((n * (1.0 - e2)) + h) * sin(lat);
}

static void test_visibility_fallbacks(void)
{
	gps_almanac_t almanac;
	uint32_t mask = 0u;
	double rx[3];
	receiver_ecef(rx);

	/* No almanac at all: everything stays searchable. */
	gps_almanac_reset(&almanac);
	assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
	                                &mask) == 0u);
	assert(mask == 0xffffffffu);

	/*
	 * Almanac entries but no subframe 5 page 25 yet: visibility must still be
	 * usable, falling back to the current week.  Page 25 comes round only
	 * once per 12.5 minutes and waiting for it would leave the receiver
	 * sweeping blindly for that whole window after a reset.
	 */
	unsigned int subframe[10];
	build_reference_page(subframe, 1u, 0u);
	assert(gps_almanac_store_page(&almanac, subframe, 5u));
	assert(!almanac.reference_valid);
	mask = 0u;
	assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
	                                &mask) == 1u);
	assert((mask & 1u) != 0u);

	/*
	 * And that fallback must agree with the explicit reference when the
	 * reference does arrive, for an almanac of the current week.
	 */
	uint32_t mask_with_reference = 0u;
	gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
	assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
	                                &mask_with_reference) == 1u);
	assert(mask_with_reference == mask);
	gps_almanac_reset(&almanac);
	assert(gps_almanac_store_page(&almanac, subframe, 5u));

	/* No receiver position: all searchable. */
	gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
	mask = 0u;
	assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, NULL, 0.0,
	                                &mask) == 0u);
	assert(mask == 0xffffffffu);
	printf("  visibility fallbacks ok\n");
}

static void test_visibility_selects_and_never_hides_unknowns(void)
{
	gps_almanac_t almanac;
	unsigned int subframe[10];
	uint32_t mask = 0u;
	double rx[3];
	receiver_ecef(rx);

	gps_almanac_reset(&almanac);
	gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
	build_reference_page(subframe, 1u, 0u);
	assert(gps_almanac_store_page(&almanac, subframe, 5u));

	/* PRN 1 is above the horizon for this place and time (verified against
	 * the same almanac independently), so bit 0 must be set. */
	double elevation = 0.0;
	assert(gps_almanac_look_angles(&almanac.sv[0], REF_WEEK, REF_WEEK,
	                               REF_TOW, rx, &elevation, NULL));
	assert(elevation > 0.0);
	assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
	                                &mask) >= 1u);
	assert((mask & 1u) != 0u);

	/* Every PRN with no almanac entry yet must stay searchable, or a
	 * partially collected almanac would hide satellites we simply have not
	 * heard about. */
	for(unsigned int prn_index = 1u; prn_index < 32u; prn_index++)
	{
		assert((mask & (1u << prn_index)) != 0u);
	}

	/* An unhealthy satellite is dropped from the visible set even though it
	 * is geometrically up. */
	gps_almanac_reset(&almanac);
	gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
	build_reference_page(subframe, 1u, 0x3fu);
	assert(gps_almanac_store_page(&almanac, subframe, 5u));
	mask = 0xffffffffu;
	assert(gps_almanac_visible_mask(&almanac, REF_WEEK, REF_TOW, rx, 0.0,
	                                &mask) == 0u);
	assert((mask & 1u) == 0u);
	printf("  visibility selection ok\n");
}

static void test_satellite_sets_below_and_rises(void)
{
	gps_almanac_t almanac;
	unsigned int subframe[10];
	double rx[3];
	receiver_ecef(rx);
	gps_almanac_reset(&almanac);
	gps_almanac_set_reference(&almanac, REF_WEEK, REF_TOA * 4096.0);
	build_reference_page(subframe, 1u, 0u);
	assert(gps_almanac_store_page(&almanac, subframe, 5u));

	/*
	 * Sweep a full orbit.  A real GPS satellite must spend a substantial part
	 * of the day both above and below the horizon; a propagation sign error
	 * or a bad inclination typically pins it to one side.
	 */
	unsigned int above = 0u;
	unsigned int below = 0u;
	for(unsigned int step = 0u; step < 720u; step++)
	{
		double tow = 100000.0 + (step * 60.0);
		double elevation = 0.0;
		assert(gps_almanac_look_angles(&almanac.sv[0], REF_WEEK, REF_WEEK,
		                               tow, rx, &elevation, NULL));
		assert(elevation >= -90.0 && elevation <= 90.0);
		if(elevation > 0.0)
		{
			above++;
		}
		else
		{
			below++;
		}
	}
	assert(above > 60u);
	assert(below > 60u);
	printf("  rise/set over 12 h: %u above, %u below\n", above, below);
}

int main(void)
{
	test_field_decoding();
	test_propagation_against_reference();
	test_week_rollover_is_symmetric();
	test_page_rejection();
	test_visibility_fallbacks();
	test_visibility_selects_and_never_hides_unknowns();
	test_satellite_sets_below_and_rises();
	printf("gps almanac tests passed\n");
	return 0;
}