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/Makefile
+1
View File
@@ -71,6 +71,7 @@ APP_SRCS := \
src/gps/gps_time_anchor.c \
src/gps/gps_nav_epoch.c \
src/gps/gps_pps.c \
src/gps/gps_tracking_measurement.c \
src/gps/linked_list.c \
src/gps/NavEquations.c \
src/gps/matrixLib.c
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
soc/software/gps_system/src/adapters/gps_fusion_publisher.c
+7
View File
@@ -24,7 +24,14 @@ bool gps_fusion_build_measurement(
measurement_out->vacc_m = solution->vacc_m;
measurement_out->residual_rms_m = solution->residual_rms_m;
measurement_out->chi_square_ratio = solution->chi_square_ratio;
memcpy(measurement_out->velocity_ecef_mps,
solution->velocity_ecef_mps,
sizeof(measurement_out->velocity_ecef_mps));
measurement_out->speed_mps = solution->speed_mps;
measurement_out->velocity_residual_rms_mps =
solution->velocity_residual_rms_mps;
measurement_out->used_satellites = solution->used_satellite_count;
measurement_out->solution_valid = solution->solution_valid;
measurement_out->velocity_valid = solution->velocity_valid;
return true;
}
soc/software/gps_system/src/adapters/gps_system_uart.c
+5 -2
View File
@@ -27,10 +27,13 @@ void gps_system_uart_publish_location(
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
"source=%s lat=%.20lf lon=%.20lf height=%.3lf "
"gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf "
"tracked=%u used=%u\r\n",
"tracked=%u used=%u velocity_valid=%u speed=%.3lf "
"velocity_rms=%.3lf\r\n",
source, latitude_deg, longitude_deg, height_m,
gps->gdop, gps->pdop, gps->hdop, gps->vdop, gps->tdop,
gps->tracked_satellite_count, gps->used_satellite_count);
gps->tracked_satellite_count, gps->used_satellite_count,
gps->velocity_valid ? 1u : 0u,
gps->speed_mps, gps->velocity_residual_rms_mps);
}
void gps_system_uart_publish_fused_location(
soc/software/gps_system/src/gps/NavEquations.c
+103 -28
View File
@@ -17,7 +17,7 @@
Returns:
rho - [out] computed range ( pseudo range)
*/
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 pos[3], vel[3], clk[2];
double los[3];
@@ -28,7 +28,7 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph
//gpstime_t * gpstime = (gpstime_t*) malloc(sizeof(gpstime_t));
//gpstime->sec = eph->toe.sec;
//gpstime->week = eph->toe.week;
//CPP'ye GOREydi, KAPATILDI// double abs_sample[SVN] = { 66564551., 66525117.,66589459.,66666672.,66633917.,66700000. };
//CPP'ye GOREydi, KAPATILDI// int codePhase = 2772; // TODO should be given by tracker. Şu anda matlab'dan alindi..
@@ -38,6 +38,10 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph
for (i = 0; i < 3; i++) {
satellitePositions->matrix[sat_no][i] = pos[i];
velocities->matrix[sat_no][i] = vel[i];
}
if(clockDrift != NULL) {
*clockDrift = clk[1];
}
/*for (i = 0; i < 3; i++) tempPos->sv_po_at_pseudo_obs[i] = pos[i];
//memset(los, '0', sizeof(double));
@@ -168,20 +172,6 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph
double ek;
double ekdot;
double cek, sek;
double pk;
double c2pk, s2pk;
double uk;
double ukdot;
double cuk, suk;
double ok;
double sok, cok;
double ik;
double ikdot;
double sik, cik;
double rk;
double rkdot;
double xpk, ypk;
double xpkdot, ypkdot;
int cnt = 0;
@@ -210,8 +200,12 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph
ekdot = mkdot/( 1 - eph->ecc*cek );
double tak = atan2(sqrt(1.0 - eph->ecc * eph->ecc) * sek, cek - eph->ecc);
double takdot = sek * ekdot * (1.0 + eph->ecc * cos(tak)) / (sin(tak) * (1.0 - eph->ecc* cek));
relativistic = -4.442807633E-10 * eph->ecc * eph->sqrta * sek;
double takdot =
sqrt(1.0 - eph->ecc * eph->ecc) * ekdot /
(1.0 - eph->ecc * cek);
const double relativistic_coefficient = -4.442807633E-10;
relativistic =
relativistic_coefficient * eph->ecc * eph->sqrta * sek;
double phi = tak + eph->omg0;
phi = remainder(phi, 2 * PI);
@@ -219,18 +213,59 @@ void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_eph
double u = phi + eph->cus * sin(2.0 * phi) + eph->cuc * cos(2.0 * phi);
double r = eph->A * (1.0 - eph->ecc * cos(ek)) + eph->crc * cos(2 * phi) + eph->crs * sin(2 * phi);
double i = eph->inc0 + eph->idot * tk + eph->cic * cos(2 * phi) + eph->cis * sin(2 * phi);
double udot = takdot +
2.0 * (eph->cus * cos(2.0 * phi) -
eph->cuc * sin(2.0 * phi)) * takdot;
double rdot = eph->A * eph->ecc * sin(ek) * ekdot +
2.0 * (eph->crs * cos(2.0 * phi) -
eph->crc * sin(2.0 * phi)) * takdot;
double idot = eph->idot +
2.0 * (eph->cis * cos(2.0 * phi) -
eph->cic * sin(2.0 * phi)) * takdot;
double Omega = eph->omgdot + (eph->omgkdot - OMEGA_EARTH) * tk - OMEGA_EARTH * eph->toe.sec;
Omega = remainder(Omega + 2 * PI, 2 * PI);
double Omega_dot = eph->omgkdot - OMEGA_EARTH;
pos[0] = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
pos[1] = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega);
pos[2] = sin(u) * r * sin(i);
double x_orbit = r * cos(u);
double y_orbit = r * sin(u);
double x_orbit_dot = rdot * cos(u) - y_orbit * udot;
double y_orbit_dot = rdot * sin(u) + x_orbit * udot;
pos[0] = x_orbit * cos(Omega) - y_orbit * cos(i) * sin(Omega);
pos[1] = x_orbit * sin(Omega) + y_orbit * cos(i) * cos(Omega);
pos[2] = y_orbit * sin(i);
vel[0] =
x_orbit_dot * cos(Omega) -
x_orbit * sin(Omega) * Omega_dot -
y_orbit_dot * cos(i) * sin(Omega) +
y_orbit * sin(i) * idot * sin(Omega) -
y_orbit * cos(i) * cos(Omega) * Omega_dot;
vel[1] =
x_orbit_dot * sin(Omega) +
x_orbit * cos(Omega) * Omega_dot +
y_orbit_dot * cos(i) * cos(Omega) -
y_orbit * sin(i) * idot * cos(Omega) -
y_orbit * cos(i) * sin(Omega) * Omega_dot;
vel[2] = y_orbit_dot * sin(i) + y_orbit * cos(i) * idot;
// Satellite clock correction
// relativistic correction
//*clockCorr = eph->af0 + tk * (eph->af1 + tk * eph->af2) + relativistic - eph->tgd; //7.23706041149e-05
*clockCorr = (eph->af2 * clock_tk + eph->af1) * clock_tk + eph->af0 - eph->tgd + relativistic;
double clock_correction =
(eph->af2 * clock_tk + eph->af1) * clock_tk +
eph->af0 - eph->tgd + relativistic;
double clock_drift =
(2.0 * eph->af2 * clock_tk) + eph->af1 +
relativistic_coefficient * eph->ecc * eph->sqrta *
cek * ekdot;
if(clockCorr != NULL) {
*clockCorr = clock_correction;
}
if(clk != NULL) {
clk[0] = clock_correction;
clk[1] = clock_drift;
}
/*if (clk[0] != SECONDS_IN_HALF_WEEK) {
*clockCorr = clk[0];//eph->af0 + tk * (eph->af1 + tk * eph->af2) + relativistic - eph->tgd;
}
@@ -268,7 +303,9 @@ static bool least_squares_ura_sigma_m(unsigned char ura_index, double* sigma_m)
static double least_squares_measurement_weight(unsigned char ura_index,
double elevation_deg,
bool elevation_valid)
bool elevation_valid,
double cn0_dbhz,
bool cn0_valid)
{
const double dtr = 0.01745329251994329576923690768489;
const double min_elevation_deg = 10.0;
@@ -296,6 +333,25 @@ static double least_squares_measurement_weight(unsigned char ura_index,
double elevation_sigma_m = elevation_sigma_zenith_m / sin_el;
variance_m2 += elevation_sigma_m * elevation_sigma_m;
}
if(cn0_valid && isfinite(cn0_dbhz))
{
/*
* Receiver-specific code-tracking model. At 45 dB-Hz the additional
* sigma is 3 m; every 20 dB change scales amplitude-domain error by
* ten. Bounds prevent one channel dominating before calibration.
*/
double cn0_sigma_m =
3.0 * pow(10.0, (45.0 - cn0_dbhz) / 20.0);
if(cn0_sigma_m < 0.5)
{
cn0_sigma_m = 0.5;
}
if(cn0_sigma_m > 30.0)
{
cn0_sigma_m = 30.0;
}
variance_m2 += cn0_sigma_m * cn0_sigma_m;
}
return 1.0 / variance_m2;
}
@@ -527,6 +583,7 @@ static bool least_squares_build_postfit(const mat_type* satpos,
const mat_type* obs,
int satellite_count,
const unsigned char* ura_indices,
const double* cn0_dbhz,
const ionoutc_t* ionoutc,
double gps_tow,
const double state[4],
@@ -583,7 +640,10 @@ static bool least_squares_build_postfit(const mat_type* satpos,
az->matrix[0][i] = topocentric.azim;
el->matrix[0][i] = topocentric.elv;
double weight = least_squares_measurement_weight(ura_indices[i], topocentric.elv, true);
double weight = least_squares_measurement_weight(
ura_indices[i], topocentric.elv, true,
(cn0_dbhz != NULL) ? cn0_dbhz[i] : NAN,
(cn0_dbhz != NULL) && isfinite(cn0_dbhz[i]));
if(!isfinite(weight) || (weight <= 0.0))
{
return false;
@@ -629,6 +689,7 @@ static double gps_wls_last_solution[4] = {0.0, 0.0, 0.0, 0.0};
static bool least_squares_candidate(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,
const double seed[4], bool seed_valid,
gps_nav_solution_t* solution_out)
@@ -748,7 +809,10 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
initialize_all(0.0, A);
for(int i = 0 ; i < ActiveSatellitesNumber ; i++)
{
absoluteWeights[i] = least_squares_measurement_weight(ura_indices[i], 0.0, false);
absoluteWeights[i] = least_squares_measurement_weight(
ura_indices[i], 0.0, false,
(cn0_dbhz != NULL) ? cn0_dbhz[i] : NAN,
(cn0_dbhz != NULL) && isfinite(cn0_dbhz[i]));
}
bool seeded_from_last_solution = seed_valid && (seed != NULL);
@@ -843,7 +907,10 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
//TODO: topocent e giren matrix lerin elemanları doğru yükleniyor mu diye bakılacak. !!!!!!!!!!!!!!!!!1
az->matrix[0][i] = topcent->azim;
el->matrix[0][i] = topcent->elv;
absoluteWeights[i] = least_squares_measurement_weight(ura_indices[i], topcent->elv, true);
absoluteWeights[i] = least_squares_measurement_weight(
ura_indices[i], topcent->elv, true,
(cn0_dbhz != NULL) ? cn0_dbhz[i] : NAN,
(cn0_dbhz != NULL) && isfinite(cn0_dbhz[i]));
iono = klobuchar_delay_meters(ionoutc, gps_tow,
phi_lambda_h->phi, phi_lambda_h->lambda,
topcent->azim, topcent->elv);
@@ -989,7 +1056,8 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
double residuals_m[GPS_TRCK_IP_COUNT];
if(!least_squares_build_postfit(satpos, obs, ActiveSatellitesNumber,
ura_indices, ionoutc, gps_tow,
ura_indices, cn0_dbhz,
ionoutc, gps_tow,
posVec->matrix[0], A, az, el,
residuals_m, absoluteWeights))
{
@@ -1234,6 +1302,7 @@ least_squares_fail:
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)
{
@@ -1246,7 +1315,8 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
gps_nav_solution_t full_solution;
if(!least_squares_candidate(satpos, obs, ActiveSatellitesNumber,
az, el, ura_indices, ionoutc, gps_tow,
az, el, ura_indices, cn0_dbhz,
ionoutc, gps_tow,
gps_wls_last_solution,
gps_wls_last_solution_valid,
&full_solution))
@@ -1278,6 +1348,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
mat_type* subset_az = create_matrix(1, subset_count, "fdeAz");
mat_type* subset_el = create_matrix(1, subset_count, "fdeEl");
unsigned char subset_ura[GPS_TRCK_IP_COUNT];
double subset_cn0[GPS_TRCK_IP_COUNT];
if((subset_satpos == NULL) || (subset_obs == NULL) ||
(subset_az == NULL) || (subset_el == NULL))
{
@@ -1307,6 +1378,9 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
}
subset_obs->matrix[0][subset_row] = obs->matrix[0][original_row];
subset_ura[subset_row] = ura_indices[original_row];
subset_cn0[subset_row] =
(cn0_dbhz != NULL) ?
cn0_dbhz[original_row] : NAN;
subset_row++;
}
@@ -1317,6 +1391,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
gps_nav_solution_t candidate;
bool candidate_ok = least_squares_candidate(subset_satpos, subset_obs, subset_count,
subset_az, subset_el, subset_ura,
subset_cn0,
ionoutc, gps_tow,
full_seed, true, &candidate);
free_matrix(subset_satpos);
soc/software/gps_system/src/gps/gps_interrupt_handlers.c
+87
View File
@@ -58,6 +58,8 @@ static volatile bool acq_completion_queued[GPS_ACQ_IP_COUNT];
static volatile bool acq_shadow_pending[GPS_ACQ_IP_COUNT];
static gps_acq_result_t acq_shadow_result[GPS_ACQ_IP_COUNT];
static gps_tracking_shadow_t trk_shadow[GPS_TRCK_IP_COUNT];
static gps_tracking_measurement_t
gps_latest_tracking_measurement[GPS_TRCK_IP_COUNT];
static volatile unsigned char satellite_tracking_ip[GPS_PRN_COUNT];
static gps_acq_result_t satellite_acq_result[GPS_PRN_COUNT];
@@ -113,6 +115,67 @@ static double gps_correct_abs_sample(uint32_t abs_sample, uint32_t rem_code_phas
return (double)abs_sample - ((double)rem_code_phase / CODE_FREQ_BASIS);
}
static void gps_update_tracking_measurement_isr(
volatile const gps_trck_t* tracking_ip,
unsigned char tracking_index,
uint64_t local_timestamp_ticks)
{
gps_tracking_measurement_t measurement;
if(!gps_tracking_measurement_read(tracking_ip, &measurement))
{
gps_runtime_diagnostics.measurement_invalid_total++;
return;
}
gps_tracking_measurement_t* previous =
&gps_latest_tracking_measurement[tracking_index];
if(previous->valid && (previous->prn == measurement.prn))
{
uint16_t sequence_delta =
(uint16_t)(measurement.sequence - previous->sequence);
if(sequence_delta == 0u)
{
/* A repeated IRQ for the same mailbox does not create a sample. */
return;
}
if(sequence_delta > 1u)
{
gps_runtime_diagnostics.measurement_missed_total +=
(uint32_t)sequence_delta - 1u;
}
/* Smooth the noisy one-bit NWPR estimate; keep raw sums for diagnostics. */
measurement.cn0_dbhz =
0.2 * measurement.cn0_dbhz +
0.8 * previous->cn0_dbhz;
}
measurement.local_timestamp_ticks = local_timestamp_ticks;
gps_latest_tracking_measurement[tracking_index] = measurement;
}
static bool gps_copy_latest_tracking_measurement(
unsigned char tracking_index,
uint32_t expected_prn,
gps_tracking_measurement_t* measurement_out)
{
if((tracking_index >= GPS_TRCK_IP_COUNT) ||
(measurement_out == NULL))
{
return false;
}
uint64_t irq_state = bsp_irq_save();
*measurement_out =
gps_latest_tracking_measurement[tracking_index];
bsp_irq_restore(irq_state);
if(!measurement_out->valid ||
(measurement_out->prn != expected_prn))
{
memset(measurement_out, 0, sizeof(*measurement_out));
return false;
}
return true;
}
static uint32_t gps_nav_ready_channel_mask(void)
{
uint32_t ready_mask = 0u;
@@ -318,6 +381,8 @@ static void gps_clear_tracking_slot(unsigned char trck_ip_no)
memset((void*)&gps_frames_parsed[trck_ip_no], 0, sizeof(gps_frames_parsed[trck_ip_no]));
memset((void*)&gps_subframes[trck_ip_no][0], 0, sizeof(gps_subframes[trck_ip_no]));
memset(&gps_latest_tracking_measurement[trck_ip_no], 0,
sizeof(gps_latest_tracking_measurement[trck_ip_no]));
}
static void gps_set_tracking_nav_bits_mode(unsigned char trck_ip_no)
@@ -332,6 +397,8 @@ static void gps_set_tracking_nav_bits_mode(unsigned char trck_ip_no)
trck_enable_nav_solve(trck_ips[trck_ip_no]);
trck_enable_nav_bits_flag(trck_ips[trck_ip_no]);
trck_enable_nav_bits_intr(trck_ips[trck_ip_no]);
trck_enable_measurement_flag(trck_ips[trck_ip_no]);
trck_enable_measurement_intr(trck_ips[trck_ip_no]);
}
static void gps_runtime_state_init(void)
@@ -776,6 +843,22 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
{
return;
}
if((status_reg & (1u << GPS_TRCK_MEASUREMENT_FLAG_POS)) != 0u)
{
gps_update_tracking_measurement_isr(
trck_ip, trck_ip_no, local_timestamp_ticks);
}
const uint32_t queued_source_mask =
(1u << GPS_TRCK_STOP_UNLOCK_POS) |
(1u << GPS_TRCK_NAV_BITS_RCV_POS) |
(1u << GPS_TRCK_NAV_SOLVE_FLAG_POS);
if((status_reg & queued_source_mask) == 0u)
{
/* 50 Hz channel measurements are newest-only mailboxes, not queue events. */
trck_clear_intr_flag(trck_ip);
return;
}
gps_isr_event_t event;
memset(&event, 0, sizeof(event));
@@ -1376,6 +1459,10 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
event->pps_capture_sequence;
gps_nav_active_snapshot.local_timestamp_ticks[trck_ip_no] =
event->local_timestamp_ticks;
(void)gps_copy_latest_tracking_measurement(
trck_ip_no,
event->prn,
&gps_nav_active_snapshot.measurement[trck_ip_no]);
}
if(assembly_result == GPS_NAV_EPOCH_EVENT_COMPLETE)
soc/software/gps_system/src/gps/gps_ip.c
+100 -7
View File
@@ -660,7 +660,8 @@ static unsigned int gps_collect_completed_epoch_locked(
uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT])
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT],
gps_tracking_measurement_t snapshot_measurements[GPS_TRCK_IP_COUNT])
{
if(epoch == NULL)
{
@@ -695,6 +696,8 @@ static unsigned int gps_collect_completed_epoch_locked(
epoch->local_timestamp_ticks[ip_no];
snapshot_raw_samples[count] =
epoch->raw_sample[ip_no];
snapshot_measurements[count] =
epoch->measurement[ip_no];
count++;
}
return count;
@@ -707,6 +710,7 @@ static unsigned int gps_select_coherent_nav_snapshot(
uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT],
gps_tracking_measurement_t snapshot_measurements[GPS_TRCK_IP_COUNT],
unsigned int candidate_count)
{
if((candidate_count == 0u) ||
@@ -756,6 +760,8 @@ static unsigned int gps_select_coherent_nav_snapshot(
snapshot_local_timestamps[source];
snapshot_raw_samples[destination] =
snapshot_raw_samples[source];
snapshot_measurements[destination] =
snapshot_measurements[source];
}
snapshot[destination].abs_sample =
gps_unwrap_corrected_abs_sample(
@@ -794,10 +800,12 @@ static uint64_t gps_nav_median_local_timestamp(
static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
const uint64_t pps_counters[GPS_TRCK_IP_COUNT],
const uint32_t pps_capture_sequences[GPS_TRCK_IP_COUNT],
const gps_tracking_measurement_t* measurements,
unsigned int satellite_count,
gps_nav_solution_t* solution_out)
{
if((ephemeris == NULL) || (solution_out == NULL) ||
if((ephemeris == NULL) || (measurements == NULL) ||
(solution_out == NULL) ||
(satellite_count < GPS_NAV_MIN_WLS_SATELLITES))
{
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "nav solve skipped, usable satellites=%u\r\n", satellite_count);
@@ -809,7 +817,9 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
range_t* rho0 = (range_t*)malloc(satellite_count * sizeof(range_t));
double* psTVLtime = (double*)malloc(satellite_count * sizeof(double));
double* clockCorr = (double*)malloc(satellite_count * sizeof(double));
double* clockDrift = (double*)malloc(satellite_count * sizeof(double));
unsigned char ura_indices[GPS_TRCK_IP_COUNT];
double cn0_dbhz[GPS_TRCK_IP_COUNT];
mat_type* satellitePositions = create_matrix(satellite_count, 3, "satellitePositions");
mat_type* velocities = create_matrix(satellite_count, 3, "velocities");
mat_type* preciseTxTimes = NULL;
@@ -825,7 +835,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "Klobuchar correction %s for this solve\r\n",
(ionoutc != NULL) ? "available" : "waiting for subframe 4 page 18");
if((rho0 == NULL) || (psTVLtime == NULL) || (clockCorr == NULL) ||
if((rho0 == NULL) || (psTVLtime == NULL) ||
(clockCorr == NULL) || (clockDrift == NULL) ||
(satellitePositions == NULL) || (velocities == NULL))
{
GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_MEM, "nav solve allocation failed\r\n");
@@ -835,6 +846,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
for(unsigned int i = 0 ; i < satellite_count ; i++)
{
ura_indices[i] = ephemeris[i].sva;
cn0_dbhz[i] =
(measurements[i].valid &&
(measurements[i].prn == ephemeris[i].SVID))
? measurements[i].cn0_dbhz : NAN;
psTVLtime[i] = DBL_MAX;
}
@@ -847,7 +862,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
"snapshot[%u]: svid=%u abs=%.6f tow=%.3f\r\n",
i, ephemeris[i].SVID, ephemeris[i].abs_sample, ephemeris[i].tow.sec);
computeRange(&(psTVLtime[i]), i, rho0, (gps_ephemeris_t*)&ephemeris[i],
NULL, NULL, NULL, &clockCorr[i], satellitePositions, velocities);
NULL, NULL, NULL, &clockCorr[i], &clockDrift[i],
satellitePositions, velocities);
}
double minimum = psTVLtime[0];
@@ -891,8 +907,57 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
}
ok = leastSquaresSTD(satellitePositions, localPseudoRanges, satellite_count, az, el,
ura_indices,
ura_indices, cn0_dbhz,
ionoutc, ephemeris[0].tow.sec, &solution);
for(unsigned int row = 0u; row < satellite_count; row++)
{
solution.cn0_dbhz[row] = measurements[row].cn0_dbhz;
solution.doppler_hz[row] = measurements[row].doppler_hz;
solution.pseudorange_rate_mps[row] =
measurements[row].pseudorange_rate_mps;
}
if(ok && solution.solution_valid)
{
double satellite_positions_array[GPS_TRCK_IP_COUNT][3];
double satellite_velocities_array[GPS_TRCK_IP_COUNT][3];
gps_tracking_measurement_t
velocity_measurements[GPS_TRCK_IP_COUNT];
for(unsigned int row = 0u; row < satellite_count; row++)
{
for(unsigned int component = 0u; component < 3u; component++)
{
satellite_positions_array[row][component] =
satellitePositions->matrix[row][component];
satellite_velocities_array[row][component] =
velocities->matrix[row][component];
}
velocity_measurements[row] = measurements[row];
if((solution.used_row_mask & (1u << row)) == 0u)
{
velocity_measurements[row].valid = false;
}
}
solution.velocity_valid = gps_tracking_velocity_wls(
solution.ecef_m,
satellite_positions_array,
satellite_velocities_array,
clockDrift,
velocity_measurements,
satellite_count,
solution.velocity_ecef_mps,
&solution.clock_drift_mps,
&solution.velocity_residual_rms_mps);
if(solution.velocity_valid)
{
solution.speed_mps =
sqrt(solution.velocity_ecef_mps[0] *
solution.velocity_ecef_mps[0] +
solution.velocity_ecef_mps[1] *
solution.velocity_ecef_mps[1] +
solution.velocity_ecef_mps[2] *
solution.velocity_ecef_mps[2]);
}
}
if(ok && solution.fde_applied)
{
for(unsigned int row = 0 ; row < satellite_count ; row++)
@@ -944,6 +1009,7 @@ cleanup:
free(rho0);
free(psTVLtime);
free(clockCorr);
free(clockDrift);
*solution_out = solution;
return ok;
}
@@ -1076,6 +1142,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT];
uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT];
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT];
gps_tracking_measurement_t snapshot_measurements[GPS_TRCK_IP_COUNT];
gps_nav_epoch_snapshot_t completed_epoch;
unsigned int satellite_count = 0;
unsigned int candidate_count = 0;
@@ -1119,7 +1186,8 @@ bool calculate_position(gps_nav_solution_t* solution_out,
&completed_epoch,
snapshot, snapshot_ips, snapshot_pps_counters,
snapshot_pps_sequences,
snapshot_local_timestamps, snapshot_raw_samples);
snapshot_local_timestamps, snapshot_raw_samples,
snapshot_measurements);
unsigned int completed_channel_count =
gps_nav_mask_count(completed_epoch.channel_mask);
if(candidate_count != completed_channel_count)
@@ -1144,6 +1212,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
snapshot, snapshot_ips, snapshot_pps_counters,
snapshot_pps_sequences,
snapshot_local_timestamps, snapshot_raw_samples,
snapshot_measurements,
candidate_count);
coherent_nav_count = satellite_count;
run_nav = satellite_count >= GPS_NAV_MIN_WLS_SATELLITES;
@@ -1197,7 +1266,9 @@ bool calculate_position(gps_nav_solution_t* solution_out,
memset(&nav_solution, 0, sizeof(nav_solution));
uint64_t nav_software_start_cycles = gps_read_cycle_counter();
bool nav_ok = nav_software_from_snapshot(snapshot, snapshot_pps_counters,
snapshot_pps_sequences, satellite_count,
snapshot_pps_sequences,
snapshot_measurements,
satellite_count,
&nav_solution);
uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles;
if(nav_ok)
@@ -1371,6 +1442,26 @@ void trck_enable_nav_bits_flag(volatile gps_trck_t* trck_ip)
trck_ip->CONTROL_REG |= (1 << GPS_TRCK_NAV_BITS_FLAG_EN_POS);
}
void trck_enable_measurement_intr(volatile gps_trck_t* trck_ip)
{
trck_ip->CONTROL_REG |= (1u << GPS_TRCK_MEASUREMENT_INTR_EN_POS);
}
void trck_disable_measurement_intr(volatile gps_trck_t* trck_ip)
{
trck_ip->CONTROL_REG &= ~(1u << GPS_TRCK_MEASUREMENT_INTR_EN_POS);
}
void trck_enable_measurement_flag(volatile gps_trck_t* trck_ip)
{
trck_ip->CONTROL_REG |= (1u << GPS_TRCK_MEASUREMENT_FLAG_EN_POS);
}
void trck_disable_measurement_flag(volatile gps_trck_t* trck_ip)
{
trck_ip->CONTROL_REG &= ~(1u << GPS_TRCK_MEASUREMENT_FLAG_EN_POS);
}
void trck_set_nav_solve_timer(unsigned int tick, volatile gps_trck_t* trck_ip)
{
@@ -1421,6 +1512,8 @@ void gps_startup_initialization(unsigned int nsp_tick)
trck_enable_nav_bits_flag(trck_ips[i]);
trck_enable_nav_bits_intr(trck_ips[i]);
trck_enable_measurement_flag(trck_ips[i]);
trck_enable_measurement_intr(trck_ips[i]);
// clear all status flags.
trck_set_nav_solve_timer(nsp_tick, trck_ips[i]);
soc/software/gps_system/src/gps/gps_tracking_measurement.c
+355
View File
@@ -0,0 +1,355 @@
#include "gps/gps_tracking_measurement.h"
#include <math.h>
#include <stddef.h>
#include <string.h>
#define GPS_TRACK_MEASUREMENT_READ_ATTEMPTS 3u
#define GPS_TRACK_MEASUREMENT_MIN_SATELLITES 4u
#define GPS_TRACK_MEASUREMENT_MAX_SATELLITES GPS_TRCK_IP_COUNT
#define GPS_TRACK_MEASUREMENT_EPSILON 1.0e-12
static double clamp_double(double value, double minimum, double maximum)
{
if(value < minimum)
{
return minimum;
}
if(value > maximum)
{
return maximum;
}
return value;
}
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)
{
if((cn0_dbhz_out == NULL) || (wideband_power == 0u) ||
(coherent_samples < 2u))
{
return false;
}
double i = (double)sum_i;
double q = (double)sum_q;
double narrowband_power = i * i + q * q;
double narrow_to_wide_ratio =
narrowband_power / (double)wideband_power;
double sample_count = (double)coherent_samples;
/*
* Narrowband/wideband-power estimator with 1 ms coherent prompt
* integrations: C/N0 = (1/T) * (R - 1) / (M - R).
*/
if(!isfinite(narrow_to_wide_ratio) ||
(narrow_to_wide_ratio <= 1.0) ||
(narrow_to_wide_ratio >= sample_count))
{
return false;
}
double cn0_linear =
1000.0 * (narrow_to_wide_ratio - 1.0) /
(sample_count - narrow_to_wide_ratio);
if(!isfinite(cn0_linear) || (cn0_linear <= 0.0))
{
return false;
}
double cn0_dbhz = 10.0 * log10(cn0_linear);
if(!isfinite(cn0_dbhz))
{
return false;
}
*cn0_dbhz_out = clamp_double(cn0_dbhz, 10.0, 60.0);
return true;
}
double gps_tracking_doppler_hz(int32_t carrier_freq_q7)
{
return ((double)carrier_freq_q7 / GPS_TRCK_CARRIER_Q7_SCALE) -
GPS_TRCK_IF_CENTER_HZ;
}
double gps_tracking_doppler_to_range_rate_mps(double doppler_hz)
{
const double wavelength_m =
(double)SPEED_OF_LIGHT / GPS_L1_CARRIER_HZ;
/* Positive received-frequency Doppler means decreasing geometric range. */
return -doppler_hz * wavelength_m;
}
bool gps_tracking_measurement_read(
volatile const gps_trck_t* tracking_ip,
gps_tracking_measurement_t* measurement_out)
{
if((tracking_ip == NULL) || (measurement_out == NULL))
{
return false;
}
for(unsigned int attempt = 0u;
attempt < GPS_TRACK_MEASUREMENT_READ_ATTEMPTS;
attempt++)
{
uint32_t sequence_before = tracking_ip->MEASUREMENT_SEQUENCE;
uint32_t meta = tracking_ip->MEASUREMENT_META;
int32_t carrier_freq_q7 = tracking_ip->MEASUREMENT_CARRIER_Q7;
uint32_t absolute_sample = tracking_ip->MEASUREMENT_ABS_SAMPLE;
uint32_t remaining_code_phase =
tracking_ip->MEASUREMENT_REM_CODE_PHASE;
int32_t sum_i = tracking_ip->MEASUREMENT_SUM_I;
int32_t sum_q = tracking_ip->MEASUREMENT_SUM_Q;
uint32_t wbp_low = tracking_ip->MEASUREMENT_WBP_LO;
uint32_t wbp_high = tracking_ip->MEASUREMENT_WBP_HI;
uint32_t sequence_after = tracking_ip->MEASUREMENT_SEQUENCE;
if(sequence_before != sequence_after)
{
continue;
}
memset(measurement_out, 0, sizeof(*measurement_out));
measurement_out->prn = tracking_ip->PRN_NUMBER;
measurement_out->sequence = sequence_after & 0xffffu;
measurement_out->absolute_sample = absolute_sample;
measurement_out->remaining_code_phase = remaining_code_phase;
measurement_out->carrier_freq_q7 = carrier_freq_q7;
measurement_out->cn0_sum_i = sum_i;
measurement_out->cn0_sum_q = sum_q;
measurement_out->cn0_wbp =
((uint64_t)(wbp_high & 0x3ffffu) << 32) | wbp_low;
measurement_out->cn0_sample_count =
(meta >> GPS_TRCK_MEAS_META_COUNT_SHIFT) &
GPS_TRCK_MEAS_META_COUNT_MASK;
measurement_out->nav_bit =
(meta & (1u << GPS_TRCK_MEAS_META_NAV_BIT_POS)) != 0u;
measurement_out->locked =
(meta & (1u << GPS_TRCK_MEAS_META_LOCK_POS)) != 0u;
measurement_out->doppler_hz =
gps_tracking_doppler_hz(carrier_freq_q7);
measurement_out->pseudorange_rate_mps =
gps_tracking_doppler_to_range_rate_mps(
measurement_out->doppler_hz);
unsigned int version =
meta >> GPS_TRCK_MEAS_META_VERSION_SHIFT;
bool cn0_valid = gps_tracking_cn0_dbhz(
sum_i, sum_q, measurement_out->cn0_wbp,
measurement_out->cn0_sample_count,
&measurement_out->cn0_dbhz);
measurement_out->valid =
(version == GPS_TRCK_MEAS_META_VERSION) &&
((meta & (1u << GPS_TRCK_MEAS_META_VALID_POS)) != 0u) &&
measurement_out->locked &&
(measurement_out->cn0_sample_count == 20u) &&
cn0_valid &&
isfinite(measurement_out->doppler_hz) &&
isfinite(measurement_out->pseudorange_rate_mps);
return measurement_out->valid;
}
memset(measurement_out, 0, sizeof(*measurement_out));
return false;
}
static bool solve_four_by_four(
double normal[4][4],
double rhs[4],
double solution[4])
{
double augmented[4][5];
for(unsigned int row = 0u; row < 4u; row++)
{
for(unsigned int column = 0u; column < 4u; column++)
{
augmented[row][column] = normal[row][column];
}
augmented[row][4] = rhs[row];
}
for(unsigned int pivot = 0u; pivot < 4u; pivot++)
{
unsigned int best_row = pivot;
double best_value = fabs(augmented[pivot][pivot]);
for(unsigned int row = pivot + 1u; row < 4u; row++)
{
double value = fabs(augmented[row][pivot]);
if(value > best_value)
{
best_value = value;
best_row = row;
}
}
if(!isfinite(best_value) ||
(best_value <= GPS_TRACK_MEASUREMENT_EPSILON))
{
return false;
}
if(best_row != pivot)
{
for(unsigned int column = pivot; column < 5u; column++)
{
double temporary = augmented[pivot][column];
augmented[pivot][column] =
augmented[best_row][column];
augmented[best_row][column] = temporary;
}
}
double divisor = augmented[pivot][pivot];
for(unsigned int column = pivot; column < 5u; column++)
{
augmented[pivot][column] /= divisor;
}
for(unsigned int row = 0u; row < 4u; row++)
{
if(row == pivot)
{
continue;
}
double factor = augmented[row][pivot];
for(unsigned int column = pivot; column < 5u; column++)
{
augmented[row][column] -=
factor * augmented[pivot][column];
}
}
}
for(unsigned int row = 0u; row < 4u; row++)
{
solution[row] = augmented[row][4];
if(!isfinite(solution[row]))
{
return false;
}
}
return true;
}
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)
{
if((receiver_ecef_m == NULL) || (satellite_positions_m == NULL) ||
(satellite_velocities_mps == NULL) ||
(satellite_clock_drifts_sps == NULL) ||
(measurements == NULL) ||
(receiver_velocity_ecef_mps_out == NULL) ||
(receiver_clock_drift_mps_out == NULL) ||
(residual_rms_mps_out == NULL) ||
(count < GPS_TRACK_MEASUREMENT_MIN_SATELLITES) ||
(count > GPS_TRACK_MEASUREMENT_MAX_SATELLITES))
{
return false;
}
double normal[4][4] = {{0.0}};
double rhs[4] = {0.0};
double design[GPS_TRACK_MEASUREMENT_MAX_SATELLITES][4];
double observation[GPS_TRACK_MEASUREMENT_MAX_SATELLITES];
unsigned int used = 0u;
for(unsigned int satellite = 0u; satellite < count; satellite++)
{
const gps_tracking_measurement_t* measurement =
&measurements[satellite];
if(!measurement->valid || !measurement->locked ||
!isfinite(measurement->cn0_dbhz) ||
!isfinite(measurement->pseudorange_rate_mps))
{
continue;
}
double delta[3];
double range_squared = 0.0;
for(unsigned int component = 0u; component < 3u; component++)
{
delta[component] =
satellite_positions_m[satellite][component] -
receiver_ecef_m[component];
range_squared += delta[component] * delta[component];
}
double range = sqrt(range_squared);
if(!isfinite(range) || (range < 1.0))
{
continue;
}
double satellite_los_rate = 0.0;
for(unsigned int component = 0u; component < 3u; component++)
{
double unit_los = delta[component] / range;
design[used][component] = -unit_los;
satellite_los_rate +=
unit_los *
satellite_velocities_mps[satellite][component];
}
design[used][3] = 1.0;
observation[used] =
measurement->pseudorange_rate_mps -
satellite_los_rate +
(double)SPEED_OF_LIGHT *
satellite_clock_drifts_sps[satellite];
/* Doppler precision improves approximately with signal amplitude. */
double sigma_rate_mps =
30.0 * pow(10.0, -measurement->cn0_dbhz / 20.0);
sigma_rate_mps = clamp_double(sigma_rate_mps, 0.05, 5.0);
double weight = 1.0 / (sigma_rate_mps * sigma_rate_mps);
for(unsigned int row = 0u; row < 4u; row++)
{
rhs[row] +=
weight * design[used][row] * observation[used];
for(unsigned int column = 0u; column < 4u; column++)
{
normal[row][column] +=
weight * design[used][row] *
design[used][column];
}
}
used++;
}
if(used < GPS_TRACK_MEASUREMENT_MIN_SATELLITES)
{
return false;
}
double solution[4];
if(!solve_four_by_four(normal, rhs, solution))
{
return false;
}
double residual_square_sum = 0.0;
for(unsigned int row = 0u; row < used; row++)
{
double predicted = 0.0;
for(unsigned int column = 0u; column < 4u; column++)
{
predicted += design[row][column] * solution[column];
}
double residual = observation[row] - predicted;
residual_square_sum += residual * residual;
}
memcpy(receiver_velocity_ecef_mps_out, solution,
3u * sizeof(solution[0]));
*receiver_clock_drift_mps_out = solution[3];
*residual_rms_mps_out =
sqrt(residual_square_sum / (double)used);
return isfinite(*residual_rms_mps_out);
}
soc/software/gps_system/tests/Makefile
+38 -1
View File
@@ -6,6 +6,8 @@ TARGETS := test_imu_calibration test_gps_nav_epoch \
test_gps_nav_epoch_coalescing test_gps_time_anchor \
test_plic_warm_reset test_gps_acq_manager \
test_gps_ephemeris_cache test_gps_receiver_hardware_sim
TARGETS += test_gps_tracking_measurement
TARGETS += test_gps_satellite_velocity
.PHONY: all run sanitize clean
@@ -37,6 +39,17 @@ test_gps_receiver_hardware_sim: test_gps_receiver_hardware_sim.c \
$(ROOT)/src/gps/gps_ephemeris_cache.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -o $@
test_gps_tracking_measurement: test_gps_tracking_measurement.c \
$(ROOT)/src/gps/gps_tracking_measurement.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
test_gps_satellite_velocity: test_gps_satellite_velocity.c \
$(ROOT)/src/gps/NavEquations.c \
$(ROOT)/src/gps/matrixLib.c
$(CC) $(CFLAGS) -Wno-comment -Wno-unused-parameter \
-Wno-unused-variable -Wno-sign-compare \
-Ifake_bsp -I$(ROOT)/header $^ -lm -o $@
run: $(TARGETS)
./test_imu_calibration
./test_gps_nav_epoch
@@ -46,6 +59,8 @@ run: $(TARGETS)
./test_gps_acq_manager
./test_gps_ephemeris_cache
./test_gps_receiver_hardware_sim
./test_gps_tracking_measurement
./test_gps_satellite_velocity
sanitize:
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
@@ -87,9 +102,31 @@ sanitize:
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 \
./test_gps_receiver_hardware_sim_san
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
-fsanitize=address,undefined -fno-omit-frame-pointer \
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
test_gps_tracking_measurement.c \
$(ROOT)/src/gps/gps_tracking_measurement.c -lm \
-o test_gps_tracking_measurement_san
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 \
./test_gps_tracking_measurement_san
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
-fsanitize=address,undefined -fno-omit-frame-pointer \
-Wno-comment -Wno-unused-parameter -Wno-unused-variable \
-Wno-sign-compare -Ifake_bsp -I$(ROOT)/header \
test_gps_satellite_velocity.c \
$(ROOT)/src/gps/NavEquations.c \
$(ROOT)/src/gps/matrixLib.c -lm \
-o test_gps_satellite_velocity_san
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 \
./test_gps_satellite_velocity_san
clean:
rm -f $(TARGETS) test_gps_nav_epoch_coalescing_repro \
test_gps_nav_epoch_san test_gps_nav_epoch_coalescing_san \
test_gps_acq_manager_san test_gps_ephemeris_cache_san \
test_gps_receiver_hardware_sim_san
test_gps_receiver_hardware_sim_san \
test_gps_tracking_measurement_san \
test_gps_satellite_velocity_san
soc/software/gps_system/tests/test_gps_satellite_velocity.c
+149
View File
@@ -0,0 +1,149 @@
#include "gps/NavEquations.h"
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
static void test_broadcast_ephemeris_velocity(void)
{
gps_ephemeris_t ephemeris;
memset(&ephemeris, 0, sizeof(ephemeris));
ephemeris.tow.week = 2400;
ephemeris.tow.sec = 345700.0;
ephemeris.toe.week = 2400;
ephemeris.toe.sec = 345600.0;
ephemeris.toc.week = 2400;
ephemeris.toc.sec = 345600.0;
ephemeris.sqrta = 5153.7954775;
ephemeris.A = ephemeris.sqrta * ephemeris.sqrta;
ephemeris.ecc = 0.0093;
ephemeris.m0 = 0.75;
ephemeris.deltan = 4.5e-9;
ephemeris.cuc = 1.0e-6;
ephemeris.cus = -3.0e-6;
ephemeris.crc = 180.0;
ephemeris.crs = -75.0;
ephemeris.cic = 1.2e-7;
ephemeris.cis = -8.0e-8;
ephemeris.inc0 = 0.96;
ephemeris.idot = -2.0e-10;
/* Legacy names: omgdot=Omega0, omg0=omega, omgkdot=Omega-dot. */
ephemeris.omgdot = 1.1;
ephemeris.omg0 = -0.4;
ephemeris.omgkdot = -8.0e-9;
ephemeris.af0 = 2.3e-5;
ephemeris.af1 = -4.0e-12;
ephemeris.af2 = 1.0e-19;
ephemeris.tgd = -2.0e-9;
double position[3];
double velocity[3];
double clock[2];
double correction;
satpos(&ephemeris, position, velocity, clock, &correction);
const double dt = 0.05;
gps_ephemeris_t before = ephemeris;
gps_ephemeris_t after = ephemeris;
before.tow.sec -= dt;
after.tow.sec += dt;
double position_before[3], position_after[3];
double velocity_unused[3], clock_before[2], clock_after[2];
double correction_before, correction_after;
satpos(&before, position_before, velocity_unused,
clock_before, &correction_before);
satpos(&after, position_after, velocity_unused,
clock_after, &correction_after);
double radius_squared = 0.0;
double speed_squared = 0.0;
for(unsigned int component = 0u; component < 3u; component++)
{
double finite_difference =
(position_after[component] - position_before[component]) /
(2.0 * dt);
assert(fabs(velocity[component] - finite_difference) < 0.05);
radius_squared += position[component] * position[component];
speed_squared += velocity[component] * velocity[component];
}
double finite_clock_drift =
(correction_after - correction_before) / (2.0 * dt);
assert(fabs(clock[1] - finite_clock_drift) < 1.0e-13);
assert(fabs(clock[0] - correction) < 1.0e-18);
assert(sqrt(radius_squared) > 25000000.0);
assert(sqrt(radius_squared) < 28000000.0);
assert(sqrt(speed_squared) > 1000.0);
assert(sqrt(speed_squared) < 5000.0);
}
static void test_cn0_changes_position_weight(void)
{
const unsigned int satellite_count = 6u;
const double receiver[3] = {6378137.0, 0.0, 0.0};
const double directions[6][3] = {
{ 1.0, 0.0, 0.0},
{ 0.2, 0.9, 0.3872983346207417},
{ 0.3, -0.4, 0.8660254037844386},
{-0.2, 0.5, 0.8426149773176358},
{ 0.4, -0.8, -0.4472135954999579},
{-0.5, -0.5, 0.7071067811865475}
};
mat_type* satellite_positions =
create_matrix((int)satellite_count, 3, "testSatPos");
mat_type* observations =
create_matrix(1, (int)satellite_count, "testObs");
mat_type* az = create_matrix(1, (int)satellite_count, "testAz");
mat_type* el = create_matrix(1, (int)satellite_count, "testEl");
assert(satellite_positions != NULL);
assert(observations != NULL);
assert(az != NULL);
assert(el != NULL);
initialize_all(0.0, satellite_positions);
initialize_all(0.0, observations);
initialize_all(0.0, az);
initialize_all(0.0, el);
unsigned char ura[6] = {0u, 0u, 0u, 0u, 0u, 0u};
double cn0_dbhz[6] = {52.0, 22.0, 45.0, 45.0, 45.0, 45.0};
for(unsigned int satellite = 0u;
satellite < satellite_count;
satellite++)
{
double range_squared = 0.0;
for(unsigned int component = 0u; component < 3u; component++)
{
satellite_positions->matrix[satellite][component] =
receiver[component] +
directions[satellite][component] * 21000000.0;
double difference =
satellite_positions->matrix[satellite][component] -
receiver[component];
range_squared += difference * difference;
}
observations->matrix[0][satellite] =
sqrt(range_squared) + 125.0;
}
gps_nav_solution_t solution;
assert(leastSquaresSTD(
satellite_positions, observations, (int)satellite_count,
az, el, ura, cn0_dbhz, NULL, 345700.0, &solution));
assert(isfinite(solution.measurement_sigma_m[0]));
assert(isfinite(solution.measurement_sigma_m[1]));
assert(solution.measurement_sigma_m[1] >
solution.measurement_sigma_m[0] + 20.0);
free_matrix(satellite_positions);
free_matrix(observations);
free_matrix(az);
free_matrix(el);
}
int main(void)
{
test_broadcast_ephemeris_velocity();
test_cn0_changes_position_weight();
puts("test_gps_satellite_velocity PASS");
return 0;
}
soc/software/gps_system/tests/test_gps_tracking_measurement.c
+179
View File
@@ -0,0 +1,179 @@
#include "gps/gps_tracking_measurement.h"
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#define TEST_SATELLITES 6u
static bool close_to(double actual, double expected, double tolerance)
{
return fabs(actual - expected) <= tolerance;
}
static uint64_t cn0_wbp_for_target(
int32_t sum_i,
int32_t sum_q,
unsigned int sample_count,
double cn0_dbhz)
{
double cn0_linear = pow(10.0, cn0_dbhz / 10.0);
double scaled = cn0_linear / 1000.0;
double ratio =
(scaled * (double)sample_count + 1.0) / (scaled + 1.0);
double narrowband =
(double)sum_i * (double)sum_i +
(double)sum_q * (double)sum_q;
return (uint64_t)llround(narrowband / ratio);
}
static void test_cn0_and_mailbox_decode(void)
{
gps_trck_t registers;
memset(&registers, 0, sizeof(registers));
const int32_t sum_i = 20000;
const int32_t sum_q = 1000;
const uint64_t wbp =
cn0_wbp_for_target(sum_i, sum_q, 20u, 45.0);
const int32_t carrier_q7 =
(int32_t)llround((GPS_TRCK_IF_CENTER_HZ + 2500.0) *
GPS_TRCK_CARRIER_Q7_SCALE);
registers.PRN_NUMBER = 7u;
registers.MEASUREMENT_META =
(GPS_TRCK_MEAS_META_VERSION <<
GPS_TRCK_MEAS_META_VERSION_SHIFT) |
(20u << GPS_TRCK_MEAS_META_COUNT_SHIFT) |
(1u << GPS_TRCK_MEAS_META_VALID_POS) |
(1u << GPS_TRCK_MEAS_META_NAV_BIT_POS) |
(1u << GPS_TRCK_MEAS_META_LOCK_POS);
registers.MEASUREMENT_SEQUENCE = 0x1234u;
registers.MEASUREMENT_CARRIER_Q7 = carrier_q7;
registers.MEASUREMENT_ABS_SAMPLE = 0x89abcdefu;
registers.MEASUREMENT_REM_CODE_PHASE = 555u;
registers.MEASUREMENT_SUM_I = sum_i;
registers.MEASUREMENT_SUM_Q = sum_q;
registers.MEASUREMENT_WBP_LO = (uint32_t)wbp;
registers.MEASUREMENT_WBP_HI = (uint32_t)(wbp >> 32);
gps_tracking_measurement_t measurement;
assert(gps_tracking_measurement_read(&registers, &measurement));
assert(measurement.valid);
assert(measurement.locked);
assert(measurement.nav_bit);
assert(measurement.prn == 7u);
assert(measurement.sequence == 0x1234u);
assert(measurement.absolute_sample == 0x89abcdefu);
assert(measurement.remaining_code_phase == 555u);
assert(measurement.cn0_sample_count == 20u);
assert(measurement.cn0_wbp == wbp);
assert(close_to(measurement.cn0_dbhz, 45.0, 0.01));
assert(close_to(measurement.doppler_hz, 2500.0, 1.0 / 128.0));
const double expected_range_rate =
-2500.0 * ((double)SPEED_OF_LIGHT / GPS_L1_CARRIER_HZ);
assert(close_to(
measurement.pseudorange_rate_mps,
expected_range_rate,
1.0e-9));
registers.MEASUREMENT_META &=
~(1u << GPS_TRCK_MEAS_META_LOCK_POS);
assert(!gps_tracking_measurement_read(&registers, &measurement));
assert(!measurement.valid);
}
static void test_velocity_wls(void)
{
const double receiver_position[3] = {6378137.0, 0.0, 0.0};
const double directions[TEST_SATELLITES][3] = {
{ 1.0, 0.0, 0.0},
{ 0.0, 1.0, 0.0},
{ 0.0, 0.0, 1.0},
{-0.5773502691896258, 0.5773502691896258, 0.5773502691896258},
{ 0.7071067811865475, -0.7071067811865475, 0.0},
{ 0.4082482904638631, 0.4082482904638631, -0.8164965809277261}
};
const double satellite_velocities[TEST_SATELLITES][3] = {
{ 0.0, 2700.0, 100.0},
{-900.0, 0.0, 2500.0},
{1200.0, -2100.0, 0.0},
{1800.0, 400.0, -800.0},
{-500.0, 1900.0, 600.0},
{ 700.0, -1200.0, 2200.0}
};
const double satellite_clock_drift[TEST_SATELLITES] = {
1.0e-11, -2.0e-11, 0.5e-11, 1.5e-11, -0.7e-11, 2.2e-11
};
const double expected_velocity[3] = {12.0, -4.0, 1.5};
const double expected_clock_drift_mps = 18.0;
double satellite_positions[TEST_SATELLITES][3];
gps_tracking_measurement_t measurements[TEST_SATELLITES];
memset(measurements, 0, sizeof(measurements));
for(unsigned int satellite = 0u;
satellite < TEST_SATELLITES;
satellite++)
{
double los_satellite_velocity = 0.0;
double los_receiver_velocity = 0.0;
for(unsigned int component = 0u; component < 3u; component++)
{
satellite_positions[satellite][component] =
receiver_position[component] +
directions[satellite][component] * 21000000.0;
los_satellite_velocity +=
directions[satellite][component] *
satellite_velocities[satellite][component];
los_receiver_velocity +=
directions[satellite][component] *
expected_velocity[component];
}
measurements[satellite].valid = true;
measurements[satellite].locked = true;
measurements[satellite].cn0_dbhz = 42.0 + (double)satellite;
measurements[satellite].pseudorange_rate_mps =
los_satellite_velocity -
los_receiver_velocity +
expected_clock_drift_mps -
(double)SPEED_OF_LIGHT *
satellite_clock_drift[satellite];
}
double solved_velocity[3];
double solved_clock_drift;
double residual_rms;
assert(gps_tracking_velocity_wls(
receiver_position,
satellite_positions,
satellite_velocities,
satellite_clock_drift,
measurements,
TEST_SATELLITES,
solved_velocity,
&solved_clock_drift,
&residual_rms));
for(unsigned int component = 0u; component < 3u; component++)
{
assert(close_to(
solved_velocity[component],
expected_velocity[component],
1.0e-9));
}
assert(close_to(
solved_clock_drift,
expected_clock_drift_mps,
1.0e-9));
assert(residual_rms < 1.0e-9);
}
int main(void)
{
test_cn0_and_mailbox_decode();
test_velocity_wls();
puts("test_gps_tracking_measurement PASS");
return 0;
}