onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add GPS integrity checks and GPS IMU ESKF fusion tools

Browse Source
This commit is contained in:
onur
2026-07-13 11:08:12 +03:00
parent 8260862854
commit 4edb665d52
50 changed files with 21581 additions and 14357 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/header/NavEquations.h
+51 -1
View File
@@ -5,6 +5,56 @@
#include "gps_ip_defines.h"
#include "matrixLib.h"
typedef struct {
double ecef_m[3];
double clock_bias_m;
double latitude_deg;
double longitude_deg;
double height_m;
double covariance_enu_m2[3][3];
double hacc_m;
double vacc_m;
double gdop;
double pdop;
double hdop;
double vdop;
double tdop;
double residual_rms_m;
double max_abs_residual_m;
double postfit_residual_m[GPS_TRCK_IP_COUNT];
double elevation_deg[GPS_TRCK_IP_COUNT];
double measurement_sigma_m[GPS_TRCK_IP_COUNT];
double full_postfit_residual_m[GPS_TRCK_IP_COUNT];
double full_elevation_deg[GPS_TRCK_IP_COUNT];
double full_measurement_sigma_m[GPS_TRCK_IP_COUNT];
double weighted_residual_rms_m;
double weighted_residual_sum;
double chi_square_threshold;
double chi_square_ratio;
double full_chi_square_ratio;
double postfit_position_correction_m;
double postfit_clock_correction_m;
double full_postfit_position_correction_m;
double full_postfit_clock_correction_m;
double quality_score;
unsigned int available_satellite_count;
unsigned int used_satellite_count;
unsigned int degrees_of_freedom;
uint32_t used_row_mask;
uint32_t excluded_row_mask;
uint64_t candidate_cycles;
uint64_t full_solution_cycles;
uint64_t fde_cycles;
uint64_t total_wls_cycles;
unsigned int iteration_count;
unsigned int fde_trial_count;
bool candidate_valid;
bool residual_test_valid;
bool chi_square_pass;
bool fde_applied;
bool solution_valid;
} gps_nav_solution_t;
void satpos(gps_ephemeris_t* eph, double* pos, double* vel, double* clk, double* clockCorr);
void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_ephemeris_t* eph, ionoutc_t* ionoutc, double* xyz, t_temp_pos* tempPos, double* clockCorr, mat_type *satellitePositions, mat_type * velocities);
double check_t(long double tkl);
@@ -15,7 +65,7 @@
mat_type * az, mat_type * el,
const unsigned char* ura_indices,
const ionoutc_t* ionoutc, double gps_tow,
double solution_out[4]); // x/y/z and common clock bias in metres
gps_nav_solution_t* solution_out);
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);
soc/software/gps_system/header/gps_interrupt_handlers.h
+15
View File
@@ -3,6 +3,19 @@
#include "gps_ip_defines.h"
typedef struct {
uint32_t event_overflow_total;
uint32_t plic_drain_limit_total;
uint32_t plic_repeat_total;
uint32_t nav_epoch_coalesced_total;
uint32_t satellite_connected_total;
uint32_t satellite_lost_total;
uint32_t last_connected_prn;
uint32_t last_lost_prn;
uint64_t last_connected_tick;
uint64_t last_lost_tick;
} gps_runtime_diagnostics_t;
bool assign_tracking_from_result(const gps_acq_result_t* result);
void acq_intr_handler(unsigned char engine_id);
void gps_dispatch_irq(unsigned int irq_id);
@@ -10,6 +23,8 @@
void gps_process_isr_events(void);
void gps_acq_scheduler_run(void);
void gps_background_task(void);
bool gps_take_nav_solve_period_ready(void);
void gps_get_runtime_diagnostics(gps_runtime_diagnostics_t* diagnostics);
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);
soc/software/gps_system/header/gps_ip_defines.h
+22 -1
View File
@@ -45,11 +45,26 @@
#define GPS_ACQ_INTR_LINE(index) (BSP_GPS_ACQ_IRQ + ((unsigned int)(index) * BSP_GPS_ACQ_IRQ_STRIDE))
#define GPS_EVENT_QUEUE_SIZE 64u
#define GPS_SYSTICK_HZ 20u
#define GPS_NAV_SOLVE_TIMER_TICK_MS 20u
#define GPS_NAV_SOLVE_TIMER_TICKS 50u
#define GPS_NAV_SOLVE_PERIOD_MS (GPS_NAV_SOLVE_TIMER_TICKS * GPS_NAV_SOLVE_TIMER_TICK_MS)
/* Keep integrity statistics active, but do not let leave-one-out FDE
* replace the full-WLS position until the real measurement-noise model
* is calibrated. The live stationary test produced a 22.7% exclusion
* rate and substantially worse position dispersion. */
#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_STUCK_WARN_TICKS (GPS_SYSTICK_HZ * 2u)
#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_TOW_CLUSTER_TOLERANCE_SEC 0.001
#define GPS_HOW_ANCHOR_MAX_AGE_SEC 12.0
_Static_assert(GPS_NAV_SOLVE_PERIOD_MS == 1000u, "NAV solve period must be 1 second");
_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
"GPS_NAV_ENABLE_FDE must be zero or one");
#if (ACQ_ENGINE_COUNT < 1UL)
#error "ACQ_ENGINE_COUNT must be at least 1"
@@ -430,13 +445,19 @@
unsigned int gps_get_acq_intr_line(unsigned int engine_id);
bool gps_prn_valid(unsigned int prn_no);
uint64_t gps_get_systick(void);
static inline uint64_t gps_read_cycle_counter(void)
{
uint64_t cycles;
asm volatile("rdcycle %0" : "=r"(cycles));
return cycles;
}
bool gps_get_ionoutc(ionoutc_t* ionoutc);
void gps_background_task(void);
void gps_process_isr_events(void);
void gps_acq_scheduler_run(void);
// parse functions. Generic parse function.
unsigned char parse_subframe(int unsigned* subframe, gps_frame_t* gps_frame,
bool update_tow);
unsigned int* how_tow_sec_out);
#ifdef EKF_ENABLE
void calculate_position(Ekf* g_ekf);
#else
soc/software/gps_system/header/gps_macros.h
+2 -2
View File
@@ -7,8 +7,8 @@
* Verbosity is cumulative:
* 1: navigation result (position and DOP)
* 2: tracked channel/PRN summary
* 3: state transitions, warnings and errors
* 4: per-epoch and low-level diagnostic detail
* 3: state transitions, warnings/errors and integrity summary
* 4: performance, NAV satellite and low-level diagnostic detail
*
* The aliases keep call sites descriptive without creating extra verbosity
* levels. GPS_DEBUG_LEVEL should normally be in the range 1..4.
soc/software/gps_system/header/gps_time_anchor.h
+34
View File
@@ -0,0 +1,34 @@
#ifndef GPS_TIME_ANCHOR_H_INCLUDED
#define GPS_TIME_ANCHOR_H_INCLUDED
#include <stdbool.h>
#include <stdint.h>
#define GPS_TIME_WEEK_SECONDS 604800.0
#define GPS_TIME_HALF_WEEK_SECONDS 302400.0
/* Resolve the week of a newly received HOW against the previous HOW anchor.
* The HOW contains seconds-of-week; the signed half-week rule makes the
* Saturday/Sunday rollover explicit without depending on a NAV timer. */
int gps_time_resolve_how_week(bool previous_valid,
int previous_week,
double previous_tow_sec,
int fallback_week,
double how_tow_sec);
/* Convert a receiver-sample timestamp to GPS time from a HOW/sample anchor.
* raw counters are modulo 2^32. max_age_sec must keep the signed difference
* unambiguous (and also rejects a stale HOW anchor). */
bool gps_time_from_sample_anchor(int anchor_week,
double anchor_tow_sec,
uint32_t anchor_raw_sample,
double anchor_corrected_sample,
uint32_t event_raw_sample,
double event_corrected_sample,
double samples_per_second,
double max_age_sec,
int* event_week_out,
double* event_tow_sec_out,
double* elapsed_sec_out);
#endif
soc/software/gps_system/header/linked_list.h
+5 -6
View File
@@ -3,17 +3,16 @@
#include "gps_ip_defines.h"
typedef enum {
GPS_TOW_SOURCE_HOW = 0,
GPS_TOW_SOURCE_NAV_SOLVE
} gps_tow_source_t;
typedef struct gps_ephem_ll {
volatile gps_ephemeris_t* data_p;
unsigned char tracking_ip_no;
struct gps_ephem_ll* next;
volatile unsigned int gps_subframe_mask;
volatile gps_tow_source_t gps_tow_source;
volatile gpstime_t gps_how_tow;
volatile uint32_t gps_how_abs_sample;
volatile double gps_how_corrected_abs_sample;
volatile uint64_t gps_how_tick;
volatile bool gps_how_anchor_valid;
volatile uint32_t gps_nav_solve_sequence;
volatile uint32_t gps_nav_solve_consumed_sequence;
volatile uint32_t gps_last_nav_solve_abs_sample;