fix(gps): synchronize complete raw navigation epochs

This commit is contained in:
2026-07-24 15:00:40 +03:00
parent fe0eb19dc5
commit e57b19b4ee
13 changed files with 344 additions and 116 deletions
@@ -3,6 +3,7 @@
#include "gps.h"
#include "gps/gps_ip_defines.h"
#include "gps/gps_nav_epoch.h"
#include "gps/matrixLib.h"
typedef struct gps_nav_solution {
@@ -23,6 +23,7 @@
typedef struct {
uint32_t channel_mask;
uint32_t prn[GPS_TRCK_IP_COUNT];
gps_ephemeris_t ephemeris[GPS_TRCK_IP_COUNT];
gpstime_t tow[GPS_TRCK_IP_COUNT];
uint32_t raw_sample[GPS_TRCK_IP_COUNT];
double corrected_abs_sample[GPS_TRCK_IP_COUNT];
@@ -18,7 +18,6 @@
// | || __/ | |_| | |___| _| | || |\ | |___ ___) |
// |___|_| |____/|_____|_| |___|_| \_|_____|____/
#define MIN_NEEDED_SATELLITE 4
#define GPS_TRCK_UNLOCK_TIMEOUT_TICKS 500000000u
#define GPS_PRN_COUNT 32u
#define GPS_PRN_INVALID 0xffu
@@ -4,6 +4,15 @@
#include <stdbool.h>
#include <stdint.h>
/*
* Four pseudoranges are the algebraic minimum for receiver x/y/z plus clock
* bias. They leave no residual degree of freedom, so a bad measurement can
* produce an arbitrary position while fitting perfectly. The raw receiver
* still exposes that minimum WLS fix and its covariance; the separate
* residual/integrity flags tell downstream fusion that no redundancy exists.
*/
#define GPS_NAV_MIN_WLS_SATELLITES 4u
/*
* The current receiver exposes nine tracking channels. Keep the epoch
* selector bounded so its exhaustive, allocation-free search has a fixed
@@ -11,6 +20,10 @@
*/
#define GPS_NAV_EPOCH_MAX_CANDIDATES 9u
_Static_assert(GPS_NAV_MIN_WLS_SATELLITES <=
GPS_NAV_EPOCH_MAX_CANDIDATES,
"GPS WLS minimum exceeds epoch capacity");
typedef struct {
int week;
double tow_sec;
@@ -50,6 +63,32 @@ bool gps_nav_epoch_select(
double tow_tolerance_sec,
gps_nav_epoch_selection_t* selection);
/*
* Receiver publication is stricter than diagnostic clustering: every
* candidate in the frozen ready mask must belong to one coherent epoch.
* selection is still populated with the largest cluster on failure so the
* caller can report why the complete epoch was rejected.
*/
bool gps_nav_epoch_validate_complete(
const gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES],
unsigned int candidate_count,
uint32_t sample_window,
double tow_tolerance_sec,
gps_nav_epoch_selection_t* selection);
/*
* Validate a newest-only hardware mailbox after one or more capture events
* were coalesced. sequence_gap receives the number of real elapsed epochs;
* missing epochs are never synthesized.
*/
bool gps_nav_capture_sequence_consistent(
uint64_t previous_counter,
uint32_t previous_sequence,
uint64_t current_counter,
uint32_t current_sequence,
uint64_t capture_period_ticks,
uint32_t* sequence_gap);
void gps_nav_epoch_assembly_reset(gps_nav_epoch_assembly_t* assembly);
gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(