Fix GPS acquisition visibility and pull-in edge cases

This commit is contained in:
Codex
2026-08-04 13:48:48 +03:00
parent 30179df8e6
commit 28b55fdfb6
7 changed files with 333 additions and 19 deletions
+2 -2
View File
@@ -213,7 +213,7 @@ always_ff @(posedge acq_clk or negedge acq_rstn)
S_PULLIN : begin
pullin_flag <= 1;
if (pullin_flag == 0) begin
freq_bin_cnt <= max_freq_bin_idx - 1;
freq_bin_cnt <= (max_freq_bin_idx == 0) ? 0 : (max_freq_bin_idx - 1);
max_corr <= 0;
//STATE <= S_START;
STATE <= S_WR_DATA;
@@ -314,7 +314,7 @@ always_ff @(posedge afe_clk or negedge afe_rstn) begin
wr_mem_end <= 0;
if (sample_align_i && wr_mem_start_r2) begin
wr_mem_en <= 1;
end else if (rf_wr_addr == 2*SAMPLES_PER_CODE) begin // 2 code lenght
end else if (rf_wr_addr == 2*SAMPLES_PER_CODE - 1) begin // 2 code length
wr_mem_en <= 0;
wr_mem_end <= 1;
end
+3 -2
View File
@@ -895,8 +895,9 @@ era base'i (`2048`) kullanilir. Sonra:
ile kullanilir ve sonraki issue-bearing page yine mismatch kontrolu yapar,
- almanak `WNa/toa` age'i yedi gunden eskiyse, iono/UTC snapshot'i yedi gunden
eskiyse reddedilir; son konum coarse seed olarak en fazla 30 gun tutulur,
- fresh complete almanak + konum varsa sky scheduler live TOW ile hemen
guncellenir. Partial/stale almanakta davranis yine tum 32 PRN round-robin'dir.
- fresh complete almanak + restored konum WLS/coarse seed olarak kullanilir, fakat
sky scheduler'i PRN suppression moduna sokmaz; bunun icin live position gerekir.
Partial/stale almanakta davranis yine tum 32 PRN round-robin'dir.
Combined full-device MCS programlama islemi programmer erase politikasi
nedeniyle runtime journal'i silebilir; bu yeni firmware'in normal QSPI runtime
@@ -80,8 +80,9 @@
* Probe one unknown PRN at a controlled rate until tracking capacity is
* filled; do not let one failing predicted-visible PRN block discovery. */
#define GPS_ACQ_UNKNOWN_PROBE_TICKS (GPS_SYSTICK_HZ * 2u)
#define GPS_ACQ_VISIBILITY_PROBE_TICKS (GPS_SYSTICK_HZ * 60u)
#define GPS_ACQ_VISIBILITY_PROBE_TICKS (GPS_SYSTICK_HZ * 5u)
#define GPS_ACQ_MIN_ELEVATION_DEG 5.0
#define GPS_ACQ_MIN_SKY_MODEL_VISIBLE_COUNT 6u
#define GPS_EPHEMERIS_CACHE_MAX_AGE_TICKS \
(GPS_SYSTICK_HZ * 2u * 60u * 60u)
/* Almanac is a coarse acquisition aid. Reject RAM records that have
@@ -91,6 +92,7 @@
#define GPS_ACQ_STATUS_LOG_PERIOD_TICKS (GPS_SYSTICK_HZ * 5u)
#define GPS_NAV_STATUS_LOG_PERIOD_TICKS GPS_SYSTICK_HZ
#define GPS_TRK_MEASUREMENT_STALE_TICKS (GPS_SYSTICK_HZ * 3u)
#define GPS_TRK_MEASUREMENT_LIVENESS_TICKS (GPS_SYSTICK_HZ * 2u)
#define GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS 25u
#define GPS_NAV_EPOCH_HOLDOFF_MS 30u
#define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001
@@ -113,6 +115,8 @@
#endif
_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
"GPS_NAV_ENABLE_FDE must be zero or one");
_Static_assert(GPS_ACQ_MIN_SKY_MODEL_VISIBLE_COUNT <= GPS_PRN_COUNT,
"sky-model visible count threshold exceeds PRN count");
#if (ACQ_ENGINE_COUNT < 1UL)
#error "ACQ_ENGINE_COUNT must be at least 1"
@@ -65,6 +65,8 @@ 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 uint64_t
gps_latest_tracking_measurement_tick[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];
@@ -200,6 +202,8 @@ static void gps_update_tracking_measurement_isr(
}
measurement.local_timestamp_ticks = local_timestamp_ticks;
gps_latest_tracking_measurement[tracking_index] = measurement;
gps_latest_tracking_measurement_tick[tracking_index] =
gps_get_systick();
}
static bool gps_copy_latest_tracking_measurement(
@@ -433,6 +437,7 @@ static void gps_clear_tracking_slot(unsigned char 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]));
gps_latest_tracking_measurement_tick[trck_ip_no] = 0u;
}
static void gps_set_tracking_nav_bits_mode(unsigned char trck_ip_no)
@@ -666,6 +671,25 @@ void gps_acq_update_visibility_hint(
predicted_visible, priority);
}
if(visible_count < GPS_ACQ_MIN_SKY_MODEL_VISIBLE_COUNT)
{
for(unsigned int prn = 0u ; prn < GPS_PRN_COUNT ; prn++)
{
(void)gps_acq_manager_set_visibility(
&gps_acq_manager, prn, false, false, 0);
}
gps_acq_manager_set_visibility_mode(
&gps_acq_manager, false, now);
gps_last_visibility_update_tick = now;
gps_acq_schedule_pending = true;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_SCHED,
"acquisition sky model rejected: predicted_visible=%u min=%u known=%u ephemeris=%u almanac=%u policy=all_prn_round_robin\r\n",
visible_count, GPS_ACQ_MIN_SKY_MODEL_VISIBLE_COUNT,
known_count, ephemeris_orbit_count,
almanac_orbit_count);
return;
}
gps_acq_manager_set_visibility_mode(
&gps_acq_manager, true, now);
gps_last_visibility_update_tick = now;
@@ -873,6 +897,55 @@ static bool gps_tracking_prn_active(unsigned int prn_no)
return false;
}
static bool gps_tracking_prn_measurement_alive(
unsigned int prn_no,
unsigned char* trck_ip_no_out)
{
if(!gps_prn_valid(prn_no))
{
return false;
}
uint64_t now = gps_get_systick();
for(unsigned char i = 0 ; i < GPS_TRCK_IP_COUNT ; i++)
{
volatile gps_trck_t* trk = trck_ips[i];
uint32_t status = trk->STATUS_REG;
bool busy = (status & (1u << GPS_TRCK_BUSY_POS)) != 0u;
bool locked = (status & (1u << GPS_TRCK_LOCK_POS)) != 0u;
bool stop_unlock =
(status & (1u << GPS_TRCK_STOP_UNLOCK_POS)) != 0u;
bool allocated =
tracking_allocated[i] || trck_ip_exists(&tracking_node, i);
if(!(allocated || busy) || (trk->PRN_NUMBER != prn_no) ||
!busy || !locked || stop_unlock)
{
continue;
}
uint64_t irq_state = bsp_irq_save();
uint64_t last_tick = gps_latest_tracking_measurement_tick[i];
bsp_irq_restore(irq_state);
if((last_tick == 0u) || (now < last_tick) ||
((now - last_tick) > GPS_TRK_MEASUREMENT_LIVENESS_TICKS))
{
continue;
}
gps_tracking_measurement_t measurement;
if(gps_copy_latest_tracking_measurement(i, prn_no, &measurement) &&
measurement.locked)
{
if(trck_ip_no_out != NULL)
{
*trck_ip_no_out = i;
}
return true;
}
}
return false;
}
static void gps_mark_satellite_tracking(unsigned int prn_no, unsigned char trck_ip_no)
{
if(gps_prn_valid(prn_no))
@@ -1507,14 +1580,6 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
(const unsigned int*)gps_subframe, how_tow_sec,
event->tick, &gps_ephemeris_cache, &gps_almanac_cache,
&restored_ephemeris, &restored_position, &assistance_time);
if(restored_position.valid)
{
gps_acq_update_visibility_hint(
restored_position.ecef_m,
restored_position.latitude_deg,
restored_position.longitude_deg,
assistance_time);
}
if(restore_approved &&
!gps_frame_parsed->gps_ephems.ephemeris_valid)
{
@@ -1993,11 +2058,24 @@ static void gps_reconcile_tracked_prns(void)
{
for(unsigned int prn = 0 ; prn < GPS_PRN_COUNT ; prn++)
{
gps_acq_prn_state_t state =
gps_acq_manager_state(&gps_acq_manager, prn);
unsigned char recovered_trk_ip = GPS_PRN_INVALID;
bool stale_alive =
(state == GPS_ACQ_PRN_STALE) &&
gps_tracking_prn_measurement_alive(
prn, &recovered_trk_ip);
if(gps_tracking_prn_active(prn) &&
(gps_acq_manager_state(&gps_acq_manager, prn) !=
GPS_ACQ_PRN_STALE))
((state != GPS_ACQ_PRN_STALE) || stale_alive))
{
gps_acq_manager_mark_tracking(&gps_acq_manager, prn);
if(state == GPS_ACQ_PRN_STALE)
{
gps_mark_satellite_tracking(prn, recovered_trk_ip);
}
else
{
gps_acq_manager_mark_tracking(&gps_acq_manager, prn);
}
}
}
}
+6 -1
View File
@@ -1,5 +1,6 @@
CC ?= gcc
CFLAGS ?= -O2 -std=c11 -Wall -Wextra -Werror
PYTHON ?= python3
ROOT := ..
TARGETS := test_imu_calibration test_gps_nav_epoch \
@@ -18,7 +19,7 @@ TARGETS += test_gps_flash_journal
TARGETS += test_gps_assistance_data
TARGETS += test_gps_assistance_store
.PHONY: all run sanitize clean
.PHONY: all run python-models sanitize clean
all: $(TARGETS)
@@ -128,6 +129,10 @@ run: $(TARGETS)
./test_gps_pps_quality
./test_gps_pps_adaptive_loop
./test_gps_visibility
$(PYTHON) test_acq_search_model.py
python-models:
$(PYTHON) test_acq_search_model.py
sanitize:
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
@@ -0,0 +1,117 @@
#!/usr/bin/env python3
"""Model-level checks for acquisition search edge cases."""
from pathlib import Path
import re
TEST_DIR = Path(__file__).resolve().parent
REPO_ROOT = TEST_DIR.parents[3]
DEFINITIONS = REPO_ROOT / "hardware/external/gps/rtl/common/definitions.sv"
ACQ_TOP = REPO_ROOT / "hardware/external/gps/rtl/acq_src/acq_top.sv"
def read_parameters(path):
text = path.read_text(encoding="utf-8")
params = {}
parameter_re = (
r"parameter\s+([A-Za-z_][A-Za-z0-9_]*)\s*=\s*([^;]+);"
)
for match in re.finditer(parameter_re, text):
value = match.group(2).strip()
if re.fullmatch(r"\d+", value):
params[match.group(1)] = int(value)
elif re.fullmatch(r"\d+\.\d+", value):
params[match.group(1)] = float(value)
return params
def check(condition, message):
if not condition:
raise AssertionError(message)
def main():
params = read_parameters(DEFINITIONS)
acq_top = ACQ_TOP.read_text(encoding="utf-8")
samples_per_code = params["SAMPLES_PER_CODE"]
num_freq_bin = params["NUM_FREQ_BIN"]
freq_bin_wl = params["FREQ_BIN_WL"]
dopp_step = params["DOPP_STEP"]
pullin_freq_step = params["PULLIN_FREQ_STEP"]
num_pullin = params["NUM_PULLIN"]
acq_base_freq = params["ACQ_BASE_FREQ"]
check(samples_per_code == 16368, "unexpected sample count model")
check(num_freq_bin == 41, "unexpected coarse Doppler bin count")
check(num_pullin == 11, "unexpected pull-in bin count")
check(dopp_step == 500, "unexpected coarse Doppler step")
check(pullin_freq_step == 100, "unexpected pull-in Doppler step")
check(acq_base_freq == 4082000, "unexpected acquisition base frequency")
check(
"freq_bin_cnt <= (max_freq_bin_idx == 0) ? 0 : "
"(max_freq_bin_idx - 1);" in acq_top,
"RTL must saturate pull-in search at coarse bin zero",
)
check(
"rf_wr_addr == 2*SAMPLES_PER_CODE - 1" in acq_top,
"RTL must capture exactly two 1 ms code windows",
)
max_encoded_bin = (1 << freq_bin_wl) - 1
legacy_bin_zero_start = (0 - 1) & max_encoded_bin
check(
legacy_bin_zero_start >= num_freq_bin,
"legacy bin-zero decrement no longer demonstrates an out-of-range wrap",
)
covered_frequencies = set()
for coarse_bin in range(num_freq_bin):
pullin_start = 0 if coarse_bin == 0 else coarse_bin - 1
check(0 <= pullin_start < num_freq_bin, "pull-in start out of coarse range")
coarse_frequency = acq_base_freq + dopp_step * coarse_bin
for pullin_bin in range(num_pullin):
pullin_frequency = (
acq_base_freq +
dopp_step * pullin_start +
pullin_freq_step * pullin_bin
)
covered_frequencies.add(pullin_frequency)
if coarse_bin == 0:
check(
pullin_frequency >= acq_base_freq,
"coarse bin zero pull-in searched below the table",
)
else:
check(
coarse_frequency - dopp_step <= pullin_frequency <=
coarse_frequency + dopp_step,
"pull-in search does not cover the expected +/-500 Hz window",
)
lowest_expected = acq_base_freq
highest_expected = (
acq_base_freq +
dopp_step * (num_freq_bin - 2) +
pullin_freq_step * (num_pullin - 1)
)
check(min(covered_frequencies) == lowest_expected, "unexpected low search edge")
check(max(covered_frequencies) == highest_expected, "unexpected high search edge")
check(len(covered_frequencies) >= num_freq_bin * 5, "pull-in coverage collapsed")
last_sample_written = (2 * samples_per_code) - 1
check(last_sample_written == 32735, "unexpected two-code capture fencepost")
print(
"ACQ search model PASS: "
f"coarse_bins={num_freq_bin} pullin_bins={num_pullin} "
f"freq_range={lowest_expected}..{highest_expected}Hz "
f"last_sample={last_sample_written}"
)
if __name__ == "__main__":
main()
@@ -21,6 +21,7 @@ typedef struct {
bool visible[GPS_ACQ_MANAGER_PRN_COUNT];
bool tracked[GPS_ACQ_MANAGER_PRN_COUNT];
bool hardware_busy[GPS_ACQ_MANAGER_PRN_COUNT];
bool measurement_alive[GPS_ACQ_MANAGER_PRN_COUNT];
uint64_t invisible_since[GPS_ACQ_MANAGER_PRN_COUNT];
uint64_t nav_ready_tick[GPS_ACQ_MANAGER_PRN_COUNT];
unsigned int not_busy_polls[GPS_ACQ_MANAGER_PRN_COUNT];
@@ -63,7 +64,9 @@ static unsigned int nav_ready_count(const receiver_sim_t* sim)
return count;
}
static void sim_init(receiver_sim_t* sim)
static void sim_init_with_visibility_probe(
receiver_sim_t* sim,
uint64_t visibility_probe_ticks)
{
memset(sim, 0, sizeof(*sim));
const gps_acq_manager_config_t config = {
@@ -71,7 +74,7 @@ static void sim_init(receiver_sim_t* sim)
.hot_retry_delay_ticks = 30u,
.hot_window_ticks = 7200u,
.unknown_probe_interval_ticks = 2u,
.visibility_probe_interval_ticks = 60u,
.visibility_probe_interval_ticks = visibility_probe_ticks,
.max_hot_streak = 4u
};
gps_acq_manager_init(&sim->manager, &config);
@@ -87,6 +90,11 @@ static void sim_init(receiver_sim_t* sim)
sim->engine_prn = GPS_ACQ_MANAGER_INVALID_PRN;
}
static void sim_init(receiver_sim_t* sim)
{
sim_init_with_visibility_probe(sim, 60u);
}
static void cache_ephemeris(receiver_sim_t* sim, unsigned int prn)
{
gps_ephemeris_t ephemeris;
@@ -141,6 +149,7 @@ static void sim_complete_engine(receiver_sim_t* sim)
gps_acq_manager_mark_tracking(&sim->manager, prn);
sim->tracked[prn] = true;
sim->hardware_busy[prn] = true;
sim->measurement_alive[prn] = true;
sim->not_busy_polls[prn] = 0u;
sim->invisible_since[prn] = 0u;
gps_ephemeris_t restored;
@@ -173,6 +182,7 @@ static void sim_complete_engine(receiver_sim_t* sim)
static void sim_mark_tracking_lost(receiver_sim_t* sim, unsigned int prn)
{
sim->tracked[prn] = false;
sim->measurement_alive[prn] = false;
sim->nav_ready_tick[prn] = 0u;
sim->not_busy_polls[prn] = 0u;
gps_acq_manager_mark_lost(
@@ -201,6 +211,7 @@ static void sim_update_tracking(receiver_sim_t* sim)
SIM_UNLOCK_TICKS)
{
sim->hardware_busy[prn] = false;
sim->measurement_alive[prn] = false;
if(!sim->drop_loss_irq)
{
sim_mark_tracking_lost(sim, prn);
@@ -223,6 +234,29 @@ static void sim_update_tracking(receiver_sim_t* sim)
}
}
static unsigned int manager_count_state(
const receiver_sim_t* sim,
gps_acq_prn_state_t state)
{
return gps_acq_manager_count_state(&sim->manager, state);
}
static void sim_reconcile_stale_locked_measurements(receiver_sim_t* sim)
{
for(unsigned int prn = 0u;
prn < GPS_ACQ_MANAGER_PRN_COUNT;
prn++)
{
if((gps_acq_manager_state(&sim->manager, prn) ==
GPS_ACQ_PRN_STALE) &&
sim->hardware_busy[prn] &&
sim->measurement_alive[prn])
{
gps_acq_manager_mark_tracking(&sim->manager, prn);
}
}
}
static void sim_step(receiver_sim_t* sim)
{
sim->tick++;
@@ -566,6 +600,79 @@ static void test_incomplete_almanac_keeps_full_round_robin(void)
sim.search_attempts[5u]);
}
static void test_wrong_sky_model_safety_probe_finds_real_satellites(void)
{
receiver_sim_t sim;
sim_init_with_visibility_probe(&sim, 5u);
for(unsigned int prn = 0u;
prn < GPS_ACQ_MANAGER_PRN_COUNT;
prn++)
{
assert(gps_acq_manager_set_visibility(
&sim.manager, prn, true, false, -900));
}
for(unsigned int prn = 0u ; prn < 4u ; prn++)
{
assert(gps_acq_manager_set_visibility(
&sim.manager, prn, true, true, 500 - (int)prn));
}
for(unsigned int prn = 10u ; prn < 14u ; prn++)
{
sim.visible[prn] = true;
}
gps_acq_manager_set_visibility_mode(&sim.manager, true, sim.tick);
sim_start_engine(&sim);
uint64_t start_tick = sim.tick;
while((tracked_count(&sim) < 4u) &&
((sim.tick - start_tick) < 120u))
{
sim_step(&sim);
}
assert(tracked_count(&sim) == 4u);
for(unsigned int prn = 10u ; prn < 14u ; prn++)
{
assert(sim.tracked[prn]);
assert(sim.search_attempts[prn] != 0u);
}
printf("wrong-sky-model recovery telemetry: ticks=%lu attempts=[%u,%u,%u,%u]\n",
(unsigned long)(sim.tick - start_tick),
sim.search_attempts[10u], sim.search_attempts[11u],
sim.search_attempts[12u], sim.search_attempts[13u]);
}
static void test_stale_locked_measurements_recover_tracking_state(void)
{
receiver_sim_t sim;
sim_init(&sim);
for(unsigned int prn = 0u ; prn < GPS_TRCK_IP_COUNT ; prn++)
{
sim.visible[prn] = true;
}
sim_start_engine(&sim);
while(tracked_count(&sim) < GPS_TRCK_IP_COUNT)
{
sim_step(&sim);
}
gps_acq_manager_mark_stale(&sim.manager, 7u);
gps_acq_manager_mark_stale(&sim.manager, 8u);
assert(manager_count_state(&sim, GPS_ACQ_PRN_TRACKING) ==
GPS_TRCK_IP_COUNT - 2u);
assert(manager_count_state(&sim, GPS_ACQ_PRN_STALE) == 2u);
sim_reconcile_stale_locked_measurements(&sim);
assert(manager_count_state(&sim, GPS_ACQ_PRN_TRACKING) ==
GPS_TRCK_IP_COUNT);
assert(manager_count_state(&sim, GPS_ACQ_PRN_STALE) == 0u);
gps_acq_manager_mark_stale(&sim.manager, 8u);
sim.measurement_alive[8u] = false;
sim_reconcile_stale_locked_measurements(&sim);
assert(gps_acq_manager_state(&sim.manager, 8u) ==
GPS_ACQ_PRN_STALE);
}
static void test_missed_completion_interrupt_is_polled(void)
{
receiver_sim_t sim;
@@ -620,6 +727,8 @@ int main(void)
test_partial_loss_short_and_15_second_gaps();
test_hot_failures_do_not_starve_cold_search();
test_incomplete_almanac_keeps_full_round_robin();
test_wrong_sky_model_safety_probe_finds_real_satellites();
test_stale_locked_measurements_recover_tracking_state();
test_missed_completion_interrupt_is_polled();
test_missed_tracking_loss_interrupt_is_reconciled();
test_stale_cache_forces_ephemeris_decode();