onur
30179df8e6
Remove the fixed pseudorange start offset, preserve per-PRN navigation assistance across RF loss and power cycles, and seed coherent navigation measurement phase on reacquisition. Add full LNAV almanac visibility scheduling, power-safe QSPI journaling, RF-domain reset isolation, and PPS Gray CDC hardening with Linux, sanitizer, RTL, and end-to-end coverage.
235 lines
7.5 KiB
C
235 lines
7.5 KiB
C
#include <assert.h>
|
|
#include <math.h>
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#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;
|
|
}
|