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

#include <stdio.h>
#include <string.h>
#include "gps/gps_ip_defines.h"

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

int main(void)
{
	unsigned int sf[10];
	unsigned char iode = 0u;

	/* Subframe 1: IODC is split, two MSBs in word 3 bits 7..6, eight LSBs in
	 * word 8 bits 29..22.  The low byte must equal the IODE. */
	memset(sf, 0, sizeof(sf));
	sf[2] = (0x02u << 6);          /* IODC[9:8] = 2 */
	sf[7] = (0x5Au << 22);         /* IODC[7:0] = 0x5A */
	CHECK(gps_lnav_broadcast_iode(sf, 1u, &iode), "sf1 iode not extracted");
	CHECK(iode == 0x5Au, "sf1 iode wrong");

	/* Subframe 2: IODE in word 3 bits 29..22. */
	memset(sf, 0, sizeof(sf));
	sf[2] = (0xA3u << 22);
	CHECK(gps_lnav_broadcast_iode(sf, 2u, &iode), "sf2 iode not extracted");
	CHECK(iode == 0xA3u, "sf2 iode wrong");

	/* Subframe 3: IODE in word 10 bits 29..22. */
	memset(sf, 0, sizeof(sf));
	sf[9] = (0x17u << 22);
	CHECK(gps_lnav_broadcast_iode(sf, 3u, &iode), "sf3 iode not extracted");
	CHECK(iode == 0x17u, "sf3 iode wrong");

	/* Subframes 4 and 5 carry no issue of data. */
	CHECK(!gps_lnav_broadcast_iode(sf, 4u, &iode), "sf4 reported an iode");
	CHECK(!gps_lnav_broadcast_iode(sf, 5u, &iode), "sf5 reported an iode");
	CHECK(!gps_lnav_broadcast_iode(NULL, 2u, &iode), "null subframe accepted");
	CHECK(!gps_lnav_broadcast_iode(sf, 2u, NULL), "null output accepted");

	/* All three subframes of one data set must agree, which is what the
	 * cross-check relies on. */
	unsigned int set1[10], set2[10], set3[10];
	memset(set1,0,sizeof(set1)); memset(set2,0,sizeof(set2)); memset(set3,0,sizeof(set3));
	set1[2] = (0x01u << 6); set1[7] = (0x77u << 22);
	set2[2] = (0x77u << 22);
	set3[9] = (0x77u << 22);
	unsigned char a=0,b=0,c=0;
	gps_lnav_broadcast_iode(set1,1u,&a);
	gps_lnav_broadcast_iode(set2,2u,&b);
	gps_lnav_broadcast_iode(set3,3u,&c);
	CHECK((a==b)&&(b==c)&&(a==0x77u), "one data set gave inconsistent iode");

	/*
	 * Restoring a persisted page must repopulate the raw-word cache, not just
	 * the parsed model.  The periodic save rebuilds its payload from that
	 * cache, so a restore that only set the parsed form would write the page
	 * back as absent and wipe the stored model on the first save after every
	 * reboot.  That regression reached hardware once and was caught only by
	 * watching "iono=0" appear in the save telemetry.
	 */
	unsigned int page18[10];
	memset(page18, 0, sizeof(page18));
	/* data ID 1, SV ID 56 -> subframe 4 page 18, with non-zero coefficients. */
	page18[2] = (1u << 28) | (56u << 22) | (0x1au << 14) | (0x0bu << 6);
	page18[3] = (0x21u << 22) | (0x33u << 14) | (0x44u << 6);
	page18[4] = (0x55u << 22) | (0x66u << 14) | (0x77u << 6);
	page18[5] = (0x12u << 22) | (0x34u << 14) | (0x56u << 6);

	uint32_t words[10];
	CHECK(!gps_lnav_get_iono_page(words),
	      "iono page reported before anything was stored");
	CHECK(gps_lnav_restore_iono_page(page18), "restoring page 18 failed");
	CHECK(gps_lnav_get_iono_page(words),
	      "restored iono page did not repopulate the raw-word cache");
	for(unsigned int i = 0u; i < 10u; i++)
	{
		CHECK(words[i] == (uint32_t)page18[i],
		      "restored iono raw words differ from what was restored");
	}

	/* Same contract for almanac pages. */
	unsigned int alm[10];
	memset(alm, 0, sizeof(alm));
	alm[2] = (1u << 28) | (5u << 22) | (0x0f3eu << 6); /* SV ID 5 */
	alm[3] = (36u << 22) | (2439u << 6);
	alm[4] = (0xfd50u << 14);
	alm[5] = (10554475u << 6);
	uint32_t pages[GPS_ALMANAC_PRN_COUNT][10];
	CHECK(gps_lnav_restore_almanac_page(alm, 5u),
	      "restoring an almanac page failed");
	uint32_t mask = gps_lnav_get_almanac_pages(pages);
	CHECK((mask & (1u << 4)) != 0u,
	      "restored almanac page not present in the raw-word cache");
	for(unsigned int i = 0u; i < 10u; i++)
	{
		CHECK(pages[4][i] == (uint32_t)alm[i],
		      "restored almanac raw words differ from what was restored");
	}


	/*
	 * Toe carries no week on the air.  The decoder reconstructs it from the
	 * subframe 1 week number and the handover word of the subframe that
	 * carried Toe, which is the only self-consistent pair available: Toe is
	 * within a couple of hours of its own transmission, so the week that puts
	 * it closest to that transmission is the right one.
	 *
	 * Without this the curve-fit age check works modulo one week and a record
	 * exactly N weeks old reads as current - harmless while ephemeris lives in
	 * RAM, reachable the moment it is persisted across a power-off.
	 */
	{
		struct { const char* name; unsigned int wn; double how_tow;
		         double toe_sec; int expect_week; } cases[] = {
			/* HOW TOW is a count of six-second units and Toe is a 16-bit
			 * field scaled by 16, so both are quantised on the air. */
			{"mid-week",            430u, 172800.0, 172800.0, 430},
			{"toe two hours ahead", 430u, 172800.0, 180000.0, 430},
			/* Broadcast at the end of the week, Toe early in the next one. */
			{"toe crosses forward", 430u, 604194.0,   1808.0, 431},
			/* Broadcast just after the rollover, Toe late in the previous. */
			{"toe crosses back",    431u,   1806.0, 604192.0, 430},
		};
		for(unsigned int c = 0u; c < 4u; c++)
		{
			gps_frame_t frame;
			unsigned int s1[10], s2[10], s3[10];
			memset(&frame, 0, sizeof(frame));
			memset(s1, 0, sizeof(s1));
			memset(s2, 0, sizeof(s2));
			memset(s3, 0, sizeof(s3));

			/* HOW: TOW count in six-second units, then the subframe id. */
			unsigned int how = (unsigned int)(cases[c].how_tow / 6.0);
			s1[1] = (how << 13) | (1u << 8);
			s2[1] = (how << 13) | (2u << 8);
			s3[1] = (how << 13) | (3u << 8);

			/* Subframe 1: week number and the IODC whose low byte is the IODE. */
			s1[2] = (cases[c].wn << 20);
			s1[7] = (0x64u << 22);
			/* Subframes 2 and 3 carry the matching IODE, and 2 carries Toe. */
			s2[2] = (0x64u << 22);
			s2[9] = (((unsigned int)(cases[c].toe_sec / 16.0)) << 14);
			s3[9] = (0x64u << 22);

			parse_subframe(s1, &frame, NULL);
			parse_subframe(s2, &frame, NULL);
			parse_subframe(s3, &frame, NULL);

			CHECK(frame.gps_ephems.ephemeris_valid,
			      "consistent ephemeris set was not committed");
			CHECK(frame.gps_ephems.toe_week_valid,
			      "toe week was not resolved");
			if(frame.gps_ephems.toe.week != cases[c].expect_week)
			{
				fprintf(stderr, "FAIL: %s: toe week %d, expected %d\n",
				        cases[c].name, frame.gps_ephems.toe.week,
				        cases[c].expect_week);
				failures++;
			}
			CHECK(frame.gps_ephems.toe.sec == cases[c].toe_sec,
			      "toe seconds did not survive the round trip");
		}
	}

	if(failures==0) puts("gps lnav iode tests passed");
	return failures==0?0:1;
}