feat(gps): add timer-based assisted reacquisition
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.
This commit is contained in:
onur
@@ -84,7 +84,8 @@
|
||||
const unsigned char* ura_indices,
|
||||
const double* cn0_dbhz,
|
||||
const ionoutc_t* ionoutc, double gps_tow,
|
||||
gps_nav_solution_t* solution_out);
|
||||
gps_nav_solution_t* solution_out);
|
||||
void gps_wls_seed_position(const double receiver_ecef_m[3]);
|
||||
void rotateSatelliteSTD(double const traveltime, const mat_type* X_sat, mat_type* X_sat_rot);
|
||||
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);
|
||||
|
||||
@@ -21,6 +21,8 @@ typedef struct {
|
||||
uint64_t retry_delay_ticks;
|
||||
uint64_t hot_retry_delay_ticks;
|
||||
uint64_t hot_window_ticks;
|
||||
uint64_t unknown_probe_interval_ticks;
|
||||
uint64_t visibility_probe_interval_ticks;
|
||||
unsigned int max_hot_streak;
|
||||
} gps_acq_manager_config_t;
|
||||
|
||||
@@ -30,6 +32,9 @@ typedef struct {
|
||||
uint64_t hot_until_tick;
|
||||
bool has_attempted;
|
||||
bool attempt_was_hot;
|
||||
bool visibility_known;
|
||||
bool predicted_visible;
|
||||
int visibility_priority;
|
||||
} gps_acq_prn_entry_t;
|
||||
|
||||
typedef struct {
|
||||
@@ -38,6 +43,9 @@ typedef struct {
|
||||
unsigned int cold_cursor;
|
||||
unsigned int hot_cursor;
|
||||
unsigned int hot_streak;
|
||||
uint64_t next_unknown_probe_tick;
|
||||
uint64_t next_visibility_probe_tick;
|
||||
bool visibility_active;
|
||||
} gps_acq_manager_t;
|
||||
|
||||
void gps_acq_manager_init(
|
||||
@@ -69,6 +77,17 @@ void gps_acq_manager_mark_failed(
|
||||
unsigned int prn,
|
||||
uint64_t now);
|
||||
|
||||
void gps_acq_manager_set_visibility_mode(
|
||||
gps_acq_manager_t* manager,
|
||||
bool active,
|
||||
uint64_t now);
|
||||
bool gps_acq_manager_set_visibility(
|
||||
gps_acq_manager_t* manager,
|
||||
unsigned int prn,
|
||||
bool known,
|
||||
bool predicted_visible,
|
||||
int priority);
|
||||
|
||||
bool gps_acq_manager_start_next(
|
||||
gps_acq_manager_t* manager,
|
||||
uint64_t now,
|
||||
|
||||
@@ -0,0 +1,95 @@
|
||||
#ifndef GPS_ALMANAC_H
|
||||
#define GPS_ALMANAC_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "gps/gps_ip_defines.h"
|
||||
|
||||
#define GPS_ALMANAC_PRN_COUNT 32u
|
||||
#define GPS_ALMANAC_UNAVAILABLE_HEALTH 0x3fu
|
||||
|
||||
typedef struct {
|
||||
double eccentricity;
|
||||
double toa_sec;
|
||||
double delta_i_rad;
|
||||
double omega_dot_rad_per_sec;
|
||||
double sqrt_a;
|
||||
double omega0_rad;
|
||||
double argument_of_perigee_rad;
|
||||
double mean_anomaly_rad;
|
||||
double af0_sec;
|
||||
double af1_sec_per_sec;
|
||||
uint64_t received_tick;
|
||||
unsigned char health8;
|
||||
bool valid;
|
||||
} gps_almanac_entry_t;
|
||||
|
||||
typedef struct {
|
||||
gps_almanac_entry_t entry[GPS_ALMANAC_PRN_COUNT];
|
||||
unsigned char health6[GPS_ALMANAC_PRN_COUNT];
|
||||
uint64_t entry_mask;
|
||||
uint64_t health6_mask;
|
||||
uint64_t reference_tick;
|
||||
uint64_t sf4_health_tick;
|
||||
uint64_t max_cache_age_ticks;
|
||||
double reference_toa_sec;
|
||||
unsigned int reference_week_mod256;
|
||||
bool reference_valid;
|
||||
bool sf4_health_valid;
|
||||
} gps_almanac_cache_t;
|
||||
|
||||
typedef struct {
|
||||
unsigned int matching_orbit_count;
|
||||
unsigned int unavailable_count;
|
||||
unsigned int missing_count;
|
||||
bool reference_valid;
|
||||
bool sf4_health_valid;
|
||||
bool sf5_health_valid;
|
||||
bool time_valid;
|
||||
bool complete;
|
||||
} gps_almanac_progress_t;
|
||||
|
||||
void gps_almanac_cache_init(
|
||||
gps_almanac_cache_t* cache,
|
||||
uint64_t max_cache_age_ticks);
|
||||
|
||||
/* Ingests a parity-qualified LNAV subframe already normalized by the tracking
|
||||
* RTL. Returns true only when an almanac orbit or page-25 reference/health
|
||||
* record changed. Identical repeats refresh RAM reception age but return
|
||||
* false so they do not trigger a flash write. */
|
||||
bool gps_almanac_cache_ingest(
|
||||
gps_almanac_cache_t* cache,
|
||||
unsigned char subframe_id,
|
||||
const unsigned int words[10],
|
||||
uint64_t now);
|
||||
|
||||
bool gps_almanac_cache_progress(
|
||||
const gps_almanac_cache_t* cache,
|
||||
uint64_t now,
|
||||
gpstime_t gps_time,
|
||||
gps_almanac_progress_t* progress);
|
||||
|
||||
bool gps_almanac_cache_complete(
|
||||
const gps_almanac_cache_t* cache,
|
||||
uint64_t now,
|
||||
gpstime_t gps_time);
|
||||
|
||||
bool gps_almanac_cache_prn_unavailable(
|
||||
const gps_almanac_cache_t* cache,
|
||||
unsigned int prn,
|
||||
uint64_t now,
|
||||
gpstime_t gps_time);
|
||||
|
||||
/* Converts the reduced-precision orbit into the existing satpos() input
|
||||
* representation. The result is for acquisition visibility only, never for
|
||||
* pseudorange/WLS navigation. */
|
||||
bool gps_almanac_cache_to_ephemeris(
|
||||
const gps_almanac_cache_t* cache,
|
||||
unsigned int prn,
|
||||
uint64_t now,
|
||||
gpstime_t gps_time,
|
||||
gps_ephemeris_t* ephemeris,
|
||||
bool* healthy);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,74 @@
|
||||
#ifndef GPS_ASSISTANCE_DATA_H
|
||||
#define GPS_ASSISTANCE_DATA_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "gps/NavEquations.h"
|
||||
#include "gps/gps_almanac.h"
|
||||
#include "gps/gps_ephemeris_cache.h"
|
||||
|
||||
#define GPS_ASSISTANCE_PAYLOAD_MAGIC UINT32_C(0x47504153)
|
||||
#define GPS_ASSISTANCE_PAYLOAD_VERSION 1u
|
||||
#define GPS_ASSISTANCE_FLAG_IONOUTC UINT32_C(0x00000001)
|
||||
#define GPS_ASSISTANCE_FLAG_POSITION UINT32_C(0x00000002)
|
||||
#define GPS_ASSISTANCE_POSITION_MAX_AGE_SEC (30.0 * 86400.0)
|
||||
|
||||
typedef struct {
|
||||
double ecef_m[3];
|
||||
double latitude_deg;
|
||||
double longitude_deg;
|
||||
double height_m;
|
||||
gpstime_t gps_time;
|
||||
bool valid;
|
||||
} gps_assistance_position_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t magic;
|
||||
uint16_t version;
|
||||
uint16_t payload_size;
|
||||
uint32_t flags;
|
||||
uint32_t ephemeris_mask;
|
||||
gpstime_t saved_time;
|
||||
gps_ephemeris_t ephemeris[GPS_PRN_COUNT];
|
||||
gps_almanac_cache_t almanac;
|
||||
ionoutc_t ionoutc;
|
||||
gps_assistance_position_t position;
|
||||
} gps_assistance_payload_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t ephemeris_mask;
|
||||
bool almanac_restored;
|
||||
bool ionoutc_restored;
|
||||
bool position_restored;
|
||||
ionoutc_t ionoutc;
|
||||
gps_assistance_position_t position;
|
||||
} gps_assistance_restore_result_t;
|
||||
|
||||
bool gps_assistance_time_valid(gpstime_t gps_time);
|
||||
double gps_assistance_time_delta_sec(gpstime_t newer, gpstime_t older);
|
||||
|
||||
void gps_assistance_payload_build(
|
||||
gps_assistance_payload_t* payload,
|
||||
const gps_ephemeris_cache_t* ephemeris_cache,
|
||||
const gps_almanac_cache_t* almanac_cache,
|
||||
const ionoutc_t* ionoutc,
|
||||
const gps_assistance_position_t* position,
|
||||
gpstime_t saved_time);
|
||||
|
||||
bool gps_assistance_payload_valid(const gps_assistance_payload_t* payload);
|
||||
|
||||
bool gps_assistance_ephemeris_issue_matches(
|
||||
const gps_ephemeris_t* ephemeris,
|
||||
unsigned char subframe_id,
|
||||
const unsigned int words[10]);
|
||||
|
||||
bool gps_assistance_restore(
|
||||
const gps_assistance_payload_t* payload,
|
||||
gpstime_t current_time,
|
||||
uint64_t now,
|
||||
gps_ephemeris_cache_t* ephemeris_cache,
|
||||
gps_almanac_cache_t* almanac_cache,
|
||||
gps_assistance_restore_result_t* result);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,48 @@
|
||||
#ifndef GPS_ASSISTANCE_STORE_H
|
||||
#define GPS_ASSISTANCE_STORE_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "gps/gps_assistance_data.h"
|
||||
|
||||
#define GPS_ASSISTANCE_FLASH_REGION0 UINT32_C(0x01f80000)
|
||||
#define GPS_ASSISTANCE_FLASH_REGION1 UINT32_C(0x01fc0000)
|
||||
#define GPS_ASSISTANCE_FLASH_REGION_SIZE UINT32_C(0x00010000)
|
||||
|
||||
void gps_assistance_store_init(
|
||||
gps_ephemeris_cache_t* ephemeris_cache,
|
||||
gps_almanac_cache_t* almanac_cache,
|
||||
uint64_t now);
|
||||
|
||||
/* Supplies a parity-qualified live subframe. Returns true when a restored
|
||||
* ephemeris has passed this PRN's first live time/issue check and may hydrate
|
||||
* its tracking channel. */
|
||||
bool gps_assistance_store_observe_subframe(
|
||||
unsigned int prn,
|
||||
unsigned char subframe_id,
|
||||
const unsigned int words[10],
|
||||
unsigned int how_tow_sec,
|
||||
uint64_t now,
|
||||
gps_ephemeris_cache_t* ephemeris_cache,
|
||||
gps_almanac_cache_t* almanac_cache,
|
||||
gps_ephemeris_t* restored_ephemeris_out,
|
||||
gps_assistance_position_t* restored_position_out,
|
||||
gpstime_t* current_time_out);
|
||||
|
||||
void gps_assistance_store_note_ephemeris_change(
|
||||
unsigned int prn,
|
||||
uint64_t now);
|
||||
void gps_assistance_store_note_almanac_change(uint64_t now);
|
||||
void gps_assistance_store_note_ionoutc_change(uint64_t now);
|
||||
void gps_assistance_store_note_solution(
|
||||
const gps_nav_solution_t* solution,
|
||||
gpstime_t gps_time,
|
||||
uint64_t now);
|
||||
|
||||
void gps_assistance_store_service(
|
||||
gps_ephemeris_cache_t* ephemeris_cache,
|
||||
const gps_almanac_cache_t* almanac_cache,
|
||||
uint64_t now);
|
||||
|
||||
#endif
|
||||
@@ -6,9 +6,20 @@
|
||||
|
||||
#include "gps/gps_ip_defines.h"
|
||||
|
||||
typedef struct {
|
||||
gpstime_t tow;
|
||||
uint32_t raw_sample;
|
||||
double corrected_sample;
|
||||
uint64_t committed_tick;
|
||||
bool week_valid;
|
||||
bool valid;
|
||||
} gps_cached_time_anchor_t;
|
||||
|
||||
typedef struct {
|
||||
gps_ephemeris_t ephemeris;
|
||||
uint64_t committed_tick;
|
||||
uint64_t age_bias_ticks;
|
||||
gps_cached_time_anchor_t time_anchor;
|
||||
bool valid;
|
||||
} gps_ephemeris_cache_entry_t;
|
||||
|
||||
@@ -25,11 +36,31 @@ bool gps_ephemeris_cache_store(
|
||||
unsigned int prn,
|
||||
const gps_ephemeris_t* ephemeris,
|
||||
uint64_t now);
|
||||
bool gps_ephemeris_cache_same_issue(
|
||||
const gps_ephemeris_t* first,
|
||||
const gps_ephemeris_t* second);
|
||||
bool gps_ephemeris_cache_import(
|
||||
gps_ephemeris_cache_t* cache,
|
||||
unsigned int prn,
|
||||
const gps_ephemeris_t* ephemeris,
|
||||
uint64_t now,
|
||||
uint64_t age_ticks);
|
||||
bool gps_ephemeris_cache_restore(
|
||||
gps_ephemeris_cache_t* cache,
|
||||
unsigned int prn,
|
||||
uint64_t now,
|
||||
gps_ephemeris_t* ephemeris_out);
|
||||
bool gps_ephemeris_cache_store_time_anchor(
|
||||
gps_ephemeris_cache_t* cache,
|
||||
unsigned int prn,
|
||||
const gps_cached_time_anchor_t* anchor,
|
||||
uint64_t now);
|
||||
bool gps_ephemeris_cache_restore_time_anchor(
|
||||
gps_ephemeris_cache_t* cache,
|
||||
unsigned int prn,
|
||||
uint64_t now,
|
||||
uint64_t max_age_ticks,
|
||||
gps_cached_time_anchor_t* anchor_out);
|
||||
void gps_ephemeris_cache_invalidate(
|
||||
gps_ephemeris_cache_t* cache,
|
||||
unsigned int prn);
|
||||
|
||||
@@ -0,0 +1,68 @@
|
||||
#ifndef GPS_FLASH_JOURNAL_H
|
||||
#define GPS_FLASH_JOURNAL_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define GPS_FLASH_JOURNAL_PAGE_SIZE 256u
|
||||
#define GPS_FLASH_JOURNAL_REGION_COUNT 2u
|
||||
|
||||
typedef bool (*gps_flash_journal_read_fn)(
|
||||
void* context, uint32_t address, void* destination, uint32_t length);
|
||||
typedef bool (*gps_flash_journal_program_fn)(
|
||||
void* context, uint32_t address, const void* source, uint32_t length);
|
||||
typedef bool (*gps_flash_journal_erase_fn)(
|
||||
void* context, uint32_t region_address);
|
||||
|
||||
typedef struct {
|
||||
void* context;
|
||||
gps_flash_journal_read_fn read;
|
||||
gps_flash_journal_program_fn program;
|
||||
gps_flash_journal_erase_fn erase_region;
|
||||
} gps_flash_journal_io_t;
|
||||
|
||||
typedef struct {
|
||||
gps_flash_journal_io_t io;
|
||||
uint32_t region_address[GPS_FLASH_JOURNAL_REGION_COUNT];
|
||||
uint32_t region_size;
|
||||
uint32_t maximum_payload_size;
|
||||
uint32_t next_offset[GPS_FLASH_JOURNAL_REGION_COUNT];
|
||||
uint64_t latest_sequence;
|
||||
uint32_t latest_payload_crc;
|
||||
uint32_t latest_payload_size;
|
||||
unsigned int active_region;
|
||||
bool latest_valid;
|
||||
bool initialized;
|
||||
} gps_flash_journal_t;
|
||||
|
||||
typedef enum {
|
||||
GPS_FLASH_JOURNAL_WRITE_FAILED = 0,
|
||||
GPS_FLASH_JOURNAL_WRITE_COMMITTED,
|
||||
GPS_FLASH_JOURNAL_WRITE_UNCHANGED
|
||||
} gps_flash_journal_write_result_t;
|
||||
|
||||
uint32_t gps_flash_journal_crc32(
|
||||
const void* data,
|
||||
uint32_t length);
|
||||
|
||||
bool gps_flash_journal_init(
|
||||
gps_flash_journal_t* journal,
|
||||
const gps_flash_journal_io_t* io,
|
||||
uint32_t first_region_address,
|
||||
uint32_t second_region_address,
|
||||
uint32_t region_size,
|
||||
uint32_t maximum_payload_size);
|
||||
|
||||
bool gps_flash_journal_load_latest(
|
||||
gps_flash_journal_t* journal,
|
||||
void* payload,
|
||||
uint32_t payload_capacity,
|
||||
uint32_t* payload_size_out,
|
||||
uint64_t* sequence_out);
|
||||
|
||||
gps_flash_journal_write_result_t gps_flash_journal_write(
|
||||
gps_flash_journal_t* journal,
|
||||
const void* payload,
|
||||
uint32_t payload_size);
|
||||
|
||||
#endif
|
||||
@@ -52,6 +52,11 @@
|
||||
const unsigned char* prns,
|
||||
unsigned int count);
|
||||
void gps_acq_scheduler_run(void);
|
||||
void gps_acq_update_visibility_hint(
|
||||
const double receiver_ecef_m[3],
|
||||
double receiver_latitude_deg,
|
||||
double receiver_longitude_deg,
|
||||
gpstime_t gps_time);
|
||||
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);
|
||||
|
||||
@@ -49,6 +49,10 @@
|
||||
/* 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
|
||||
/* LNAV transmits only week modulo 1024. Products built for the
|
||||
* current constellation era use the 2019 rollover epoch until a
|
||||
* saved full week is available. */
|
||||
#define GPS_LNAV_WEEK_ERA_BASE 2048
|
||||
#define GPS_NAV_SOLVE_TIMER_TICK_MS 20u
|
||||
#ifndef GPS_NAV_SOLVE_TIMER_TICKS
|
||||
#define GPS_NAV_SOLVE_TIMER_TICKS 50u
|
||||
@@ -63,20 +67,40 @@
|
||||
#define GPS_NAV_ENABLE_FDE 0u
|
||||
#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)
|
||||
/* A failed reacquisition scan must not impose a 30 s blind period.
|
||||
* Keep a short per-PRN delay so one absent SV cannot monopolize the
|
||||
* engine, while a restored signal is revisited within two seconds. */
|
||||
#define GPS_ACQ_HOT_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ * 2u)
|
||||
#define GPS_ACQ_REACQ_PRIORITY_TICKS \
|
||||
(GPS_SYSTICK_HZ * 2u * 60u * 60u)
|
||||
#define GPS_ACQ_MAX_HOT_STREAK 4u
|
||||
#define GPS_ACQ_STUCK_WARN_TICKS (GPS_SYSTICK_HZ * 8u)
|
||||
#define GPS_ACQ_VISIBILITY_UPDATE_TICKS (GPS_SYSTICK_HZ * 10u)
|
||||
/* Satellites without an orbit model cannot be classified by location.
|
||||
* Probe one unknown PRN at a controlled rate until tracking capacity is
|
||||
* filled; do not let one failing predicted-visible PRN block discovery. */
|
||||
#define GPS_ACQ_UNKNOWN_PROBE_TICKS (GPS_SYSTICK_HZ * 2u)
|
||||
#define GPS_ACQ_VISIBILITY_PROBE_TICKS (GPS_SYSTICK_HZ * 60u)
|
||||
#define GPS_ACQ_MIN_ELEVATION_DEG 5.0
|
||||
#define GPS_EPHEMERIS_CACHE_MAX_AGE_TICKS \
|
||||
(GPS_SYSTICK_HZ * 2u * 60u * 60u)
|
||||
/* Almanac is a coarse acquisition aid. Reject RAM records that have
|
||||
* not been observed for a week; GPS-time/toa age is checked separately. */
|
||||
#define GPS_ALMANAC_CACHE_MAX_AGE_TICKS \
|
||||
(GPS_SYSTICK_HZ * 7u * 24u * 60u * 60u)
|
||||
#define GPS_ACQ_STATUS_LOG_PERIOD_TICKS (GPS_SYSTICK_HZ * 5u)
|
||||
#define GPS_NAV_STATUS_LOG_PERIOD_TICKS GPS_SYSTICK_HZ
|
||||
#define GPS_TRK_MEASUREMENT_STALE_TICKS (GPS_SYSTICK_HZ * 3u)
|
||||
#define GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS 25u
|
||||
#define GPS_NAV_EPOCH_HOLDOFF_MS 30u
|
||||
#define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001
|
||||
#define GPS_HOW_ANCHOR_MAX_AGE_SEC 12.0
|
||||
/* A signed 32-bit RF-sample delta is unambiguous for 131.2 s.
|
||||
* Keep margin below that limit while covering a 30 s tunnel plus
|
||||
* acquisition time. */
|
||||
#define GPS_HOW_ANCHOR_MAX_AGE_SEC 120.0
|
||||
#define GPS_HOW_REACQ_ANCHOR_MAX_AGE_TICKS \
|
||||
(GPS_SYSTICK_HZ * 110u)
|
||||
#define GPS_HOW_REACQ_LOCAL_TIMER_TOLERANCE_SEC 0.30
|
||||
#define GPS_HOW_CONTINUITY_MAX_GAP_SEC 120.0
|
||||
#define GPS_HOW_CONTINUITY_TOLERANCE_SEC 0.050
|
||||
|
||||
@@ -190,8 +214,6 @@
|
||||
#define SECONDS_IN_MINUTE 60.0
|
||||
|
||||
//#define SAMPLE_RATE 16368000
|
||||
#define STARTOFFSET 68.802
|
||||
|
||||
/*! \brief Maximum number of iterations for least square search */
|
||||
#define NUMBER_OF_ITERATIONS (100)
|
||||
|
||||
@@ -250,7 +272,7 @@
|
||||
uint32_t NAV_BIT_READ_REG9; // 'h3c
|
||||
uint32_t TIMEOUT_REG; // 'h40
|
||||
uint32_t NAV_SOLVE_TIMER; // 'h44
|
||||
uint32_t RESERVED_48; // 'h48
|
||||
uint32_t NAV_SOLVE_PHASE; // 'h48, write seeds timer phase
|
||||
uint32_t REM_CODE_PHASE_REG; // 'h4c
|
||||
uint32_t MEASUREMENT_META; // 'h50
|
||||
uint32_t MEASUREMENT_SEQUENCE; // 'h54
|
||||
@@ -289,6 +311,7 @@
|
||||
_Static_assert(offsetof(gps_trck_t, PRN_NUMBER) == 0x04, "gps_trck_t PRN offset mismatch");
|
||||
_Static_assert(offsetof(gps_trck_t, STATUS_REG) == 0x10, "gps_trck_t STATUS offset mismatch");
|
||||
_Static_assert(offsetof(gps_trck_t, DOPPLER_REG) == 0x14, "gps_trck_t DOPPLER offset mismatch");
|
||||
_Static_assert(offsetof(gps_trck_t, NAV_SOLVE_PHASE) == 0x48, "gps_trck_t NAV phase offset mismatch");
|
||||
_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");
|
||||
@@ -480,6 +503,7 @@
|
||||
void trck_disable_measurement_flag(volatile gps_trck_t* trck_ip);
|
||||
|
||||
void trck_set_nav_solve_timer(unsigned int tick, volatile gps_trck_t* trck_ip); // in case of gps, 1 tick is 20 ms.
|
||||
void trck_seed_nav_solve_phase(unsigned int phase, volatile gps_trck_t* trck_ip);
|
||||
|
||||
void trck_clear_intr_flag(volatile gps_trck_t* trck_ip); // clear tracking generic interrupt flag.
|
||||
|
||||
@@ -505,6 +529,7 @@
|
||||
#endif
|
||||
}
|
||||
bool gps_get_ionoutc(ionoutc_t* ionoutc);
|
||||
void gps_set_ionoutc(const ionoutc_t* ionoutc);
|
||||
void gps_background_task(void);
|
||||
void gps_process_isr_events(void);
|
||||
void gps_acq_scheduler_run(void);
|
||||
|
||||
@@ -0,0 +1,51 @@
|
||||
#ifndef GPS_MEASUREMENT_FRESHNESS_H_INCLUDED
|
||||
#define GPS_MEASUREMENT_FRESHNESS_H_INCLUDED
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/*
|
||||
* Tracking has two fundamentally different event cadences. During cold
|
||||
* acquisition, parity-checked LNAV subframes arrive every six seconds. Once
|
||||
* ephemeris is ready, the receiver arms the short-period NAV-solve stream.
|
||||
* Keeping that phase explicit prevents a periodic-measurement deadline from
|
||||
* being applied to LNAV collection.
|
||||
*/
|
||||
typedef enum {
|
||||
GPS_MEASUREMENT_PHASE_LNAV = 0,
|
||||
GPS_MEASUREMENT_PHASE_NAV_SOLVE_ARMED,
|
||||
GPS_MEASUREMENT_PHASE_NAV_SOLVE_ACTIVE
|
||||
} gps_measurement_phase_t;
|
||||
|
||||
typedef struct {
|
||||
uint64_t last_event_tick;
|
||||
bool valid;
|
||||
gps_measurement_phase_t phase;
|
||||
} gps_measurement_freshness_t;
|
||||
|
||||
void gps_measurement_freshness_init(
|
||||
volatile gps_measurement_freshness_t* freshness);
|
||||
void gps_measurement_freshness_note_lnav(
|
||||
volatile gps_measurement_freshness_t* freshness,
|
||||
bool tracking_locked,
|
||||
uint64_t tick);
|
||||
void gps_measurement_freshness_arm_nav_solve(
|
||||
volatile gps_measurement_freshness_t* freshness,
|
||||
bool tracking_locked,
|
||||
uint64_t tick);
|
||||
void gps_measurement_freshness_note_nav_solve(
|
||||
volatile gps_measurement_freshness_t* freshness,
|
||||
bool tracking_locked,
|
||||
uint64_t tick);
|
||||
void gps_measurement_freshness_invalidate(
|
||||
volatile gps_measurement_freshness_t* freshness);
|
||||
bool gps_measurement_freshness_expired(
|
||||
const volatile gps_measurement_freshness_t* freshness,
|
||||
uint64_t now,
|
||||
uint64_t periodic_deadline_ticks);
|
||||
bool gps_measurement_freshness_usable(
|
||||
const volatile gps_measurement_freshness_t* freshness,
|
||||
uint64_t now,
|
||||
uint64_t periodic_deadline_ticks);
|
||||
|
||||
#endif
|
||||
@@ -39,10 +39,11 @@ bool gps_pps_select_reference(
|
||||
unsigned int* reference_index_out);
|
||||
|
||||
/*
|
||||
* The position WLS observations are formed from capture_i - min(capture).
|
||||
* That subtraction removes the common receiver-clock offset and drift before
|
||||
* WLS. The fitted clock-bias datum is STARTOFFSET*c - reference_range (plus
|
||||
* the reference satellite clock correction), so its derivative is
|
||||
* The position WLS observations are formed from corrected RF-sample and
|
||||
* satellite-transmit-time differences relative to one stable reference.
|
||||
* That subtraction removes the arbitrary RF-counter origin before WLS. The
|
||||
* fitted clock-bias datum is -reference_range (plus the reference satellite
|
||||
* clock correction), so its derivative is
|
||||
* -reference_range_rate + c*reference_satellite_clock_drift.
|
||||
*/
|
||||
bool gps_pps_reference_bias_drift(
|
||||
|
||||
@@ -70,6 +70,55 @@ bool gps_time_correct_capture_counter(
|
||||
uint64_t* corrected_counter_out,
|
||||
double* applied_correction_sec_out);
|
||||
|
||||
/*
|
||||
* Form one clock-datum-free code observation from the free-running receiver
|
||||
* counter and the decoded satellite transmit time. The returned value is
|
||||
*
|
||||
* (receiver_i - receiver_reference) -
|
||||
* (transmit_i - transmit_reference)
|
||||
*
|
||||
* in seconds. Multiplying by c gives a pseudorange with an arbitrary common
|
||||
* offset; the fourth WLS state estimates that offset. No receiver startup
|
||||
* delay or calibrated signal-flight-time constant is required.
|
||||
*/
|
||||
bool gps_time_differential_observation(
|
||||
int reference_week,
|
||||
double reference_tow_sec,
|
||||
uint64_t reference_counter,
|
||||
int event_week,
|
||||
double event_tow_sec,
|
||||
uint64_t event_counter,
|
||||
double counter_frequency_hz,
|
||||
double* observation_sec_out);
|
||||
|
||||
/*
|
||||
* Form the same clock-datum-free observation from the receiver's shared RF
|
||||
* sample counter. The caller supplies samples already unwrapped around the
|
||||
* selected epoch reference. This counter is latched with code phase in the
|
||||
* tracking clock domain and therefore preserves the precision that would be
|
||||
* lost by independently crossing each event into the PPS/APB clock domain.
|
||||
*/
|
||||
bool gps_time_sample_differential_observation(
|
||||
int reference_week,
|
||||
double reference_tow_sec,
|
||||
double reference_sample,
|
||||
int event_week,
|
||||
double event_tow_sec,
|
||||
double event_sample,
|
||||
double samples_per_second,
|
||||
double* observation_sec_out);
|
||||
|
||||
/* Recover receiver GPS TOW from the WLS bias of the differential observation
|
||||
* above. The reference row's receiver and transmit differences are zero, so
|
||||
* receiver time is corrected_transmit_tow - bias/c. The caller may pass a
|
||||
* value just outside the GPS week; gps_pps_commit_solution() normalizes it and
|
||||
* adjusts the week number atomically. */
|
||||
bool gps_time_receiver_tow_from_differential_bias(
|
||||
double corrected_transmit_tow_sec,
|
||||
double wls_clock_bias_m,
|
||||
double propagation_speed_mps,
|
||||
double* receiver_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. */
|
||||
@@ -85,4 +134,15 @@ bool gps_time_anchors_consistent(int first_week,
|
||||
double max_gap_sec,
|
||||
double tolerance_sec);
|
||||
|
||||
/* Cross-check an RF-counter interval against an independent free-running CPU
|
||||
* timer. This detects an RF/MMCM/sample-counter reset while retaining cached
|
||||
* ephemeris across the outage. */
|
||||
bool gps_time_elapsed_consistent_with_local_timer(
|
||||
double rf_elapsed_sec,
|
||||
uint64_t anchor_local_tick,
|
||||
uint64_t event_local_tick,
|
||||
double local_ticks_per_second,
|
||||
double tolerance_sec,
|
||||
double* local_elapsed_sec_out);
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
#ifndef GPS_VISIBILITY_H
|
||||
#define GPS_VISIBILITY_H
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
/* Compute geometric elevation from ECEF positions and the receiver's
|
||||
* geodetic latitude/longitude. This intentionally has no acquisition state
|
||||
* so the sky-mask geometry is host-testable. */
|
||||
bool gps_visibility_elevation_deg(
|
||||
const double receiver_ecef_m[3],
|
||||
double receiver_latitude_deg,
|
||||
double receiver_longitude_deg,
|
||||
const double satellite_ecef_m[3],
|
||||
double* elevation_deg_out);
|
||||
|
||||
#endif
|
||||
@@ -2,6 +2,7 @@
|
||||
#define LINKED_LIST_H_INCLUDED
|
||||
|
||||
#include "gps/gps_ip_defines.h"
|
||||
#include "gps/gps_measurement_freshness.h"
|
||||
|
||||
typedef struct gps_ephem_ll {
|
||||
volatile gps_ephemeris_t* data_p;
|
||||
@@ -14,6 +15,7 @@ typedef struct gps_ephem_ll {
|
||||
volatile uint64_t gps_how_tick;
|
||||
volatile bool gps_how_anchor_valid;
|
||||
volatile bool gps_how_week_valid;
|
||||
volatile bool gps_nav_phase_seeded;
|
||||
volatile gpstime_t gps_pending_how_tow;
|
||||
volatile uint32_t gps_pending_how_abs_sample;
|
||||
volatile double gps_pending_how_corrected_abs_sample;
|
||||
@@ -25,8 +27,7 @@ typedef struct gps_ephem_ll {
|
||||
volatile uint64_t gps_last_nav_solve_local_ticks;
|
||||
volatile uint64_t gps_last_nav_solve_pps_counter;
|
||||
volatile uint32_t gps_last_pps_capture_sequence;
|
||||
volatile uint64_t gps_last_measurement_tick;
|
||||
volatile bool gps_measurement_valid;
|
||||
gps_measurement_freshness_t gps_measurement_freshness;
|
||||
} gps_ephem_ll_t;
|
||||
|
||||
extern volatile gps_ephem_ll_t* tracking_node;
|
||||
|
||||
Reference in New Issue
Block a user