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

fix(gps): synchronize complete raw navigation epochs

Browse Source
This commit is contained in:
onur
2026-07-24 15:00:40 +03:00
parent fe0eb19dc5
commit e57b19b4ee
13 changed files with 344 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/src/gps/NavEquations.c
+10 -7
View File
@@ -636,7 +636,7 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
if((satpos == NULL) || (obs == NULL) || (az == NULL) || (el == NULL) ||
(ura_indices == NULL) || (solution_out == NULL) ||
(satpos->matrix == NULL) || (obs->matrix == NULL) ||
(ActiveSatellitesNumber < MIN_NEEDED_SATELLITE) ||
(ActiveSatellitesNumber < (int)GPS_NAV_MIN_WLS_SATELLITES) ||
((unsigned int)ActiveSatellitesNumber > GPS_TRCK_IP_COUNT))
{
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "least squares skipped invalid inputs, sats=%d\r\n",
@@ -1201,11 +1201,14 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
solution_out->fde_applied = false;
/* Four observations exactly determine x/y/z plus receiver clock bias. With
* zero residual DOF no chi-square integrity statement is possible, but the
* finite WLS solution and geometry covariance are still a valid minimum fix. */
* raw finite WLS solution and covariance remain available to downstream
* fusion, which performs its own temporal innovation test. */
solution_out->solution_valid = solution_out->candidate_valid &&
(((solution_out->used_satellite_count == 4u) &&
(((solution_out->used_satellite_count ==
GPS_NAV_MIN_WLS_SATELLITES) &&
(solution_out->degrees_of_freedom == 0u)) ||
(solution_out->residual_test_valid && solution_out->chi_square_pass));
(solution_out->residual_test_valid &&
solution_out->chi_square_pass));
solution_out->iteration_count = iteration_count;
solution_out->candidate_cycles = gps_read_cycle_counter() - candidate_start_cycles;
@@ -1392,7 +1395,8 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
? (gps_read_cycle_counter() - fde_start_cycles) : 0u;
selected_solution.total_wls_cycles = gps_read_cycle_counter() - total_wls_start_cycles;
selected_solution.solution_valid = selected_solution.candidate_valid &&
(((selected_solution.used_satellite_count == 4u) &&
(((selected_solution.used_satellite_count ==
GPS_NAV_MIN_WLS_SATELLITES) &&
(selected_solution.degrees_of_freedom == 0u)) ||
(selected_solution.residual_test_valid &&
selected_solution.chi_square_pass));
@@ -1406,8 +1410,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
}
*solution_out = selected_solution;
/* Print only publishable fixes. A four-satellite minimum fix is publishable,
* although its separate integrity flag remains false because residual DOF=0. */
/* Print every finite raw fix. Integrity remains a separate downstream input. */
if(solution_out->solution_valid)
{
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
soc/software/gps_system/src/gps/gps_interrupt_handlers.c
+27 -40
View File
@@ -269,7 +269,7 @@ static void gps_nav_publish_active_snapshot(uint32_t channel_mask,
bool reset_assembly)
{
channel_mask &= gps_nav_active_snapshot.channel_mask;
if(gps_mask_count(channel_mask) < MIN_NEEDED_SATELLITE)
if(gps_mask_count(channel_mask) < GPS_NAV_MIN_WLS_SATELLITES)
{
gps_runtime_diagnostics.nav_epoch_incomplete_total++;
gps_nav_discard_active_snapshot(reset_assembly);
@@ -299,16 +299,10 @@ bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot)
now_local_ticks))
{
gps_runtime_diagnostics.nav_epoch_incomplete_total++;
if(gps_nav_epoch_assembly_usable(&gps_nav_epoch_assembly,
MIN_NEEDED_SATELLITE))
{
gps_nav_publish_active_snapshot(
gps_nav_epoch_assembly.received_mask, true);
}
else
{
gps_nav_discard_active_snapshot(true);
}
/* An epoch is an all-or-nothing hardware measurement set. Publishing
* a timed-out subset changes its satellite geometry according to IRQ
* timing and was the source of large raw-position jumps. */
gps_nav_discard_active_snapshot(true);
}
bool ready = gps_nav_completed_snapshot_pending;
@@ -1195,29 +1189,17 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
if(event->pps_capture_valid)
{
uint32_t previous_capture_sequence = node->gps_last_pps_capture_sequence;
if(previous_capture_sequence != 0u)
if(node->gps_last_nav_solve_pps_counter != 0u)
{
uint32_t sequence_delta =
event->pps_capture_sequence - previous_capture_sequence;
if(sequence_delta == 0u)
{
return;
}
uint32_t sequence_delta = 0u;
bool sequence_consistent =
event->pps_counter > node->gps_last_nav_solve_pps_counter;
if(sequence_consistent)
{
uint64_t counter_delta =
event->pps_counter -
node->gps_last_nav_solve_pps_counter;
uint64_t expected_delta =
(counter_delta + (GPS_NAV_CAPTURE_PERIOD_TICKS / 2u)) /
GPS_NAV_CAPTURE_PERIOD_TICKS;
sequence_consistent =
(expected_delta != 0u) &&
(expected_delta <= UINT32_MAX) &&
(sequence_delta == (uint32_t)expected_delta);
}
gps_nav_capture_sequence_consistent(
node->gps_last_nav_solve_pps_counter,
previous_capture_sequence,
event->pps_counter,
event->pps_capture_sequence,
GPS_NAV_CAPTURE_PERIOD_TICKS,
&sequence_delta);
if(sequence_consistent)
{
captured_epoch_count = sequence_delta;
@@ -1236,6 +1218,13 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
previous_capture_sequence,
event->pps_capture_sequence,
gps_runtime_diagnostics.pps_sequence_discontinuity_total);
/* Rebase the newest-only transport metadata, but never let
* an inconsistent counter/sequence pair enter an epoch. */
node->gps_last_nav_solve_pps_counter =
event->pps_counter;
node->gps_last_pps_capture_sequence =
event->pps_capture_sequence;
return;
}
}
/* Capture sequencing is transport metadata. Rebase it even if the
@@ -1310,14 +1299,10 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
if(assembly_result == GPS_NAV_EPOCH_EVENT_RESTARTED)
{
gps_runtime_diagnostics.nav_epoch_incomplete_total++;
if(gps_mask_count(previous_received) >= MIN_NEEDED_SATELLITE)
{
gps_nav_publish_active_snapshot(previous_received, false);
}
else
{
gps_nav_discard_active_snapshot(false);
}
/* The first interrupt from a newer period proves the old expected
* set will never complete. Drop it rather than solving a partial
* satellite set. */
gps_nav_discard_active_snapshot(false);
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"NAV capture epoch advanced: old_expected=0x%x old_received=0x%x new_counter=%lu first_trk=%u\r\n",
previous_expected, previous_received,
@@ -1331,6 +1316,8 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
uint32_t channel_mask = 1u << trck_ip_no;
gps_nav_active_snapshot.channel_mask |= channel_mask;
gps_nav_active_snapshot.prn[trck_ip_no] = event->prn;
gps_nav_active_snapshot.ephemeris[trck_ip_no] =
gps_frame_parsed->gps_ephems;
gps_nav_active_snapshot.tow[trck_ip_no].week = tow_week;
gps_nav_active_snapshot.tow[trck_ip_no].sec = tow_sec;
gps_nav_active_snapshot.raw_sample[trck_ip_no] =
soc/software/gps_system/src/gps/gps_ip.c
+57 -41
View File
@@ -18,6 +18,17 @@
_Static_assert(GPS_TRCK_IP_COUNT <= GPS_NAV_EPOCH_MAX_CANDIDATES,
"GPS nav epoch selector capacity is smaller than the tracking channel count");
static unsigned int gps_nav_mask_count(uint32_t mask)
{
unsigned int count = 0u;
while(mask != 0u)
{
count += mask & 1u;
mask >>= 1u;
}
return count;
}
volatile gps_acq_t* const acq0_ip = (volatile gps_acq_t*)ACQ0_IP_BASE_ADDR;
volatile gps_acq_t* acq_ips[GPS_ACQ_IP_COUNT];
unsigned int acq_ips_intr_lines[GPS_ACQ_IP_COUNT];
@@ -127,7 +138,6 @@ static bool gps_snapshot_has_svid(const gps_ephemeris_t snapshot[GPS_TRCK_IP_COU
return false;
}
static uint32_t gps_nav_epoch_excluded_total;
static uint32_t gps_nav_epoch_reject_total;
static int32_t gps_sign_extend(uint32_t value, unsigned int bits)
@@ -507,6 +517,21 @@ static bool gps_node_has_committed_ephemeris(volatile gps_ephem_ll_t* node)
node->data_p->ephemeris_valid;
}
static bool gps_ephemeris_ready_for_position(
const gps_ephemeris_t* eph,
unsigned int expected_svid)
{
return (eph != NULL) &&
eph->ephemeris_valid &&
(eph->SVID == expected_svid) &&
(eph->update == 0u) &&
(eph->svhlth == 0u) &&
(eph->sva < 15u) &&
(eph->A > 0.0) &&
(eph->ecc >= 0.0) &&
(eph->ecc < 1.0);
}
static bool gps_node_ready_for_position(volatile gps_ephem_ll_t* node)
{
if((node == NULL) || (node->data_p == NULL) || (node->tracking_ip_no >= GPS_TRCK_IP_COUNT))
@@ -530,14 +555,9 @@ static bool gps_node_ready_for_position(volatile gps_ephem_ll_t* node)
return false;
}
gps_ephemeris_t* eph = (gps_ephemeris_t*)node->data_p;
return (eph->SVID == (unsigned char)trck_ips[node->tracking_ip_no]->PRN_NUMBER) &&
(eph->update == 0u) &&
(eph->svhlth == 0u) &&
(eph->sva < 15u) &&
(eph->A > 0.0) &&
(eph->ecc >= 0.0) &&
(eph->ecc < 1.0);
return gps_ephemeris_ready_for_position(
(const gps_ephemeris_t*)node->data_p,
trck_ips[node->tracking_ip_no]->PRN_NUMBER);
}
static bool gps_channel_nav_solve_enabled(unsigned char trck_ip_no)
@@ -655,14 +675,9 @@ static unsigned int gps_collect_completed_epoch_locked(
{
continue;
}
volatile gps_ephem_ll_t* node =
get_element_by_ip_number(&tracking_node, ip_no);
gps_ephemeris_t* eph =
(node != NULL) ? (gps_ephemeris_t*)node->data_p : NULL;
if((node == NULL) || (eph == NULL) ||
!gps_node_ready_for_position(node) ||
!gps_channel_nav_solve_enabled(ip_no) ||
(eph->SVID != epoch->prn[ip_no]) ||
const gps_ephemeris_t* eph = &epoch->ephemeris[ip_no];
if(!gps_ephemeris_ready_for_position(
eph, epoch->prn[ip_no]) ||
gps_snapshot_has_svid(snapshot, count, eph->SVID))
{
continue;
@@ -710,10 +725,11 @@ static unsigned int gps_select_coherent_nav_snapshot(
}
gps_nav_epoch_selection_t selection;
if(!gps_nav_epoch_select(candidates, candidate_count,
GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_SAMPLES,
GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC,
&selection))
if(!gps_nav_epoch_validate_complete(
candidates, candidate_count,
GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_SAMPLES,
GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC,
&selection))
{
return 0u;
}
@@ -783,7 +799,7 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
gps_nav_solution_t* solution_out)
{
if((ephemeris == NULL) || (solution_out == NULL) ||
(satellite_count < MIN_NEEDED_SATELLITE))
(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);
return false;
@@ -1089,7 +1105,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
tracked_count = gps_collect_tracked_channels_locked(
GPS_DEBUG_ENABLED(DEBUG_CHANNELS) ? tracked_prns : NULL);
if(ready_count < MIN_NEEDED_SATELLITE)
if(ready_count < GPS_NAV_MIN_WLS_SATELLITES)
{
gps_initial_position_valid = false;
}
@@ -1105,6 +1121,18 @@ bool calculate_position(gps_nav_solution_t* solution_out,
snapshot, snapshot_ips, snapshot_pps_counters,
snapshot_pps_sequences,
snapshot_local_timestamps, snapshot_raw_samples);
unsigned int completed_channel_count =
gps_nav_mask_count(completed_epoch.channel_mask);
if(candidate_count != completed_channel_count)
{
gps_nav_epoch_reject_total++;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"immutable NAV snapshot rejected: captured=%u valid=%u mask=0x%x rejects=%u\r\n",
completed_channel_count, candidate_count,
completed_epoch.channel_mask,
gps_nav_epoch_reject_total);
candidate_count = 0u;
}
initial_run = !gps_initial_position_valid;
}
}
@@ -1119,26 +1147,16 @@ bool calculate_position(gps_nav_solution_t* solution_out,
snapshot_local_timestamps, snapshot_raw_samples,
candidate_count);
coherent_nav_count = satellite_count;
run_nav = satellite_count >= MIN_NEEDED_SATELLITE;
run_nav = satellite_count >= GPS_NAV_MIN_WLS_SATELLITES;
if(run_nav && (satellite_count < candidate_count))
if(!run_nav)
{
unsigned int excluded_count = candidate_count - satellite_count;
gps_nav_epoch_excluded_total += excluded_count;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"NAV coherent subset selected: candidates=%u selected=%u excluded=%u excluded_total=%u\r\n",
candidate_count, satellite_count, excluded_count,
gps_nav_epoch_excluded_total);
}
else if(!run_nav)
{
if(candidate_count >= MIN_NEEDED_SATELLITE)
if(candidate_count >= GPS_NAV_MIN_WLS_SATELLITES)
{
gps_nav_epoch_reject_total++;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"NAV epoch has no coherent four-satellite subset: candidates=%u best=%u rejects=%u\r\n",
candidate_count, satellite_count,
gps_nav_epoch_reject_total);
"complete NAV epoch rejected as incoherent: candidates=%u rejects=%u\r\n",
candidate_count, gps_nav_epoch_reject_total);
}
}
}
@@ -1210,7 +1228,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
"math_clock_corr=%.6lf "
"isr_overflow=%u plic_repeat=%u drain_limit=%u coalesced=%u lost=%u "
"pps_discontinuity=%u epoch_incomplete=%u snapshot_overwrite=%u how_reject=%u "
"epoch_excluded=%u epoch_rejected=%u\r\n",
"epoch_rejected=%u\r\n",
GPS_NAV_SOLVE_PERIOD_MS,
(unsigned long)GPS_PROFILE_CPU_HZ,
(unsigned long)foreground_cycles,
@@ -1237,7 +1255,6 @@ bool calculate_position(gps_nav_solution_t* solution_out,
runtime_diagnostics.nav_epoch_incomplete_total,
runtime_diagnostics.nav_epoch_snapshot_overwrite_total,
runtime_diagnostics.how_anchor_rejected_total,
gps_nav_epoch_excluded_total,
gps_nav_epoch_reject_total);
(void)foreground_cycles;
(void)nav_software_cycles;
@@ -1380,7 +1397,6 @@ void gps_startup_initialization(unsigned int nsp_tick)
gps_pps_init(nsp_tick);
gps_ionoutc_valid = false;
gps_last_ionoutc = (ionoutc_t){0};
gps_nav_epoch_excluded_total = 0u;
gps_nav_epoch_reject_total = 0u;
for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
{
soc/software/gps_system/src/gps/gps_nav_epoch.c
+56
View File
@@ -179,6 +179,62 @@ bool gps_nav_epoch_select(
return selection->selected_count != 0u;
}
bool gps_nav_epoch_validate_complete(
const gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES],
unsigned int candidate_count,
uint32_t sample_window,
double tow_tolerance_sec,
gps_nav_epoch_selection_t* selection)
{
if(!gps_nav_epoch_select(candidates, candidate_count, sample_window,
tow_tolerance_sec, selection))
{
return false;
}
uint32_t complete_mask = (1u << candidate_count) - 1u;
return (selection->selected_count == candidate_count) &&
(selection->selected_mask == complete_mask);
}
bool gps_nav_capture_sequence_consistent(
uint64_t previous_counter,
uint32_t previous_sequence,
uint64_t current_counter,
uint32_t current_sequence,
uint64_t capture_period_ticks,
uint32_t* sequence_gap)
{
if(sequence_gap != NULL)
{
*sequence_gap = 0u;
}
if((previous_counter == 0u) || (current_counter <= previous_counter) ||
(capture_period_ticks == 0u))
{
return false;
}
uint32_t sequence_delta = current_sequence - previous_sequence;
if(sequence_delta == 0u)
{
return false;
}
uint64_t counter_delta = current_counter - previous_counter;
uint64_t expected_delta =
(counter_delta + (capture_period_ticks / 2u)) /
capture_period_ticks;
if((expected_delta == 0u) || (expected_delta > UINT32_MAX) ||
(sequence_delta != (uint32_t)expected_delta))
{
return false;
}
if(sequence_gap != NULL)
{
*sequence_gap = sequence_delta;
}
return true;
}
void gps_nav_epoch_assembly_reset(gps_nav_epoch_assembly_t* assembly)
{
if(assembly == NULL)