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

fix(gps): adapt Doppler bandwidth from board validation

Browse Source
This commit is contained in:
onur
2026-07-26 03:15:52 +03:00
parent 23650be635
commit 156cfc8b77
24 changed files with 592 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/src/adapters/gps_system_uart.c
+2 -2
View File
@@ -115,7 +115,7 @@ void gps_system_uart_publish_periodic_fusion_status(
if(output->global_valid)
{
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
"state=%s lat=%.10lf lon=%.10lf height=%.3lf "
"speed=%.3lf pos_nis=%.3lf vel_nis=%.3lf "
"gps=%lu/%lu velocity=%lu/%lu recoveries=%lu "
@@ -142,7 +142,7 @@ void gps_system_uart_publish_periodic_fusion_status(
return;
}
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
"state=%s calibration_samples=%u gps=%lu/%lu recoveries=%lu "
"imu=%lu fused_imu=%lu overflow=%lu missed=%lu\r\n",
navigation_fusion_state_name(output->engine_state),
soc/software/gps_system/src/fusion/navigation_fusion.c
+2
View File
@@ -141,6 +141,8 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output)
sqrt(fmax(0.0, runtime->filter.covariance[axis][axis]));
}
output->speed_mps = eskf_speed_mps(&runtime->filter);
output->linear_acceleration_norm_mps2 =
runtime->motion.last_linear_accel_norm_mps2;
output->last_position_nis = runtime->filter.last_position_nis;
output->last_velocity_nis = runtime->filter.last_velocity_nis;
output->motion_state = runtime->motion.state;
soc/software/gps_system/src/gps/gps_interrupt_handlers.c
+31 -3
View File
@@ -9,6 +9,7 @@
#include <bsp/irq.h>
#include <bsp/mmio.h>
#include <bsp/plic.h>
#include <math.h>
#include <string.h>
typedef enum {
@@ -91,9 +92,13 @@ static gps_external_irq_handler_t gps_external_irq_handler = NULL;
(((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS) / 1000u)
#define GPS_NAV_HOLDOFF_LOCAL_TICKS \
(((uint64_t)BSP_CLINT_TIMEBASE_HZ * GPS_NAV_EPOCH_HOLDOFF_MS) / 1000u)
#define GPS_TRACK_DOPPLER_SMOOTH_ALPHA 0.05
#define GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN 0.05
#define GPS_TRACK_ESKF_DOPPLER_ALPHA_MAX 0.40
#define GPS_NAV_TOW_SNAP_TOLERANCE_SEC 0.005
static volatile double gps_tracking_eskf_doppler_alpha =
GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN;
_Static_assert(GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS <
(GPS_NAV_SOLVE_PERIOD_MS / 2u),
"NAV capture window must be below half the solve period");
@@ -112,6 +117,22 @@ static unsigned int gps_mask_count(uint32_t mask)
return count;
}
void gps_tracking_set_eskf_doppler_alpha(double alpha)
{
if(!isfinite(alpha))
{
return;
}
gps_tracking_eskf_doppler_alpha = fmax(
GPS_TRACK_ESKF_DOPPLER_ALPHA_MIN,
fmin(GPS_TRACK_ESKF_DOPPLER_ALPHA_MAX, alpha));
}
double gps_tracking_get_eskf_doppler_alpha(void)
{
return gps_tracking_eskf_doppler_alpha;
}
static double gps_correct_abs_sample(uint32_t abs_sample, uint32_t rem_code_phase)
{
return (double)abs_sample - ((double)rem_code_phase / CODE_FREQ_BASIS);
@@ -157,12 +178,19 @@ static void gps_update_tracking_measurement_isr(
* 0.4 s time constant while retaining real receiver dynamics.
*/
measurement.filtered_doppler_hz =
GPS_TRACK_DOPPLER_SMOOTH_ALPHA * measurement.doppler_hz +
(1.0 - GPS_TRACK_DOPPLER_SMOOTH_ALPHA) *
GPS_TRACK_PPS_DOPPLER_ALPHA * measurement.doppler_hz +
(1.0 - GPS_TRACK_PPS_DOPPLER_ALPHA) *
previous->filtered_doppler_hz;
measurement.filtered_pseudorange_rate_mps =
gps_tracking_doppler_to_range_rate_mps(
measurement.filtered_doppler_hz);
double eskf_alpha = gps_tracking_eskf_doppler_alpha;
measurement.eskf_doppler_hz =
eskf_alpha * measurement.doppler_hz +
(1.0 - eskf_alpha) * previous->eskf_doppler_hz;
measurement.eskf_pseudorange_rate_mps =
gps_tracking_doppler_to_range_rate_mps(
measurement.eskf_doppler_hz);
}
measurement.local_timestamp_ticks = local_timestamp_ticks;
gps_latest_tracking_measurement[tracking_index] = measurement;
soc/software/gps_system/src/gps/gps_ip.c
+63 -5
View File
@@ -69,11 +69,25 @@ static bool gps_pps_reference_svid_valid;
static unsigned int gps_pps_reference_svid;
static bool gps_pps_filter_reference_svid_valid;
static unsigned int gps_pps_filter_reference_svid;
#if GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES > 0
static uint64_t gps_pps_predictor_ab_update_counter;
#endif
#if !defined(GPS_PPS_STATUS_LOG_INTERVAL) || (GPS_PPS_STATUS_LOG_INTERVAL < 1)
#error "GPS_PPS_STATUS_LOG_INTERVAL must be at least one"
#endif
#if !defined(GPS_PPS_DOPPLER_PREDICTOR_ENABLE) || \
((GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 0) && \
(GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 1))
#error "GPS_PPS_DOPPLER_PREDICTOR_ENABLE must be zero or one"
#endif
#if !defined(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES) || \
(GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES < 0)
#error "GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES must be zero or positive"
#endif
volatile uint64_t gps_systick_ticks = 0;
static void gps_init_acq_tables(void)
@@ -854,6 +868,9 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
bool pps_clock_bias_drift_valid = false;
double pps_clock_bias_drift_mps = 0.0;
bool pps_velocity_valid = false;
double eskf_doppler_alpha =
gps_tracking_get_eskf_doppler_alpha();
double pps_velocity_speed_mps = NAN;
double pps_velocity_residual_rms_mps = 0.0;
uint32_t pps_velocity_excluded_row_mask = 0u;
gps_nav_solution_t solution;
@@ -964,6 +981,13 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
}
if(ok && solution.solution_valid)
{
double reference_noise_ratio =
eskf_doppler_alpha *
(2.0 - GPS_TRACK_PPS_DOPPLER_ALPHA) /
(GPS_TRACK_PPS_DOPPLER_ALPHA *
(2.0 - eskf_doppler_alpha));
double eskf_cn0_penalty_db =
10.0 * log10(fmax(1.0, reference_noise_ratio));
double satellite_positions_array[GPS_TRCK_IP_COUNT][3];
double satellite_velocities_array[GPS_TRCK_IP_COUNT][3];
gps_tracking_measurement_t
@@ -978,6 +1002,12 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
velocities->matrix[row][component];
}
velocity_measurements[row] = measurements[row];
velocity_measurements[row].doppler_hz =
measurements[row].eskf_doppler_hz;
velocity_measurements[row].pseudorange_rate_mps =
measurements[row].eskf_pseudorange_rate_mps;
velocity_measurements[row].cn0_dbhz -=
eskf_cn0_penalty_db;
if((solution.used_row_mask & (1u << row)) == 0u)
{
velocity_measurements[row].valid = false;
@@ -1011,10 +1041,13 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
/*
* PPS keeps the existing slow Doppler path as a low-noise derivative
* predictor for the position-WLS reference datum. This result is not
* published to fusion; fusion receives the raw robust solution above.
* published to fusion; fusion receives the adaptive low-latency
* solution above.
*/
for(unsigned int row = 0u; row < satellite_count; row++)
{
velocity_measurements[row].cn0_dbhz =
measurements[row].cn0_dbhz;
velocity_measurements[row].doppler_hz =
measurements[row].filtered_doppler_hz;
velocity_measurements[row].pseudorange_rate_mps =
@@ -1037,6 +1070,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
&pps_velocity_excluded_row_mask);
if(pps_velocity_valid)
{
pps_velocity_speed_mps =
sqrt(pps_velocity_ecef_mps[0] * pps_velocity_ecef_mps[0] +
pps_velocity_ecef_mps[1] * pps_velocity_ecef_mps[1] +
pps_velocity_ecef_mps[2] * pps_velocity_ecef_mps[2]);
pps_clock_bias_drift_valid =
gps_pps_reference_bias_drift(
solution.ecef_m,
@@ -1050,8 +1087,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
(pps_velocity_excluded_row_mask != 0u))
{
GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
"velocity FDE raw_mask=0x%02lx pps_mask=0x%02lx "
"raw_rms=%.3lf pps_rms=%.3lf\r\n",
"velocity FDE adaptive_mask=0x%02lx pps_mask=0x%02lx "
"adaptive_rms=%.3lf pps_rms=%.3lf\r\n",
(unsigned long)solution.velocity_excluded_row_mask,
(unsigned long)pps_velocity_excluded_row_mask,
solution.velocity_residual_rms_mps,
@@ -1178,11 +1215,21 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
}
double filtered_clock_bias_m = 0.0;
double clock_innovation_m = 0.0;
bool pps_predictor_enabled =
(GPS_PPS_DOPPLER_PREDICTOR_ENABLE != 0) &&
pps_clock_bias_drift_valid;
#if GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES > 0
pps_predictor_enabled =
(((gps_pps_predictor_ab_update_counter /
GPS_PPS_DOPPLER_PREDICTOR_AB_UPDATES) & 1u) != 0u) &&
pps_clock_bias_drift_valid;
gps_pps_predictor_ab_update_counter++;
#endif
gps_pps_time_filter_result_t filter_result =
gps_pps_filter_clock_bias(
pps_reference_counter,
solution.clock_bias_m,
pps_clock_bias_drift_valid,
pps_predictor_enabled,
pps_clock_bias_drift_mps,
solution.tdop,
&filtered_clock_bias_m,
@@ -1244,7 +1291,10 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
"PPS profile=%s mode=%s locked=%u tdop=%.2lf gdop=%.2lf "
"clock_sigma=%.2lf cn0=%.1lf/%.1lf innovation=%.2lfm "
"raw_bias=%.3lfm filtered_bias=%.3lfm "
"clock_drift=%.3lfmps bias_drift=%.3lfmps ref_svid=%u "
"clock_drift=%.3lfmps bias_drift=%.3lfmps "
"predictor=%u eskf_alpha=%.2lf "
"eskf_v=%.3lf/%.3lf/0x%02lx "
"pps_v=%.3lf/%.3lf/0x%02lx ref_svid=%u "
"capture_corr=%.2lfns "
"hw_phase=%.2lfns rate_cmd=%.3lfppm integ=%.3lfppm "
"pulse=%u seq=%u status=0x%03x\r\n",
@@ -1261,6 +1311,14 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
filtered_clock_bias_m,
solution.clock_drift_mps,
pps_clock_bias_drift_mps,
pps_predictor_enabled ? 1u : 0u,
eskf_doppler_alpha,
solution.speed_mps,
solution.velocity_residual_rms_mps,
(unsigned long)solution.velocity_excluded_row_mask,
pps_velocity_speed_mps,
pps_velocity_residual_rms_mps,
(unsigned long)pps_velocity_excluded_row_mask,
ephemeris[reference_index].SVID,
pps_capture_correction_sec * 1.0e9,
hardware_phase_ns, rate_command_ppm, integrator_ppm,
soc/software/gps_system/src/gps/gps_receiver.c
+10
View File
@@ -42,3 +42,13 @@ uint64_t gps_receiver_status_ticks_per_second(void)
{
return GPS_SYSTICK_HZ;
}
void gps_receiver_set_eskf_doppler_alpha(double alpha)
{
gps_tracking_set_eskf_doppler_alpha(alpha);
}
double gps_receiver_get_eskf_doppler_alpha(void)
{
return gps_tracking_get_eskf_doppler_alpha();
}
soc/software/gps_system/src/gps/gps_tracking_measurement.c
+6
View File
@@ -141,6 +141,10 @@ bool gps_tracking_measurement_read(
measurement_out->doppler_hz;
measurement_out->filtered_pseudorange_rate_mps =
measurement_out->pseudorange_rate_mps;
measurement_out->eskf_doppler_hz =
measurement_out->doppler_hz;
measurement_out->eskf_pseudorange_rate_mps =
measurement_out->pseudorange_rate_mps;
unsigned int version =
meta >> GPS_TRCK_MEAS_META_VERSION_SHIFT;
@@ -156,6 +160,8 @@ bool gps_tracking_measurement_read(
cn0_valid &&
isfinite(measurement_out->doppler_hz) &&
isfinite(measurement_out->pseudorange_rate_mps) &&
isfinite(measurement_out->eskf_doppler_hz) &&
isfinite(measurement_out->eskf_pseudorange_rate_mps) &&
isfinite(measurement_out->filtered_doppler_hz) &&
isfinite(measurement_out->filtered_pseudorange_rate_mps);
return measurement_out->valid;