diff --git a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c index 60e131d..2277756 100644 --- a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c +++ b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c @@ -91,6 +91,7 @@ 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 _Static_assert(GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS < (GPS_NAV_SOLVE_PERIOD_MS / 2u), @@ -147,6 +148,20 @@ static void gps_update_tracking_measurement_isr( measurement.cn0_dbhz = 0.2 * measurement.cn0_dbhz + 0.8 * previous->cn0_dbhz; + /* + * The carrier loop's 20 ms snapshots contain several hertz of + * zero-mean jitter. At L1 that is metres/second of range-rate noise, + * which poor satellite geometry amplifies heavily in velocity WLS. + * A 0.05 pole at the 50 Hz navigation-bit cadence has an approximately + * 0.4 s time constant while retaining real receiver dynamics. + */ + measurement.doppler_hz = + GPS_TRACK_DOPPLER_SMOOTH_ALPHA * measurement.doppler_hz + + (1.0 - GPS_TRACK_DOPPLER_SMOOTH_ALPHA) * + previous->doppler_hz; + measurement.pseudorange_rate_mps = + gps_tracking_doppler_to_range_rate_mps( + measurement.doppler_hz); } measurement.local_timestamp_ticks = local_timestamp_ticks; gps_latest_tracking_measurement[tracking_index] = measurement;