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);