/* * 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 #include #include #include #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; }