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