fix(gps): correct week-number handling across almanac, ephemeris and flash
Persisting navigation data changed its lifetime from one power cycle to weeks, and every piece of arithmetic that had only ever run over the shorter lifetime turned out to be wrong. Each fix below reproduces in a host test that fails against the previous behaviour. Almanac (gps_almanac.c, gps_lnav_parser.c) WNa was lifted against gps_ephems.toe.week, a field nothing ever writes, so any WNa above 128 underflowed to ~4.29e9. The propagation then reduced tk with a while-loop, turning one visibility refresh into 4.29e9 iterations - hours on the target, inside a 10 s periodic call. WNa is the week modulo 256, so this armed at GPS week 2433 and then for 127 weeks in every 256. Now lifted against tow.week in signed arithmetic, refused if impossible, and the propagation is closed form. The IS-GPS-200 20.3.3.5.2.3 half-week wrap was also applied to tk unconditionally. It is a week-crossover correction, not a general reduction: a GPS week is 14.0384 orbital periods, so aliasing one week moves a satellite 5161 km, enough to report one 5.1 degrees below the horizon as 5.7 above. The crossover guess now lives in gps_almanac_assumed_week(), used only before page 25 arrives, and an almanac too old to propagate yields no prediction rather than a wrong one. Ephemeris age (gps_ephemeris_validity.c, gps_lnav_parser.c) The curve-fit check compared two second-of-week values and never read a week at all, so a record exactly N weeks old read as current. Harmless while ephemeris lived in RAM; directly reachable once persisted across a power-off. gps_resolve_toe_week() now reconstructs Toe's week at commit time from the subframe 1 week number and the handover word; an unresolvable week yields no age, which reads as a rejection. A restored record could also outrank the live broadcast, because the Toe ordering inverts past half a week. Flash records are now marked restored and are candidates, never incumbents. Flash wear (gps_nav_context_store.c, gps_interrupt_handlers.c) The CRC wear policy never engaged: the payload carried the live WLS position and a receiver timestamp nothing on the restore path read, so every 60 s save was an erase - about 139 days to wear-out. The seed is now held and refreshed only on 5 km of movement, at most hourly: one erase per day instead of 1440. Measurement model (NavEquations.c) The elevation weighting was floored at 10 degrees, capping its term at 17.3 m where the model's own answer is 34.4 m at 5 degrees and 86 m at 2, so a low satellite was trusted several times more than the model allowed. Floor lowered to 3 degrees, which is where 1/sin must still be bounded because a zero weight fails the whole epoch. Measured on a six-satellite geometry, 30 m of uncorrected delay now moves the fix 4.18 m through a 4-degree satellite against 48.50 m through a 72-degree one. Both atmospheric corrections switched off below the horizon, so a satellite crossing it lost 78.8 m of tropospheric delay in one step. Elevation is now clamped at the horizon for the models, and the extrapolation is paid for in variance. Also: gps_pps_select_reference() returned early on the preferred PRN, leaving later non-finite capture times unscreened. Checked and found correct, with regressions added: the epoch assembly under satellite connect/disconnect and reassignment, coalesced captures, the 32-bit RF sample counter wrap (pseudoranges agree to 3.3 um across it), the refusal to publish a solvable subset of an incoherent epoch, and the Klobuchar model against IS-GPS-200 20.3.3.5.2.5. Tests: 22 host suites, 5 RTL testbenches, sanitizer clean. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
This commit is contained in:
onur
@@ -89,6 +89,19 @@
|
||||
void topocent(const double* XPtr, const double* dxPtr, int size, t_phi_Lambda_h * phi_Lambda_h, t_topocentData* topcent);
|
||||
void togeod(double a, double finv, double X, double Y, double Z, t_phi_Lambda_h * phi_Lambda_h, t_topocentData* topcent);
|
||||
double tropo(double sinel, double hsta);
|
||||
/*
|
||||
* One measurement's standard deviation in metres: the URA the
|
||||
* satellite broadcasts, the residual atmosphere implied by its
|
||||
* elevation, and the code-tracking jitter implied by its
|
||||
* carrier-to-noise density, combined in variance. This is what
|
||||
* decides how far one satellite can move the fix, so it is
|
||||
* exposed for host testing rather than kept file-static.
|
||||
*/
|
||||
double gps_nav_measurement_sigma_m(unsigned char ura_index,
|
||||
double elevation_deg,
|
||||
bool elevation_valid,
|
||||
double cn0_dbhz,
|
||||
bool cn0_valid);
|
||||
double SquareRoot(double number);
|
||||
void cart2geo(double X, double Y, double Z, int i, t_phi_Lambda_h * phi_Lambda_h);
|
||||
int findUtmZone(double latitude, double longitude);
|
||||
|
||||
@@ -22,6 +22,20 @@ typedef struct {
|
||||
uint64_t hot_retry_delay_ticks;
|
||||
uint64_t hot_window_ticks;
|
||||
unsigned int max_hot_streak;
|
||||
/*
|
||||
* Number of consecutive failed hot attempts served at the fast cadence
|
||||
* before the long backoff applies. A satellite tracked seconds ago is
|
||||
* almost certainly still visible, so that evidence must decay with
|
||||
* repeated failures rather than be discarded on the first one.
|
||||
*/
|
||||
unsigned int hot_fast_attempts;
|
||||
/*
|
||||
* Retry cadence for PRNs the almanac predicts are below the elevation
|
||||
* mask. They are slowed down, never excluded: the visibility model is
|
||||
* advisory, and a stale almanac or a stale position must not be able to
|
||||
* make a genuinely visible satellite unreachable.
|
||||
*/
|
||||
uint64_t unlikely_retry_delay_ticks;
|
||||
} gps_acq_manager_config_t;
|
||||
|
||||
typedef struct {
|
||||
@@ -30,6 +44,8 @@ typedef struct {
|
||||
uint64_t hot_until_tick;
|
||||
bool has_attempted;
|
||||
bool attempt_was_hot;
|
||||
/* Consecutive failed attempts since this PRN became hot. */
|
||||
unsigned int hot_attempts;
|
||||
} gps_acq_prn_entry_t;
|
||||
|
||||
typedef struct {
|
||||
@@ -38,6 +54,8 @@ typedef struct {
|
||||
unsigned int cold_cursor;
|
||||
unsigned int hot_cursor;
|
||||
unsigned int hot_streak;
|
||||
/* Bit i set => PRN index i is predicted visible. All ones until known. */
|
||||
uint32_t search_mask;
|
||||
} gps_acq_manager_t;
|
||||
|
||||
void gps_acq_manager_init(
|
||||
@@ -51,6 +69,17 @@ gps_acq_prn_state_t gps_acq_manager_state(
|
||||
unsigned int prn);
|
||||
const char* gps_acq_manager_state_name(gps_acq_prn_state_t state);
|
||||
|
||||
/*
|
||||
* Publish the predicted-visible set. Bit i corresponds to PRN index i (the
|
||||
* same 0-based index used everywhere in this API). Passing 0 restores the
|
||||
* all-searchable default rather than silencing every PRN.
|
||||
*/
|
||||
void gps_acq_manager_set_search_mask(
|
||||
gps_acq_manager_t* manager,
|
||||
uint32_t search_mask);
|
||||
uint32_t gps_acq_manager_search_mask(
|
||||
const gps_acq_manager_t* manager);
|
||||
|
||||
void gps_acq_manager_mark_acquired(
|
||||
gps_acq_manager_t* manager,
|
||||
unsigned int prn);
|
||||
|
||||
@@ -0,0 +1,146 @@
|
||||
#ifndef GPS_ALMANAC_H
|
||||
#define GPS_ALMANAC_H
|
||||
|
||||
/*
|
||||
* GPS LNAV almanac store and satellite-visibility model.
|
||||
*
|
||||
* The receiver already demodulates subframes 4 and 5 on every tracked
|
||||
* channel; before this module the orbital elements in those pages were
|
||||
* decoded far enough to identify the page and then discarded. That left the
|
||||
* acquisition scheduler with no idea which satellites are above the horizon,
|
||||
* so it swept all 32 PRNs blindly. Measured on the board, 75% of acquisition
|
||||
* attempts were spent on satellites that were below the horizon and could
|
||||
* never be found - with a single acquisition engine that directly multiplies
|
||||
* the time to reacquire a real satellite.
|
||||
*
|
||||
* The almanac is deliberately advisory. Visibility narrows *how often* a PRN
|
||||
* is searched, never whether it may be searched at all, so a stale almanac, a
|
||||
* stale position or a decoding error can cost search efficiency but can never
|
||||
* make a visible satellite permanently unreachable.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define GPS_ALMANAC_PRN_COUNT 32u
|
||||
|
||||
/* Reference inclination for the LNAV almanac, IS-GPS-200 20.3.3.5.1.2. */
|
||||
#define GPS_ALMANAC_I0_REF_SEMICIRCLES 0.30
|
||||
|
||||
/*
|
||||
* How far an almanac may be propagated before it stops being evidence.
|
||||
*
|
||||
* This bound exists because the alternative is silent nonsense. A GPS week is
|
||||
* 14.04 orbital periods, not 14, so aliasing the propagation time by one week
|
||||
* moves the satellite 0.04 of an orbit - about 14 degrees of orbital angle,
|
||||
* enough to swap "high in the sky" for "below the horizon". Anything that
|
||||
* cannot be propagated inside this window therefore yields no prediction at
|
||||
* all, which leaves the PRN fully searchable, rather than a confident wrong
|
||||
* one that would demote a satellite that is actually up.
|
||||
*
|
||||
* Fourteen days is chosen to cover a realistic power-off: the persisted
|
||||
* almanac has to survive a fortnight on the shelf and still aim the first
|
||||
* cold-start sweep. Propagated correctly over that span the Keplerian
|
||||
* almanac model is worth well under a degree of elevation, far inside the
|
||||
* mask it feeds.
|
||||
*/
|
||||
#define GPS_ALMANAC_MAX_AGE_SEC 1209600.0
|
||||
|
||||
/*
|
||||
* Largest week number that can be a GPS week. The receiver works on the
|
||||
* 10-bit broadcast scale (0..1023) and full weeks are in the low thousands;
|
||||
* 16383 is far past either and far below anything that would make a
|
||||
* propagation interval meaningless.
|
||||
*/
|
||||
#define GPS_ALMANAC_WEEK_LIMIT 16383u
|
||||
|
||||
typedef struct {
|
||||
bool valid;
|
||||
unsigned char prn; /* 1..32 */
|
||||
unsigned char health; /* 6-bit SV health, 0 = healthy */
|
||||
double ecc;
|
||||
double toa_sec;
|
||||
double i0_rad;
|
||||
double omega_dot_rps;
|
||||
double sqrt_a;
|
||||
double omega0_rad;
|
||||
double omega_rad;
|
||||
double m0_rad;
|
||||
double af0_sec;
|
||||
double af1_sec_per_sec;
|
||||
} gps_almanac_entry_t;
|
||||
|
||||
typedef struct {
|
||||
gps_almanac_entry_t sv[GPS_ALMANAC_PRN_COUNT];
|
||||
/* Reference week/toa from subframe 5 page 25, when it has been seen. */
|
||||
unsigned int week;
|
||||
double toa_sec;
|
||||
bool reference_valid;
|
||||
} gps_almanac_t;
|
||||
|
||||
void gps_almanac_reset(gps_almanac_t* almanac);
|
||||
|
||||
/*
|
||||
* Store one almanac page. subframe[] holds the ten 30-bit LNAV words with
|
||||
* parity in the low six bits, the same representation parse_subframe() uses.
|
||||
* Returns true when an almanac entry was decoded and stored.
|
||||
*/
|
||||
bool gps_almanac_store_page(gps_almanac_t* almanac,
|
||||
const unsigned int* subframe,
|
||||
unsigned char subframe_id);
|
||||
|
||||
/*
|
||||
* Adopt the almanac reference week/toa. False when the pair is not a possible
|
||||
* GPS reference, which is how a corrupt WNa lift or a flash record written by
|
||||
* a build that had one is kept out of the propagation.
|
||||
*/
|
||||
bool gps_almanac_set_reference(gps_almanac_t* almanac,
|
||||
unsigned int week,
|
||||
double toa_sec);
|
||||
|
||||
unsigned int gps_almanac_valid_count(const gps_almanac_t* almanac);
|
||||
|
||||
/*
|
||||
* The week an almanac entry must belong to if it is less than half a week old,
|
||||
* for use before subframe 5 page 25 has supplied the real WNa. This is the
|
||||
* only place that assumption is made; gps_almanac_satellite_ecef() treats the
|
||||
* week it is given as fact.
|
||||
*/
|
||||
unsigned int gps_almanac_assumed_week(const gps_almanac_entry_t* entry,
|
||||
unsigned int week,
|
||||
double tow_sec);
|
||||
|
||||
/*
|
||||
* ECEF position of one almanac satellite at (week, tow_sec). False when the
|
||||
* entry is unusable or when almanac_week/toa place it further than
|
||||
* GPS_ALMANAC_MAX_AGE_SEC from the requested time - an almanac that cannot be
|
||||
* propagated must produce no answer rather than a wrong one.
|
||||
*/
|
||||
bool gps_almanac_satellite_ecef(const gps_almanac_entry_t* entry,
|
||||
unsigned int almanac_week,
|
||||
unsigned int week,
|
||||
double tow_sec,
|
||||
double ecef_out[3]);
|
||||
|
||||
bool gps_almanac_look_angles(const gps_almanac_entry_t* entry,
|
||||
unsigned int almanac_week,
|
||||
unsigned int week,
|
||||
double tow_sec,
|
||||
const double receiver_ecef_m[3],
|
||||
double* elevation_deg_out,
|
||||
double* azimuth_deg_out);
|
||||
|
||||
/*
|
||||
* Bit i of *mask_out is set when PRN (i + 1) is predicted above
|
||||
* mask_elevation_deg and healthy. Returns the number of satellites marked
|
||||
* visible. With no usable almanac or receiver position the mask is left all
|
||||
* ones so acquisition behaves exactly as it did before.
|
||||
*/
|
||||
unsigned int gps_almanac_visible_mask(const gps_almanac_t* almanac,
|
||||
unsigned int week,
|
||||
double tow_sec,
|
||||
const double receiver_ecef_m[3],
|
||||
double mask_elevation_deg,
|
||||
uint32_t* mask_out);
|
||||
|
||||
#endif
|
||||
@@ -10,6 +10,12 @@ typedef struct {
|
||||
gps_ephemeris_t ephemeris;
|
||||
uint64_t committed_tick;
|
||||
bool valid;
|
||||
/*
|
||||
* True while the entry came from flash and has not yet been confirmed by
|
||||
* anything heard on the air. A restored record is a candidate, not an
|
||||
* incumbent: it must never outrank a live broadcast.
|
||||
*/
|
||||
bool restored;
|
||||
} gps_ephemeris_cache_entry_t;
|
||||
|
||||
typedef struct {
|
||||
@@ -34,4 +40,54 @@ void gps_ephemeris_cache_invalidate(
|
||||
gps_ephemeris_cache_t* cache,
|
||||
unsigned int prn);
|
||||
|
||||
/*
|
||||
* Store only when the record carries new information, so repeatedly decoding
|
||||
* the same broadcast data set neither churns the cache nor produces a flash
|
||||
* write. changed_out reports whether the stored content actually differs
|
||||
* from what was already held; it is the flash-write trigger.
|
||||
*/
|
||||
bool gps_ephemeris_cache_store_if_new(
|
||||
gps_ephemeris_cache_t* cache,
|
||||
unsigned int prn,
|
||||
const gps_ephemeris_t* ephemeris,
|
||||
uint64_t now,
|
||||
bool* changed_out);
|
||||
|
||||
/*
|
||||
* Adopt a record read back from flash.
|
||||
*
|
||||
* Kept separate from gps_ephemeris_cache_store_if_new() because a persisted
|
||||
* record has no known age relative to what is currently being broadcast, and
|
||||
* the Toe comparison that orders two broadcast records cannot say so. Toe is
|
||||
* a second-of-week, so gps_ephemeris_supersedes() resolves it with the signed
|
||||
* half-week rule; that is exact between records from the same era and wrong in
|
||||
* the wrong direction once the gap exceeds half a week. A receiver switched
|
||||
* off from Friday to Tuesday would therefore rank its four-day-old flash
|
||||
* record above the live broadcast and keep re-persisting the stale one for
|
||||
* ever. Marking restored records instead lets the first broadcast for that
|
||||
* PRN replace one outright, whatever the Toe arithmetic says.
|
||||
*/
|
||||
bool gps_ephemeris_cache_store_restored(
|
||||
gps_ephemeris_cache_t* cache,
|
||||
unsigned int prn,
|
||||
const gps_ephemeris_t* ephemeris,
|
||||
uint64_t now);
|
||||
|
||||
/*
|
||||
* Restore a cached record and additionally qualify it against the current
|
||||
* GPS time using the IS-GPS-200 curve fit rules. With time_known false only
|
||||
* the structural rules apply, which is the boot case before any satellite has
|
||||
* supplied authoritative time. A record rejected on age is dropped from the
|
||||
* cache; a record that only fails the time check while time is unknown is
|
||||
* kept as a candidate.
|
||||
*/
|
||||
bool gps_ephemeris_cache_restore_at(
|
||||
gps_ephemeris_cache_t* cache,
|
||||
unsigned int prn,
|
||||
uint64_t now,
|
||||
bool time_known,
|
||||
int week,
|
||||
double tow_sec,
|
||||
gps_ephemeris_t* ephemeris_out);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,127 @@
|
||||
#ifndef GPS_EPHEMERIS_STORE_H
|
||||
#define GPS_EPHEMERIS_STORE_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "gps/gps_ip_defines.h"
|
||||
|
||||
/*
|
||||
* Persistent ephemeris cache in SPI NOR flash.
|
||||
*
|
||||
* What is stored is the three raw LNAV subframes, not the parsed structure.
|
||||
* That keeps the on-flash format independent of compiler layout and of any
|
||||
* later change to gps_ephemeris_t, and it means a restored record is decoded by
|
||||
* exactly the same parser the live receiver uses.
|
||||
*
|
||||
* Layout: two erase sectors used as an append-only log pair. Each sector opens
|
||||
* with a header slot carrying a generation number; the sector with the higher
|
||||
* valid generation is active. Records are appended one slot at a time and are
|
||||
* committed by their CRC, so a record torn by power loss simply fails to
|
||||
* validate and is skipped. When the active sector fills, the live set is
|
||||
* rewritten into the spare sector at generation+1 and the old sector is erased,
|
||||
* which is also the only time an erase happens.
|
||||
*
|
||||
* All flash access goes through the injected interface, so the whole module is
|
||||
* exercised on the host against a simulated device.
|
||||
*/
|
||||
|
||||
#define GPS_EPH_STORE_SLOT_SIZE 256u /* one flash page */
|
||||
#define GPS_EPH_STORE_MAGIC 0x45504853u /* "EPHS" */
|
||||
#define GPS_EPH_STORE_HEADER_MAGIC 0x45504844u /* "EPHD" */
|
||||
#define GPS_EPH_STORE_VERSION 1u
|
||||
|
||||
typedef struct {
|
||||
/* Reads never fail on NOR flash; erase/program report success. */
|
||||
void (*read)(void* ctx, uint32_t address, void* dst, uint32_t len);
|
||||
bool (*erase_sector)(void* ctx, uint32_t address);
|
||||
bool (*program)(void* ctx, uint32_t address, const void* src,
|
||||
uint32_t len);
|
||||
void* ctx;
|
||||
uint32_t base_address; /* first sector; must be erase aligned */
|
||||
uint32_t sector_size; /* erase granularity */
|
||||
} gps_ephemeris_store_flash_t;
|
||||
|
||||
typedef struct {
|
||||
gps_ephemeris_store_flash_t flash;
|
||||
bool mounted;
|
||||
/*
|
||||
* Latched when compaction cannot fit the live record set into one sector.
|
||||
* Compaction erases the spare sector before it discovers this, so retrying
|
||||
* would burn two erase cycles per attempt indefinitely. Persistence
|
||||
* disables itself instead: losing the cache is recoverable, wearing the
|
||||
* flash out is not. Unreachable in production (32 PRNs vs 255 slots); it
|
||||
* exists because the slot cap makes it reachable, and because a future
|
||||
* larger record would shrink the margin.
|
||||
*/
|
||||
bool capacity_exceeded;
|
||||
unsigned int active_sector; /* 0 or 1 */
|
||||
uint32_t generation;
|
||||
uint32_t next_slot; /* next free slot index within the sector */
|
||||
uint32_t slots_per_sector;
|
||||
/* Highest sequence seen per PRN, so a stale duplicate never wins. */
|
||||
uint32_t prn_sequence[GPS_PRN_COUNT];
|
||||
bool prn_present[GPS_PRN_COUNT];
|
||||
uint32_t sequence;
|
||||
/* Diagnostics. */
|
||||
uint32_t records_scanned;
|
||||
uint32_t records_rejected;
|
||||
uint32_t writes;
|
||||
uint32_t erases;
|
||||
uint32_t compactions;
|
||||
} gps_ephemeris_store_t;
|
||||
|
||||
/* CRC-32 (IEEE 802.3, reflected) over the record body. Exposed for tests. */
|
||||
uint32_t gps_ephemeris_store_crc32(const void* data, uint32_t len);
|
||||
|
||||
/*
|
||||
* Scan both sectors and adopt the newer consistent one. Returns false only
|
||||
* when the configuration is unusable; an empty or wholly corrupt store mounts
|
||||
* successfully and simply holds nothing.
|
||||
*/
|
||||
bool gps_ephemeris_store_mount(
|
||||
gps_ephemeris_store_t* store,
|
||||
const gps_ephemeris_store_flash_t* flash);
|
||||
|
||||
/*
|
||||
* Cap the usable slots per sector below what the sector physically holds.
|
||||
* Compaction is otherwise only reachable after a full sector of records, which
|
||||
* is impractical to reach on hardware; capping makes the same code path - real
|
||||
* erases included - testable in seconds. Must be called after mount, and only
|
||||
* shrinks. Zero means no cap.
|
||||
*/
|
||||
void gps_ephemeris_store_limit_slots(
|
||||
gps_ephemeris_store_t* store,
|
||||
uint32_t max_slots);
|
||||
|
||||
/*
|
||||
* Rebuild every valid persisted ephemeris into the caller's handler. The
|
||||
* subframes are re-parsed with the live decoder, and only records that pass the
|
||||
* structural validity rules are delivered. Records are delivered newest-first
|
||||
* per PRN.
|
||||
*/
|
||||
typedef void (*gps_ephemeris_store_load_fn)(
|
||||
void* ctx, unsigned int prn, const gps_ephemeris_t* ephemeris);
|
||||
|
||||
unsigned int gps_ephemeris_store_load_all(
|
||||
gps_ephemeris_store_t* store,
|
||||
gps_ephemeris_store_load_fn handler,
|
||||
void* ctx);
|
||||
|
||||
/*
|
||||
* Persist one ephemeris, given the three raw subframes that produced it.
|
||||
* Returns false on a flash failure. Writing is skipped, and true returned,
|
||||
* when the record is not newer than what is already persisted - that is the
|
||||
* wear policy, and it is why re-decoding the same broadcast costs nothing.
|
||||
*/
|
||||
bool gps_ephemeris_store_save(
|
||||
gps_ephemeris_store_t* store,
|
||||
unsigned int prn,
|
||||
const gps_ephemeris_t* ephemeris,
|
||||
const unsigned int subframe_words[3][10],
|
||||
bool* wrote_out);
|
||||
|
||||
/* Erase both sectors and start an empty generation. */
|
||||
bool gps_ephemeris_store_format(gps_ephemeris_store_t* store);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,98 @@
|
||||
#ifndef GPS_EPHEMERIS_VALIDITY_H
|
||||
#define GPS_EPHEMERIS_VALIDITY_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "gps/gps_ip_defines.h"
|
||||
|
||||
/*
|
||||
* IS-GPS-200 ephemeris validity rules, kept separate from the decoder so a
|
||||
* cached (RAM or flash) record can be re-qualified against the current
|
||||
* receiver time without re-decoding subframes 1..3.
|
||||
*
|
||||
* Age is always evaluated as a signed half-week difference between the
|
||||
* requested GPS time and Toe. That keeps the check correct across the
|
||||
* Saturday/Sunday second-of-week rollover without needing a week number for
|
||||
* Toe, which LNAV does not broadcast.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Curve fit interval implied by the subframe 2 fit flag and the IODC
|
||||
* (IS-GPS-200 Table 20-XI / 20-XII). The returned value is the full
|
||||
* interval; the usable window is +/- half of it around Toe.
|
||||
*
|
||||
* Project assumption: only the IODC ranges the ICD tabulates explicitly are
|
||||
* encoded. A fit flag of one with an IODC outside those ranges falls back to
|
||||
* GPS_EPHEMERIS_LONG_FIT_FALLBACK_HOURS, which is shorter than the ICD's
|
||||
* smallest tabulated long fit (8 hours) and therefore conservative.
|
||||
*/
|
||||
#define GPS_EPHEMERIS_NOMINAL_FIT_HOURS 4.0
|
||||
#define GPS_EPHEMERIS_LONG_FIT_FALLBACK_HOURS 6.0
|
||||
|
||||
double gps_ephemeris_fit_interval_hours(unsigned int iodc, bool fit_flag);
|
||||
|
||||
/* Half of the curve fit interval, in seconds: the maximum |t - Toe|. */
|
||||
double gps_ephemeris_max_toe_offset_sec(const gps_ephemeris_t* eph);
|
||||
|
||||
/*
|
||||
* Signed seconds from Toe to (week, tow_sec), wrapped into +/- half a week.
|
||||
* Returns false when the inputs are not finite or Toe is out of range.
|
||||
*/
|
||||
bool gps_ephemeris_toe_offset_sec(
|
||||
const gps_ephemeris_t* eph,
|
||||
int week,
|
||||
double tow_sec,
|
||||
double* offset_sec_out);
|
||||
|
||||
typedef enum {
|
||||
GPS_EPHEMERIS_OK = 0,
|
||||
GPS_EPHEMERIS_REJECT_NULL,
|
||||
GPS_EPHEMERIS_REJECT_UNCOMMITTED,
|
||||
GPS_EPHEMERIS_REJECT_PRN,
|
||||
GPS_EPHEMERIS_REJECT_IOD,
|
||||
GPS_EPHEMERIS_REJECT_HEALTH,
|
||||
GPS_EPHEMERIS_REJECT_ACCURACY,
|
||||
GPS_EPHEMERIS_REJECT_PARAMETERS,
|
||||
GPS_EPHEMERIS_REJECT_AGE
|
||||
} gps_ephemeris_status_t;
|
||||
|
||||
const char* gps_ephemeris_status_name(gps_ephemeris_status_t status);
|
||||
|
||||
/*
|
||||
* Structural qualification: committed flag, PRN identity, IODE/IODC
|
||||
* consistency, health, URA index and orbital parameter sanity. This is the
|
||||
* part of the check that does not need receiver time, so it can run at boot
|
||||
* on a flash record before any satellite has been decoded.
|
||||
*/
|
||||
gps_ephemeris_status_t gps_ephemeris_check_static(
|
||||
const gps_ephemeris_t* eph,
|
||||
unsigned int expected_prn);
|
||||
|
||||
/*
|
||||
* Full qualification. When time_known is false only the static rules run and
|
||||
* the record is reported as a candidate; the caller must not navigate on it
|
||||
* until receiver time exists.
|
||||
*/
|
||||
gps_ephemeris_status_t gps_ephemeris_check(
|
||||
const gps_ephemeris_t* eph,
|
||||
unsigned int expected_prn,
|
||||
bool time_known,
|
||||
int week,
|
||||
double tow_sec);
|
||||
|
||||
/*
|
||||
* True when candidate should replace current in the cache. A record only
|
||||
* supersedes another when it carries different issue-of-data or a strictly
|
||||
* newer Toe, so decoding the same ephemeris again never triggers a rewrite.
|
||||
*/
|
||||
bool gps_ephemeris_supersedes(
|
||||
const gps_ephemeris_t* candidate,
|
||||
const gps_ephemeris_t* current);
|
||||
|
||||
/* True when the two records describe the same broadcast data set. */
|
||||
bool gps_ephemeris_same_issue(
|
||||
const gps_ephemeris_t* first,
|
||||
const gps_ephemeris_t* second);
|
||||
|
||||
#endif
|
||||
@@ -10,7 +10,17 @@
|
||||
uint32_t plic_repeat_total;
|
||||
uint32_t nav_epoch_coalesced_total;
|
||||
uint32_t pps_sequence_discontinuity_total;
|
||||
/*
|
||||
* Historic counter, kept for continuity: it conflates two unrelated
|
||||
* events and misled three separate diagnoses. Use the two below.
|
||||
*/
|
||||
uint32_t nav_epoch_incomplete_total;
|
||||
/* The epoch assembly genuinely failed to gather its expected set. */
|
||||
uint32_t nav_epoch_assembly_failed_total;
|
||||
/* The epoch assembled correctly but carried fewer than
|
||||
* GPS_NAV_MIN_WLS_SATELLITES, so no solution was possible. This is a
|
||||
* satellite-availability statement, not an epoch fault. */
|
||||
uint32_t nav_epoch_below_minimum_total;
|
||||
uint32_t nav_epoch_snapshot_overwrite_total;
|
||||
uint32_t how_anchor_rejected_total;
|
||||
uint32_t measurement_missed_total;
|
||||
@@ -19,6 +29,62 @@
|
||||
uint32_t satellite_lost_total;
|
||||
uint32_t ephemeris_cache_store_total;
|
||||
uint32_t ephemeris_cache_restore_total;
|
||||
uint32_t ephemeris_cache_miss_total;
|
||||
uint32_t ephemeris_rejected_total;
|
||||
uint32_t warm_seed_armed_total;
|
||||
uint32_t warm_seed_confirmed_total;
|
||||
uint32_t warm_seed_rejected_total;
|
||||
/* Split of warm_seed_rejected_total by cause: the tracking IP declined
|
||||
* the armed seed (channel lost lock between the check and the arm), or
|
||||
* the first seeded epoch failed the nav-solve grid snap (PRN context no
|
||||
* longer describes this satellite). */
|
||||
uint32_t warm_seed_refused_total;
|
||||
uint32_t warm_seed_snap_failed_total;
|
||||
/* Broadcast issue-of-data differs from the ephemeris currently in use,
|
||||
* i.e. the in-use record (often flash restored) has been superseded. */
|
||||
uint32_t ephemeris_iod_mismatch_total;
|
||||
/*
|
||||
* Why an epoch was discarded, not just that it was. expected_mask is
|
||||
* latched at epoch start; any channel in it that never reports holds the
|
||||
* epoch to its deadline. missing/blocked name those channels.
|
||||
*/
|
||||
uint32_t nav_epoch_last_expected_mask;
|
||||
uint32_t nav_epoch_last_missing_mask;
|
||||
uint32_t nav_epoch_blocked_mask;
|
||||
uint32_t nav_epoch_silenced_total;
|
||||
uint32_t nav_epoch_reseed_total;
|
||||
/*
|
||||
* Why epochs are discarded. deadline vs restart are different faults:
|
||||
* deadline means an expected channel never reported inside the
|
||||
* collection window, restart means a newer epoch's event arrived first.
|
||||
*/
|
||||
uint32_t nav_epoch_deadline_discard_total;
|
||||
uint32_t nav_epoch_restart_discard_total;
|
||||
/*
|
||||
* Capture-counter spread of a COMPLETE epoch, in PPS ticks (125 MHz).
|
||||
* This is the real inter-satellite arrival spread and is what the
|
||||
* 25 ms sync window has to accommodate; transit time alone varies by
|
||||
* ~18 ms between a zenith and a horizon satellite. Only the firmware
|
||||
* can measure it: the epoch exists for ~30 ms in every 200 ms, so an
|
||||
* external debugger cannot sample it.
|
||||
*/
|
||||
uint32_t nav_epoch_spread_last_ticks;
|
||||
uint32_t nav_epoch_spread_max_ticks;
|
||||
/* Per-channel count of "was expected but did not report". */
|
||||
uint32_t nav_epoch_missing_count[GPS_TRCK_IP_COUNT];
|
||||
/* Warm reacquisition latency, CLINT-resolution (2 us on Genesys2).
|
||||
* lock_to_epoch isolates what the seed changed: tracking start to the
|
||||
* first accepted navigation epoch. drop_to_epoch adds the acquisition
|
||||
* search, which dominates and is unrelated to the seed. */
|
||||
uint32_t warm_seed_lock_to_epoch_us;
|
||||
uint32_t warm_seed_drop_to_epoch_us;
|
||||
uint32_t warm_seed_lock_to_epoch_min_us;
|
||||
uint32_t warm_seed_lock_to_epoch_max_us;
|
||||
uint32_t flash_ephemeris_loaded_total;
|
||||
uint32_t flash_ephemeris_write_total;
|
||||
uint32_t flash_ephemeris_error_total;
|
||||
uint32_t last_ephemeris_reject_prn;
|
||||
uint32_t last_ephemeris_reject_reason;
|
||||
uint32_t last_connected_prn;
|
||||
uint32_t last_lost_prn;
|
||||
uint64_t last_connected_tick;
|
||||
@@ -55,6 +121,9 @@
|
||||
void gps_background_task(void);
|
||||
bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot);
|
||||
void gps_get_runtime_diagnostics(gps_runtime_diagnostics_t* diagnostics);
|
||||
/* Records why a decoded or cached ephemeris was refused by the solver.
|
||||
* reason carries a gps_ephemeris_status_t value. */
|
||||
void gps_report_ephemeris_rejection(unsigned int prn, unsigned int reason);
|
||||
void gps_tracking_set_eskf_doppler_alpha(double alpha);
|
||||
double gps_tracking_get_eskf_doppler_alpha(void);
|
||||
void trck_intr_handler(volatile gps_trck_t* const trck_ip, volatile gps_frame_t *gps_frame_parsed, volatile int unsigned gps_subframe[10], unsigned char trck_ip_no);
|
||||
|
||||
@@ -8,6 +8,7 @@
|
||||
#include <bsp/soc.h>
|
||||
#include "gps/matrixLib.h"
|
||||
#include "gps/gps_macros.h"
|
||||
#include "gps/gps_almanac.h"
|
||||
|
||||
#ifdef __riscv
|
||||
#define GPS_IRQ_HANDLER_ATTR __attribute__((interrupt("machine"), aligned(4)))
|
||||
@@ -45,7 +46,12 @@
|
||||
#define GPS_ACQ_IP_COUNT ACQ_ENGINE_COUNT
|
||||
#define GPS_ACQ_IP_BASE_ADDR(index) (BSP_GPS_ACQ_BASE + ((uintptr_t)(index) * BSP_GPS_ACQ_STRIDE))
|
||||
#define GPS_ACQ_INTR_LINE(index) (BSP_GPS_ACQ_IRQ + ((unsigned int)(index) * BSP_GPS_ACQ_IRQ_STRIDE))
|
||||
#define GPS_EVENT_QUEUE_SIZE 64u
|
||||
/* Sized for the worst foreground stall, which is now a flash
|
||||
* compaction sector erase (hundreds of ms). At 9 channels the
|
||||
* 200 ms nav-solve events alone contribute ~23 entries during such
|
||||
* a stall; 64 left no headroom for a concurrent burst and the
|
||||
* startup burst was already overflowing. */
|
||||
#define GPS_EVENT_QUEUE_SIZE 128u
|
||||
/* Genesys2 CLINT is 500 kHz. Keep foreground maintenance at
|
||||
* 10 Hz; the tracking IP's 20 ms NAV timer is independent. */
|
||||
#define GPS_SYSTICK_HZ 10u
|
||||
@@ -61,12 +67,66 @@
|
||||
* is calibrated. The live stationary test produced a 22.7% exclusion
|
||||
* rate and substantially worse position dispersion. */
|
||||
#define GPS_NAV_ENABLE_FDE 0u
|
||||
/*
|
||||
* Measurement weighting. The elevation term is 3 m at zenith
|
||||
* mapped by 1/sin(elevation); the floor bounds it where 1/sin
|
||||
* diverges, because a zero weight is a failed epoch rather than a
|
||||
* discounted satellite. Three degrees leaves the term at 57 m -
|
||||
* some 350 times lighter than zenith - and, unlike the ten degree
|
||||
* floor it replaces, lets the mapping actually work across the
|
||||
* range where a satellite carries most of the error.
|
||||
*/
|
||||
#define GPS_NAV_WEIGHT_MIN_ELEVATION_DEG 3.0
|
||||
/*
|
||||
* Charged on top when a satellite is at or below the horizon, so
|
||||
* the extrapolated atmospheric correction and the reflection it
|
||||
* probably arrived by are paid for in variance rather than
|
||||
* assumed away.
|
||||
*/
|
||||
#define GPS_NAV_BELOW_HORIZON_SIGMA_M 50.0
|
||||
/* Upper bound on the carrier-to-noise term, before calibration. */
|
||||
#define GPS_NAV_CN0_SIGMA_MAX_M 30.0
|
||||
#define GPS_PROFILE_CPU_HZ 50000000ULL
|
||||
#define GPS_ACQ_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ / 2u)
|
||||
#define GPS_ACQ_HOT_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ * 30u)
|
||||
#define GPS_ACQ_REACQ_PRIORITY_TICKS \
|
||||
(GPS_SYSTICK_HZ * 2u * 60u * 60u)
|
||||
#define GPS_ACQ_MAX_HOT_STREAK 4u
|
||||
/*
|
||||
* Fast retries granted to a freshly lost satellite before the long
|
||||
* hot backoff applies. Six attempts at the 0.5 s cold cadence is
|
||||
* ~3 s, comfortably longer than the tracking loop needs to settle
|
||||
* after a signal interruption, and far below the 30 s backoff that
|
||||
* previously applied from the first retry.
|
||||
*/
|
||||
#define GPS_ACQ_HOT_FAST_ATTEMPTS 6u
|
||||
/*
|
||||
* Almanac-directed acquisition. A PRN predicted below the mask
|
||||
* is retried on this slow cadence instead of the 0.5 s sweep, so
|
||||
* the single acquisition engine spends its time on satellites
|
||||
* that are actually up. It is a demotion, not an exclusion: the
|
||||
* PRN is still revisited so a stale almanac or a stale position
|
||||
* self-corrects, and a satellite that has just risen is found
|
||||
* without waiting for the next almanac refresh.
|
||||
*/
|
||||
#define GPS_ACQ_UNLIKELY_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ * 20u)
|
||||
#define GPS_ACQ_VISIBILITY_REFRESH_TICKS (GPS_SYSTICK_HZ * 10u)
|
||||
/*
|
||||
* Search a little below the horizon: almanac elevation carries a
|
||||
* few degrees of error when the data is days old, and a satellite
|
||||
* about to rise is worth finding early.
|
||||
*/
|
||||
#define GPS_ACQ_VISIBILITY_MASK_ELEVATION_DEG (-5.0)
|
||||
/*
|
||||
* The same mask while the receiver position is still the seed
|
||||
* read back from flash rather than a fix taken this run. The
|
||||
* seed says where the receiver was; nothing yet says it has not
|
||||
* been moved since. A displacement of D km moves a look angle by
|
||||
* roughly D/111 degrees, so -25 tolerates a couple of thousand
|
||||
* kilometres of unknown migration while still ruling out the far
|
||||
* side of the earth.
|
||||
*/
|
||||
#define GPS_ACQ_VISIBILITY_SEED_MASK_ELEVATION_DEG (-25.0)
|
||||
#define GPS_ACQ_STUCK_WARN_TICKS (GPS_SYSTICK_HZ * 8u)
|
||||
#define GPS_EPHEMERIS_CACHE_MAX_AGE_TICKS \
|
||||
(GPS_SYSTICK_HZ * 2u * 60u * 60u)
|
||||
@@ -79,6 +139,83 @@
|
||||
#define GPS_HOW_ANCHOR_MAX_AGE_SEC 12.0
|
||||
#define GPS_HOW_CONTINUITY_MAX_GAP_SEC 120.0
|
||||
#define GPS_HOW_CONTINUITY_TOLERANCE_SEC 0.050
|
||||
/* Warm/hot reacquisition phase seeding. The PRN context is only
|
||||
* reused while the receiver clock prediction stays far inside one
|
||||
* 20 ms navigation bit; at a few ppm the 15 minute bound leaves
|
||||
* roughly two orders of magnitude of margin, and broadcast
|
||||
* ephemeris validity caps reuse well before that anyway. */
|
||||
/* Persistent ephemeris cache. The Genesys2 image places the
|
||||
* bitstream at 0 and the application at 0x01800000 on a 32 MiB
|
||||
* S25FL256S, so the last two 64 KiB sectors are free. Keeping the
|
||||
* store at the very top leaves the application region free to grow. */
|
||||
#ifndef GPS_EPHEMERIS_FLASH_ENABLE
|
||||
#define GPS_EPHEMERIS_FLASH_ENABLE 1
|
||||
#endif
|
||||
/* Test hooks. MAX_SLOTS caps the usable slots per sector so
|
||||
* compaction can be exercised on real hardware without writing 256
|
||||
* records; 0 means use the whole sector. FORCE_FORMAT erases both
|
||||
* sectors at boot so the format path can be observed. */
|
||||
#ifndef GPS_EPHEMERIS_STORE_MAX_SLOTS
|
||||
#define GPS_EPHEMERIS_STORE_MAX_SLOTS 0u
|
||||
#endif
|
||||
#ifndef GPS_EPHEMERIS_FLASH_FORCE_FORMAT
|
||||
#define GPS_EPHEMERIS_FLASH_FORCE_FORMAT 0
|
||||
#endif
|
||||
#ifndef GPS_EPHEMERIS_FLASH_BASE
|
||||
#define GPS_EPHEMERIS_FLASH_BASE 0x01FE0000UL
|
||||
#endif
|
||||
/*
|
||||
* Persistent navigation context (almanac, ionosphere model, last
|
||||
* position) in the two 64 KiB sectors immediately below the
|
||||
* ephemeris store. The application image lives at 0x01800000 and
|
||||
* is far smaller than the gap.
|
||||
*/
|
||||
#ifndef GPS_NAV_CONTEXT_FLASH_BASE
|
||||
#define GPS_NAV_CONTEXT_FLASH_BASE 0x01FC0000UL
|
||||
#endif
|
||||
#ifndef GPS_NAV_CONTEXT_ENABLE
|
||||
#define GPS_NAV_CONTEXT_ENABLE 1
|
||||
#endif
|
||||
/*
|
||||
* How often the context is rebuilt and offered to the store. The
|
||||
* store compares a CRC first, so a save that would not change the
|
||||
* bytes never reaches the flash; this period only bounds how
|
||||
* quickly a genuine change is captured.
|
||||
*/
|
||||
#define GPS_NAV_CONTEXT_SAVE_PERIOD_TICKS (GPS_SYSTICK_HZ * 60u)
|
||||
/*
|
||||
* What counts as the receiver having moved, for the purpose of
|
||||
* the persisted position seed, and how often that seed may be
|
||||
* refreshed. The seed only aims a visibility prediction and a
|
||||
* look angle changes by ~0.003 degrees per kilometre of receiver
|
||||
* error, so 5 km is far inside what the mask can notice. The two
|
||||
* limits together are what keep the erase count tied to real news
|
||||
* rather than to the save timer: a stationary receiver offers
|
||||
* byte-identical payloads for ever and never touches the flash.
|
||||
*/
|
||||
#define GPS_NAV_CONTEXT_POSITION_EPSILON_M 5000.0
|
||||
#define GPS_NAV_CONTEXT_POSITION_REFRESH_TICKS \
|
||||
(GPS_SYSTICK_HZ * 60u * 60u)
|
||||
#ifndef GPS_WARM_SEED_ENABLE
|
||||
#define GPS_WARM_SEED_ENABLE 1
|
||||
#endif
|
||||
#define GPS_NAV_WARM_SEED_MAX_AGE_SEC 900.0
|
||||
/* Never arm within this distance of a navigation-bit boundary, so
|
||||
* the seed cannot be applied one bit late. */
|
||||
#define GPS_NAV_WARM_SEED_GUARD_SEC 0.004
|
||||
/* A seeded epoch must land on the known nav-solve grid within this
|
||||
* tolerance before its time anchor is trusted. */
|
||||
#define GPS_NAV_WARM_SEED_SNAP_SEC 0.050
|
||||
/*
|
||||
* A channel is only expected to contribute to an epoch while it is
|
||||
* actually producing nav-solve events. busy+locked+IRQ-enabled is
|
||||
* not the same thing: the RTL timer returns to IDLE whenever
|
||||
* trck_busy drops and will not restart without a subframe or a
|
||||
* seed, so a channel that dips and regains lock looks ready while
|
||||
* emitting nothing. Left in the expected set it holds every epoch
|
||||
* to its deadline until its next subframe, up to six seconds.
|
||||
*/
|
||||
#define GPS_NAV_SILENT_CHANNEL_EPOCHS 2u
|
||||
|
||||
#if (GPS_NAV_SOLVE_TIMER_TICKS == 0u)
|
||||
#error "GPS_NAV_SOLVE_TIMER_TICKS must be non-zero"
|
||||
@@ -143,6 +280,9 @@
|
||||
#define GPS_TRCK_NAV_SOLVE_FLAG_EN_POS 12 // (Default 1)
|
||||
#define GPS_TRCK_MEASUREMENT_INTR_EN_POS 13
|
||||
#define GPS_TRCK_MEASUREMENT_FLAG_EN_POS 14
|
||||
/* Arms NAV_SOLVE_PHASE_SEED. Hardware answers and clears this bit, so it
|
||||
* reads back zero once the timer has evaluated the request. */
|
||||
#define GPS_TRCK_NAV_SOLVE_SEED_ARM_POS 15
|
||||
|
||||
|
||||
// acq ip status reg bits.
|
||||
@@ -158,6 +298,8 @@
|
||||
#define GPS_TRCK_STOP_UNLOCK_POS 5
|
||||
#define GPS_TRCK_NAV_SOLVE_FLAG_POS 6
|
||||
#define GPS_TRCK_MEASUREMENT_FLAG_POS 7
|
||||
/* NAV solve epochs are running on a firmware seed no subframe has confirmed. */
|
||||
#define GPS_TRCK_NAV_SOLVE_PROVISIONAL_POS 8
|
||||
|
||||
#define GPS_TRCK_MEAS_META_VALID_POS 0
|
||||
#define GPS_TRCK_MEAS_META_NAV_BIT_POS 1
|
||||
@@ -261,6 +403,7 @@
|
||||
int32_t MEASUREMENT_SUM_Q; // 'h68
|
||||
uint32_t MEASUREMENT_WBP_LO; // 'h6c
|
||||
uint32_t MEASUREMENT_WBP_HI; // 'h70
|
||||
uint32_t NAV_SOLVE_PHASE_SEED; // 'h74
|
||||
}gps_trck_t;
|
||||
|
||||
typedef enum {
|
||||
@@ -292,6 +435,7 @@
|
||||
_Static_assert(offsetof(gps_trck_t, MEASUREMENT_META) == 0x50, "gps_trck_t measurement meta offset mismatch");
|
||||
_Static_assert(offsetof(gps_trck_t, MEASUREMENT_CARRIER_Q7) == 0x58, "gps_trck_t measurement carrier offset mismatch");
|
||||
_Static_assert(offsetof(gps_trck_t, MEASUREMENT_WBP_HI) == 0x70, "gps_trck_t measurement WBP offset mismatch");
|
||||
_Static_assert(offsetof(gps_trck_t, NAV_SOLVE_PHASE_SEED) == 0x74, "gps_trck_t nav solve phase seed offset mismatch");
|
||||
#endif
|
||||
|
||||
|
||||
@@ -333,11 +477,20 @@
|
||||
double tgd; // from subframe 1
|
||||
gpstime_t tow; // tow value of last reached subframe, calculated from TOW in Handover Word.
|
||||
gpstime_t toc; // from subframe 1
|
||||
gpstime_t toe; // from subframe 2
|
||||
gpstime_t toe; // from subframe 2; .week resolved at commit
|
||||
/*
|
||||
* LNAV broadcasts Toe as a second-of-week only, so toe.week has to be
|
||||
* reconstructed from the subframe 1 week number and the handover word
|
||||
* of the subframe that carried it. Without it the curve-fit age check
|
||||
* can only work modulo one week, and a record exactly N weeks old
|
||||
* reads as brand new. False until that reconstruction has happened.
|
||||
*/
|
||||
bool toe_week_valid;
|
||||
double af0; // from subframe 1
|
||||
double af1; // from subframe 1
|
||||
double af2; // from subframe 1
|
||||
unsigned char iode; // from subframe 2
|
||||
bool fit_flag; // from subframe 2 word 10: curve fit interval > 4 h.
|
||||
double crs; // from subframe 2
|
||||
double deltan; // from subframe 2
|
||||
double m0; // from subframe 2
|
||||
@@ -460,6 +613,10 @@
|
||||
void trck_disable_nav_solve_flag(volatile gps_trck_t* trck_ip); // disables tracking ip nav solve flag generation.
|
||||
void trck_enable_nav_solve_flag(volatile gps_trck_t* trck_ip); // disables tracking ip nav solve flag generation.
|
||||
void trck_enable_nav_solve_intr(volatile gps_trck_t* trck_ip); // enables tracking ip nav solve interrupt generation.
|
||||
void trck_arm_nav_solve_phase_seed(volatile gps_trck_t* trck_ip,
|
||||
uint32_t phase_seed); // warm-start navigation-bit phase for nav solve epochs.
|
||||
bool trck_nav_solve_provisional(volatile gps_trck_t* trck_ip); // epochs run on an unconfirmed firmware seed.
|
||||
bool trck_nav_solve_seed_pending(volatile gps_trck_t* trck_ip); // arm bit still set; hardware has not answered yet.
|
||||
void trck_disable_nav_solve_intr(volatile gps_trck_t* trck_ip); // enables tracking ip nav solve interrupt generation.
|
||||
|
||||
void trck_disable_stop_on_unlock(volatile gps_trck_t* trck_ip); // enable functionality.
|
||||
@@ -505,6 +662,37 @@
|
||||
#endif
|
||||
}
|
||||
bool gps_get_ionoutc(ionoutc_t* ionoutc);
|
||||
void gps_lnav_reset_ionoutc(void);
|
||||
/*
|
||||
* Receiver-global almanac collected from subframe 4/5 pages on every
|
||||
* tracked channel. Used to predict which PRNs are above the horizon
|
||||
* so acquisition does not sweep the whole constellation blindly.
|
||||
*/
|
||||
const gps_almanac_t* gps_lnav_get_almanac(void);
|
||||
void gps_lnav_reset_almanac(void);
|
||||
/*
|
||||
* Raw LNAV words for the pages worth persisting, and the matching
|
||||
* restore entry points. Restoring re-parses through the live decoder,
|
||||
* so a corrupt or stale flash record is rejected exactly as a bad
|
||||
* broadcast would be.
|
||||
*/
|
||||
bool gps_lnav_get_iono_page(uint32_t words_out[10]);
|
||||
uint32_t gps_lnav_get_almanac_pages(
|
||||
uint32_t words_out[GPS_ALMANAC_PRN_COUNT][10]);
|
||||
bool gps_lnav_restore_iono_page(const unsigned int* subframe);
|
||||
bool gps_lnav_restore_almanac_page(const unsigned int* subframe,
|
||||
unsigned char subframe_id);
|
||||
void gps_lnav_restore_almanac_reference(unsigned int week,
|
||||
double toa_sec);
|
||||
/*
|
||||
* Last converged WLS receiver position, for consumers that need a
|
||||
* coarse position rather than a fix - the almanac visibility model in
|
||||
* particular. False until the first solution.
|
||||
*/
|
||||
bool gps_nav_last_receiver_ecef(double ecef_out[3]);
|
||||
bool gps_lnav_broadcast_iode(const unsigned int* subframe,
|
||||
unsigned char subframe_id,
|
||||
unsigned char* iode_out);
|
||||
void gps_background_task(void);
|
||||
void gps_process_isr_events(void);
|
||||
void gps_acq_scheduler_run(void);
|
||||
|
||||
@@ -0,0 +1,127 @@
|
||||
#ifndef GPS_NAV_CONTEXT_STORE_H
|
||||
#define GPS_NAV_CONTEXT_STORE_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "gps/gps_almanac.h"
|
||||
#include "gps/gps_ephemeris_store.h"
|
||||
|
||||
/*
|
||||
* Persistent navigation context: almanac, ionosphere/UTC model, and the last
|
||||
* known receiver position.
|
||||
*
|
||||
* These three share one store because they share one lifetime and one purpose.
|
||||
* Unlike ephemeris, none of them is per-channel and none expires in hours:
|
||||
*
|
||||
* - The almanac stays usable for weeks and tells the acquisition scheduler
|
||||
* which satellites are above the horizon. Collected from the air it takes
|
||||
* 12.5 minutes, because the 25 pages come round one per 30 s frame, so
|
||||
* without persistence every reset sweeps the whole constellation blind for
|
||||
* that entire window.
|
||||
* - The ionosphere/UTC page (subframe 4 page 18) arrives on the same 12.5
|
||||
* minute cycle, and without it every pseudorange carries an uncorrected
|
||||
* ionospheric delay.
|
||||
* - The last position is what turns an almanac into a visibility prediction.
|
||||
*
|
||||
* Shape follows use. This is a small blob rewritten rarely and in full, so it
|
||||
* is stored as an A/B pair of whole sectors rather than through the ephemeris
|
||||
* store's append log: the log is keyed per PRN and compacts, which buys
|
||||
* nothing here and costs the complexity that produced a destructive bug once
|
||||
* already. A save that would not change the bytes does not touch the flash at
|
||||
* all, so the erase count follows how often the constellation actually
|
||||
* publishes new data - roughly daily - not how often the receiver boots.
|
||||
*
|
||||
* Like the ephemeris store, what is persisted is the raw LNAV words rather
|
||||
* than parsed structures, so the on-flash format is independent of compiler
|
||||
* layout and a restored page is decoded by exactly the live decoder.
|
||||
*/
|
||||
|
||||
#define GPS_NAV_CONTEXT_MAGIC 0x4E435458u /* "NCTX" */
|
||||
#define GPS_NAV_CONTEXT_VERSION 1u
|
||||
#define GPS_NAV_CONTEXT_WORDS 10u
|
||||
|
||||
typedef struct {
|
||||
/* Raw LNAV almanac page per PRN; entry absent when its mask bit is 0. */
|
||||
uint32_t almanac_words[GPS_ALMANAC_PRN_COUNT][GPS_NAV_CONTEXT_WORDS];
|
||||
uint32_t almanac_present_mask;
|
||||
/* Subframe 5 page 25 reference, when it has been seen. */
|
||||
uint32_t almanac_week;
|
||||
uint32_t almanac_toa_sec;
|
||||
/* Raw subframe 4 page 18 (ionosphere/UTC). */
|
||||
uint32_t iono_words[GPS_NAV_CONTEXT_WORDS];
|
||||
/* Last converged WLS position and the receiver time it was taken at. */
|
||||
double position_ecef_m[3];
|
||||
double position_tow_sec;
|
||||
uint32_t position_week;
|
||||
uint8_t almanac_reference_valid;
|
||||
uint8_t iono_valid;
|
||||
uint8_t position_valid;
|
||||
uint8_t reserved;
|
||||
} gps_nav_context_payload_t;
|
||||
|
||||
typedef struct {
|
||||
gps_ephemeris_store_flash_t flash;
|
||||
bool mounted;
|
||||
bool has_payload;
|
||||
unsigned int active_sector; /* 0 or 1; the one holding the live copy */
|
||||
uint32_t generation;
|
||||
uint32_t payload_crc32; /* of the active payload, for the wear policy */
|
||||
/* Diagnostics. */
|
||||
uint32_t writes;
|
||||
uint32_t erases;
|
||||
uint32_t skipped_writes;
|
||||
uint32_t rejected;
|
||||
} gps_nav_context_store_t;
|
||||
|
||||
/*
|
||||
* Adopt the newer of the two sectors that carries a self-consistent payload.
|
||||
* An empty or wholly corrupt store still mounts; it simply holds nothing.
|
||||
*/
|
||||
bool gps_nav_context_store_mount(
|
||||
gps_nav_context_store_t* store,
|
||||
const gps_ephemeris_store_flash_t* flash);
|
||||
|
||||
/* Read the persisted payload. False when nothing valid is stored. */
|
||||
bool gps_nav_context_store_load(
|
||||
const gps_nav_context_store_t* store,
|
||||
gps_nav_context_payload_t* payload_out);
|
||||
|
||||
/*
|
||||
* Persist the payload, writing only when the bytes would actually change.
|
||||
* *wrote_out reports whether flash was touched.
|
||||
*
|
||||
* The comparison is a CRC over the whole struct, padding included, so the
|
||||
* caller must zero the payload before filling it. A caller that does not
|
||||
* would feed indeterminate padding into the CRC, every save would look like a
|
||||
* change, and the wear policy would silently stop working.
|
||||
*/
|
||||
bool gps_nav_context_store_save(
|
||||
gps_nav_context_store_t* store,
|
||||
const gps_nav_context_payload_t* payload,
|
||||
bool* wrote_out);
|
||||
|
||||
bool gps_nav_context_store_format(gps_nav_context_store_t* store);
|
||||
|
||||
/*
|
||||
* Is a new position worth rewriting the context for?
|
||||
*
|
||||
* The wear policy above only works if the bytes offered are actually stable
|
||||
* while nothing meaningful has changed, and a live WLS position is not: it
|
||||
* moves by metres every epoch, so feeding it in unfiltered makes every
|
||||
* periodic save a fresh CRC and therefore an erase. At a 60 s period that is
|
||||
* 1440 erase cycles a day on a two-sector pair - months, not years, of endurance
|
||||
* - to persist a seed that is only ever read once, at the next boot.
|
||||
*
|
||||
* The seed's only job is to turn an almanac into a visibility prediction, and
|
||||
* a look angle at 20200 km altitude moves by about 0.003 degrees per kilometre
|
||||
* of receiver error. Kilometres are therefore free and metres are noise, so
|
||||
* the question this answers is "has the receiver moved far enough that the
|
||||
* stored seed would aim the next cold start differently", not "has the
|
||||
* position changed".
|
||||
*/
|
||||
bool gps_nav_context_position_changed(const double persisted_ecef_m[3],
|
||||
const double candidate_ecef_m[3],
|
||||
double epsilon_m);
|
||||
|
||||
#endif
|
||||
@@ -76,6 +76,27 @@ bool gps_nav_epoch_validate_complete(
|
||||
double tow_tolerance_sec,
|
||||
gps_nav_epoch_selection_t* selection);
|
||||
|
||||
/*
|
||||
* Lift one channel's corrected sample onto the same continuous axis as the
|
||||
* epoch's reference sample.
|
||||
*
|
||||
* The RF sample counter is 32 bits at 16.368 MHz, so it wraps every 262
|
||||
* seconds - roughly every four and a half minutes of operation, in the middle
|
||||
* of normal tracking. Every accepted member of an epoch lies inside the
|
||||
* coherence window, so its signed modulo-2^32 distance from the reference is
|
||||
* unambiguous; taking that distance and re-adding it to the reference produces
|
||||
* a set that is internally consistent even when some channels have wrapped and
|
||||
* others have not. Pseudoranges are formed as differences against the
|
||||
* reference, so the absolute value never has to be continuous - only the set
|
||||
* does.
|
||||
*
|
||||
* Exposed rather than kept file-static because this is the one piece of
|
||||
* arithmetic standing between a counter wrap and every pseudorange in the fix.
|
||||
*/
|
||||
double gps_nav_epoch_unwrap_sample(uint32_t raw_sample,
|
||||
double corrected_sample,
|
||||
uint32_t reference_sample);
|
||||
|
||||
/*
|
||||
* Validate a newest-only hardware mailbox after one or more capture events
|
||||
* were coalesced. sequence_gap receives the number of real elapsed epochs;
|
||||
|
||||
@@ -26,6 +26,8 @@ void gps_pps_set_holdover(bool holdover);
|
||||
void gps_pps_reset_time_filter(void);
|
||||
bool gps_pps_read_diagnostics(gps_pps_diagnostics_t* diagnostics);
|
||||
bool gps_pps_read_capture(unsigned int channel, uint64_t* counter, uint32_t* sequence);
|
||||
/* Coherent 64-bit read of the free-running PPS-domain counter. */
|
||||
uint64_t gps_pps_read_raw_counter(void);
|
||||
gps_pps_time_filter_result_t gps_pps_filter_clock_bias(
|
||||
uint64_t epoch_counter,
|
||||
double measured_clock_bias_m,
|
||||
|
||||
@@ -70,6 +70,48 @@ bool gps_time_correct_capture_counter(
|
||||
uint64_t* corrected_counter_out,
|
||||
double* applied_correction_sec_out);
|
||||
|
||||
/*
|
||||
* Navigation-bit phase for the tracking IP's NAV_SOLVE_PHASE_SEED register.
|
||||
*
|
||||
* The seed is the counter value in effect for the next navigation bit after
|
||||
* the timer consumes it, and the hardware raises an epoch on the bit whose
|
||||
* counter value is nav_solve_ticks - 1. So the seed is the absolute
|
||||
* navigation-bit index of that next bit, modulo nav_solve_ticks, which
|
||||
* reproduces exactly the grid the broadcast HOW Z-count would have produced.
|
||||
*
|
||||
* elapsed_sec is receiver time since the anchor, measured with the free-running
|
||||
* PPS counter. That counter is 64-bit and never wraps in practice, unlike the
|
||||
* 32-bit RF sample counter which repeats every ~262 s, so it is the only
|
||||
* timebase that spans a multi-minute outage unambiguously.
|
||||
*
|
||||
* The request is refused when the predicted position inside the current
|
||||
* navigation bit is within guard_sec of either boundary, so the seed can never
|
||||
* be applied one bit late. A refused request simply means "retry later".
|
||||
*/
|
||||
bool gps_nav_bit_phase_from_elapsed(
|
||||
double anchor_tow_sec,
|
||||
double elapsed_sec,
|
||||
double bit_period_sec,
|
||||
uint32_t nav_solve_ticks,
|
||||
double guard_sec,
|
||||
uint32_t* phase_seed_out);
|
||||
|
||||
/*
|
||||
* Transmit time of a seeded channel's first navigation-solve epoch.
|
||||
*
|
||||
* The hardware can only raise that epoch on a multiple of the nav-solve period,
|
||||
* so snapping the prediction to that grid recovers the exact time and, at the
|
||||
* same time, proves the anchor is still valid: a prediction further than
|
||||
* max_snap_sec from the grid means the context no longer describes this
|
||||
* satellite. The result may exceed one week; the caller normalises it.
|
||||
*/
|
||||
bool gps_nav_epoch_snap_from_elapsed(
|
||||
double anchor_tow_sec,
|
||||
double elapsed_sec,
|
||||
double period_sec,
|
||||
double max_snap_sec,
|
||||
double* snapped_tow_sec_out);
|
||||
|
||||
/* Validate that two decoded HOW/sample anchors describe the same monotonic
|
||||
* receiver timeline. This keeps a single transient HOW from replacing a
|
||||
* healthy anchor while still permitting normal week rollover. */
|
||||
|
||||
Reference in New Issue
Block a user