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

feat(gps): expose Doppler and signal quality measurements

Browse Source
This commit is contained in:
onur
2026-07-25 11:30:38 +03:00
parent 64860d1ced
commit b2ca7db633
26 changed files with 2042 additions and 59 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/header/fusion/fusion_measurement.h
+4
View File
@@ -21,8 +21,12 @@ typedef struct {
double vacc_m;
double residual_rms_m;
double chi_square_ratio;
double velocity_ecef_mps[3];
double speed_mps;
double velocity_residual_rms_mps;
unsigned int used_satellites;
bool solution_valid;
bool velocity_valid;
} fusion_gps_measurement_t;
/* Timestamped IMU plus the independently supervised platform state consumed
soc/software/gps_system/header/gps/NavEquations.h
+10 -1
View File
@@ -9,6 +9,10 @@
typedef struct gps_nav_solution {
double ecef_m[3];
double clock_bias_m;
double velocity_ecef_mps[3];
double clock_drift_mps;
double speed_mps;
double velocity_residual_rms_mps;
double latitude_deg;
double longitude_deg;
double height_m;
@@ -25,6 +29,9 @@
double postfit_residual_m[GPS_TRCK_IP_COUNT];
double elevation_deg[GPS_TRCK_IP_COUNT];
double measurement_sigma_m[GPS_TRCK_IP_COUNT];
double cn0_dbhz[GPS_TRCK_IP_COUNT];
double doppler_hz[GPS_TRCK_IP_COUNT];
double pseudorange_rate_mps[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];
@@ -55,10 +62,11 @@
bool chi_square_pass;
bool fde_applied;
bool solution_valid;
bool velocity_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);
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, double* clockDrift, mat_type *satellitePositions, mat_type * velocities);
double check_t(long double tkl);
void subVect(double* y, const double* x1, const double* x2);
double normVect(const double* x);
@@ -66,6 +74,7 @@
bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber,
mat_type * az, mat_type * el,
const unsigned char* ura_indices,
const double* cn0_dbhz,
const ionoutc_t* ionoutc, double gps_tow,
gps_nav_solution_t* solution_out);
void rotateSatelliteSTD(double const traveltime, const mat_type* X_sat, mat_type* X_sat_rot);
soc/software/gps_system/header/gps/gps_interrupt_handlers.h
+4
View File
@@ -2,6 +2,7 @@
#define GPS_INT_HAND
#include "gps/gps_ip_defines.h"
#include "gps/gps_tracking_measurement.h"
typedef struct {
uint32_t event_overflow_total;
@@ -12,6 +13,8 @@
uint32_t nav_epoch_incomplete_total;
uint32_t nav_epoch_snapshot_overwrite_total;
uint32_t how_anchor_rejected_total;
uint32_t measurement_missed_total;
uint32_t measurement_invalid_total;
uint32_t satellite_connected_total;
uint32_t satellite_lost_total;
uint32_t ephemeris_cache_store_total;
@@ -32,6 +35,7 @@
uint64_t pps_counter[GPS_TRCK_IP_COUNT];
uint32_t pps_capture_sequence[GPS_TRCK_IP_COUNT];
uint64_t local_timestamp_ticks[GPS_TRCK_IP_COUNT];
gps_tracking_measurement_t measurement[GPS_TRCK_IP_COUNT];
} gps_nav_epoch_snapshot_t;
typedef void (*gps_external_irq_handler_t)(void);
soc/software/gps_system/header/gps/gps_ip_defines.h
+30
View File
@@ -138,6 +138,8 @@
#define GPS_TRCK_NAV_SOLVE_EN_POS 10
#define GPS_TRCK_NAV_SOLVE_INTR_EN_POS 11
#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
// acq ip status reg bits.
@@ -152,6 +154,18 @@
#define GPS_TRCK_NAV_BITS_RCV_POS 4
#define GPS_TRCK_STOP_UNLOCK_POS 5
#define GPS_TRCK_NAV_SOLVE_FLAG_POS 6
#define GPS_TRCK_MEASUREMENT_FLAG_POS 7
#define GPS_TRCK_MEAS_META_VALID_POS 0
#define GPS_TRCK_MEAS_META_NAV_BIT_POS 1
#define GPS_TRCK_MEAS_META_LOCK_POS 2
#define GPS_TRCK_MEAS_META_COUNT_SHIFT 8
#define GPS_TRCK_MEAS_META_COUNT_MASK 0x1fu
#define GPS_TRCK_MEAS_META_VERSION_SHIFT 24
#define GPS_TRCK_MEAS_META_VERSION 1u
#define GPS_TRCK_CARRIER_Q7_SCALE 128.0
#define GPS_TRCK_IF_CENTER_HZ 4092000.0
#define GPS_L1_CARRIER_HZ 1575420000.0
@@ -232,6 +246,15 @@
uint32_t NAV_SOLVE_TIMER; // 'h44
uint32_t RESERVED_48; // 'h48
uint32_t REM_CODE_PHASE_REG; // 'h4c
uint32_t MEASUREMENT_META; // 'h50
uint32_t MEASUREMENT_SEQUENCE; // 'h54
int32_t MEASUREMENT_CARRIER_Q7; // 'h58
uint32_t MEASUREMENT_ABS_SAMPLE; // 'h5c
uint32_t MEASUREMENT_REM_CODE_PHASE; // 'h60
int32_t MEASUREMENT_SUM_I; // 'h64
int32_t MEASUREMENT_SUM_Q; // 'h68
uint32_t MEASUREMENT_WBP_LO; // 'h6c
uint32_t MEASUREMENT_WBP_HI; // 'h70
}gps_trck_t;
typedef enum {
@@ -260,6 +283,9 @@
_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, 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");
#endif
@@ -442,6 +468,10 @@
void trck_enable_nav_bits_flag(volatile gps_trck_t* trck_ip); // enable nav bits internal flag generation.
void trck_enable_nav_bits_intr(volatile gps_trck_t* trck_ip); // enable nav bits intr generation.
void trck_disable_nav_bits_intr(volatile gps_trck_t* trck_ip); // disable nav bits intr generation.
void trck_enable_measurement_intr(volatile gps_trck_t* trck_ip);
void trck_disable_measurement_intr(volatile gps_trck_t* trck_ip);
void trck_enable_measurement_flag(volatile gps_trck_t* trck_ip);
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.
soc/software/gps_system/header/gps/gps_tracking_measurement.h
+61
View File
@@ -0,0 +1,61 @@
#ifndef GPS_TRACKING_MEASUREMENT_H
#define GPS_TRACKING_MEASUREMENT_H
#include <stdbool.h>
#include <stdint.h>
#include "gps/gps_ip_defines.h"
typedef struct {
uint32_t prn;
uint32_t sequence;
uint32_t absolute_sample;
uint32_t remaining_code_phase;
uint64_t local_timestamp_ticks;
int32_t carrier_freq_q7;
int32_t cn0_sum_i;
int32_t cn0_sum_q;
uint64_t cn0_wbp;
unsigned int cn0_sample_count;
double doppler_hz;
double pseudorange_rate_mps;
double cn0_dbhz;
bool nav_bit;
bool locked;
bool valid;
} gps_tracking_measurement_t;
/*
* Decode a coherent hardware mailbox. The sequence-before/after check protects
* callers that are not running directly in the measurement ISR.
*/
bool gps_tracking_measurement_read(
volatile const gps_trck_t* tracking_ip,
gps_tracking_measurement_t* measurement_out);
bool gps_tracking_cn0_dbhz(
int32_t sum_i,
int32_t sum_q,
uint64_t wideband_power,
unsigned int coherent_samples,
double* cn0_dbhz_out);
double gps_tracking_doppler_hz(int32_t carrier_freq_q7);
double gps_tracking_doppler_to_range_rate_mps(double doppler_hz);
/*
* Four-state WLS: receiver ECEF velocity (m/s) and receiver clock drift
* (m/s). Only valid/locked measurements with finite C/N0 are used.
*/
bool gps_tracking_velocity_wls(
const double receiver_ecef_m[3],
const double satellite_positions_m[][3],
const double satellite_velocities_mps[][3],
const double satellite_clock_drifts_sps[],
const gps_tracking_measurement_t measurements[],
unsigned int count,
double receiver_velocity_ecef_mps_out[3],
double* receiver_clock_drift_mps_out,
double* residual_rms_mps_out);
#endif