#include #include #include #include #include #include #include "gps/NavEquations.h" #include "gps/gps_almanac.h" #define TEST_PI 3.1415926535897932384626433832795 static uint32_t unsigned_field(int32_t value, unsigned int bits) { uint32_t mask = (UINT32_C(1) << bits) - 1u; return (uint32_t)value & mask; } static void make_orbit_page( unsigned int words[10], unsigned int sv_id, unsigned int toa_raw, unsigned char health8) { const uint32_t eccentricity_raw = 0x1234u; const int32_t delta_i_raw = -1234; const int32_t omega_dot_raw = -321; const uint32_t sqrt_a_raw = 10555000u; const int32_t omega0_raw = -345678; const int32_t argument_raw = 456789; const int32_t mean_anomaly_raw = -567890; const int32_t af0_raw_signed = -100; const int32_t af1_raw_signed = 50; uint32_t af0_raw = unsigned_field(af0_raw_signed, 11u); uint32_t af1_raw = unsigned_field(af1_raw_signed, 11u); memset(words, 0, sizeof(unsigned int) * 10u); words[2] = (1u << 28) | ((sv_id & 0x3fu) << 22) | (eccentricity_raw << 6); words[3] = ((toa_raw & 0xffu) << 22) | (unsigned_field(delta_i_raw, 16u) << 6); words[4] = (unsigned_field(omega_dot_raw, 16u) << 14) | ((uint32_t)health8 << 6); words[5] = sqrt_a_raw << 6; words[6] = unsigned_field(omega0_raw, 24u) << 6; words[7] = unsigned_field(argument_raw, 24u) << 6; words[8] = unsigned_field(mean_anomaly_raw, 24u) << 6; words[9] = ((af0_raw >> 3) << 22) | (af1_raw << 11) | ((af0_raw & 0x07u) << 8); } static void make_sf5_reference( unsigned int words[10], unsigned int toa_raw, unsigned int week_mod256, const unsigned char health6[GPS_ALMANAC_PRN_COUNT]) { memset(words, 0, sizeof(unsigned int) * 10u); words[2] = (1u << 28) | (51u << 22) | ((toa_raw & 0xffu) << 14) | ((week_mod256 & 0xffu) << 6); for(unsigned int prn = 0u ; prn < 24u ; prn++) { unsigned int word = 3u + (prn / 4u); unsigned int shift = 24u - (6u * (prn % 4u)); words[word] |= (uint32_t)(health6[prn] & 0x3fu) << shift; } } static void make_sf4_health( unsigned int words[10], const unsigned char health6[GPS_ALMANAC_PRN_COUNT]) { memset(words, 0, sizeof(unsigned int) * 10u); words[2] = (1u << 28) | (63u << 22); words[7] = (uint32_t)(health6[24] & 0x3fu) << 6; for(unsigned int prn = 25u ; prn <= 28u ; prn++) { unsigned int shift = 24u - (6u * (prn - 25u)); words[8] |= (uint32_t)(health6[prn] & 0x3fu) << shift; } for(unsigned int prn = 29u ; prn <= 31u ; prn++) { unsigned int shift = 24u - (6u * (prn - 29u)); words[9] |= (uint32_t)(health6[prn] & 0x3fu) << shift; } } static void ingest_constellation( gps_almanac_cache_t* cache, unsigned int toa_raw, unsigned int week_mod256, int skipped_prn, int unavailable_prn, uint64_t tick) { unsigned int words[10]; unsigned char health6[GPS_ALMANAC_PRN_COUNT] = {0}; if(unavailable_prn >= 0) { health6[unavailable_prn] = GPS_ALMANAC_UNAVAILABLE_HEALTH; } for(unsigned int prn = 0u ; prn < GPS_ALMANAC_PRN_COUNT ; prn++) { if((int)prn == skipped_prn) { continue; } make_orbit_page(words, prn + 1u, toa_raw, 0u); assert(gps_almanac_cache_ingest( cache, (prn < 24u) ? 5u : 4u, words, tick + prn)); } make_sf4_health(words, health6); assert(gps_almanac_cache_ingest(cache, 4u, words, tick + 40u)); make_sf5_reference(words, toa_raw, week_mod256, health6); assert(gps_almanac_cache_ingest(cache, 5u, words, tick + 41u)); } static void test_page_bit_layout_and_zero_based_mapping(void) { gps_almanac_cache_t cache; gps_almanac_cache_init(&cache, 10000u); unsigned int words[10]; make_orbit_page(words, 1u, 40u, 0x12u); assert(gps_almanac_cache_ingest(&cache, 5u, words, 100u)); const gps_almanac_entry_t* entry = &cache.entry[0]; assert(entry->valid); assert(entry->health8 == 0x12u); assert(fabs(entry->eccentricity - (0x1234u * 0x1p-21)) < 1e-15); assert(entry->toa_sec == 40.0 * 4096.0); assert(fabs(entry->delta_i_rad - (-1234.0 * 0x1p-19 * TEST_PI)) < 1e-15); assert(fabs(entry->omega_dot_rad_per_sec - (-321.0 * 0x1p-38 * TEST_PI)) < 1e-20); assert(fabs(entry->sqrt_a - (10555000.0 * 0x1p-11)) < 1e-12); assert(fabs(entry->af0_sec - (-100.0 * 0x1p-20)) < 1e-15); assert(fabs(entry->af1_sec_per_sec - (50.0 * 0x1p-38)) < 1e-20); assert(!cache.entry[1].valid); /* Identical repeats refresh RAM age without reporting a content change to * the flash persistence policy. */ assert(!gps_almanac_cache_ingest(&cache, 5u, words, 200u)); assert(cache.entry[0].received_tick == 200u); } static void test_complete_requires_reference_health_and_every_available_orbit(void) { gps_almanac_cache_t cache; gps_almanac_cache_init(&cache, 10000u); const gpstime_t gps_time = {.week = 2300, .sec = 164000.0}; unsigned int week_mod256 = (unsigned int)gps_time.week & 0xffu; ingest_constellation( &cache, 40u, week_mod256, 31, -1, 100u); gps_almanac_progress_t progress; assert(gps_almanac_cache_progress( &cache, 200u, gps_time, &progress)); assert(!progress.complete); assert(progress.matching_orbit_count == 31u); assert(progress.missing_count == 1u); unsigned int words[10]; make_orbit_page(words, 32u, 40u, 0u); assert(gps_almanac_cache_ingest(&cache, 4u, words, 201u)); assert(gps_almanac_cache_complete(&cache, 201u, gps_time)); } static void test_explicit_unavailable_satellite_allows_completion(void) { gps_almanac_cache_t cache; gps_almanac_cache_init(&cache, 10000u); const gpstime_t gps_time = {.week = 2300, .sec = 164000.0}; ingest_constellation( &cache, 40u, (unsigned int)gps_time.week & 0xffu, 31, 31, 100u); assert(gps_almanac_cache_complete(&cache, 200u, gps_time)); assert(gps_almanac_cache_prn_unavailable( &cache, 31u, 200u, gps_time)); gps_ephemeris_t ephemeris; assert(!gps_almanac_cache_to_ephemeris( &cache, 31u, 200u, gps_time, &ephemeris, NULL)); } static void test_conversion_week_health_and_staleness(void) { gps_almanac_cache_t cache; gps_almanac_cache_init(&cache, 1000u); const gpstime_t gps_time = {.week = 2300, .sec = 164000.0}; ingest_constellation( &cache, 40u, (unsigned int)gps_time.week & 0xffu, -1, -1, 100u); gps_ephemeris_t ephemeris; bool healthy = false; assert(gps_almanac_cache_to_ephemeris( &cache, 0u, 200u, gps_time, &ephemeris, &healthy)); assert(healthy); assert(ephemeris.SVID == 0u); assert(ephemeris.WN == 2300u); assert(ephemeris.toe.sec == 40.0 * 4096.0); assert(fabs(ephemeris.inc0 - ((0.30 * TEST_PI) + cache.entry[0].delta_i_rad)) < 1e-15); assert(ephemeris.A == ephemeris.sqrta * ephemeris.sqrta); double position_m[3] = {0.0, 0.0, 0.0}; double velocity_mps[3] = {0.0, 0.0, 0.0}; double clock[2] = {0.0, 0.0}; double clock_correction = 0.0; satpos(&ephemeris, position_m, velocity_mps, clock, &clock_correction); double radius_m = sqrt( (position_m[0] * position_m[0]) + (position_m[1] * position_m[1]) + (position_m[2] * position_m[2])); assert(isfinite(radius_m)); assert(radius_m > 2.0e7); assert(radius_m < 3.0e7); assert(!gps_almanac_cache_complete(&cache, 1200u, gps_time)); gpstime_t far_time = gps_time; far_time.sec += 8.0 * 86400.0; far_time.week++; far_time.sec -= 604800.0; assert(!gps_almanac_cache_complete(&cache, 200u, far_time)); } int main(void) { test_page_bit_layout_and_zero_based_mapping(); test_complete_requires_reference_health_and_every_available_orbit(); test_explicit_unavailable_satellite_allows_completion(); test_conversion_week_health_and_staleness(); puts("gps LNAV SF4/SF5 almanac tests passed"); return 0; }