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

fix(gps): harden navigation epoch recovery

Browse Source
This commit is contained in:
onur
2026-07-24 14:01:58 +03:00
parent ea055a36d2
commit fe0eb19dc5
15 changed files with 1007 additions and 773 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/src/gps/gps_interrupt_handlers.c
+315 -93
View File
@@ -25,7 +25,6 @@ typedef struct {
uint32_t rem_code_phase;
double corrected_abs_sample;
uint32_t nav_solve_timer;
uint32_t nav_solve_event_count;
uint64_t local_timestamp_ticks;
uint64_t pps_counter;
uint32_t pps_capture_sequence;
@@ -40,7 +39,6 @@ typedef struct {
bool nav_solve_pending;
bool misc_pending;
unsigned int nav_bits_overwrite_count;
uint32_t nav_solve_event_count;
gps_isr_event_t drop_event;
gps_isr_event_t nav_bits_event;
gps_isr_event_t nav_solve_event;
@@ -71,14 +69,40 @@ static volatile unsigned int gps_plic_drain_limit_count = 0;
static volatile unsigned int gps_plic_repeat_count = 0;
static volatile bool gps_acq_schedule_pending = true;
static volatile bool gps_timer_maintenance_pending = true;
static volatile bool gps_nav_solve_period_pending = false;
static gps_nav_epoch_assembly_t gps_nav_epoch_assembly;
static gps_nav_epoch_snapshot_t gps_nav_active_snapshot;
static gps_nav_epoch_snapshot_t gps_nav_completed_snapshot;
static bool gps_nav_completed_snapshot_pending;
static volatile gps_runtime_diagnostics_t gps_runtime_diagnostics;
static unsigned int gps_acq_round_robin_prn = 0;
static unsigned int gps_external_irq_id = 0u;
static gps_external_irq_handler_t gps_external_irq_handler = NULL;
#define GPS_PLIC_DRAIN_LIMIT (GPS_TRCK_IP_COUNT + GPS_ACQ_IP_COUNT + 4u)
#define GPS_NAV_CAPTURE_PERIOD_TICKS \
(((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_SOLVE_PERIOD_MS) / 1000u)
#define GPS_NAV_CAPTURE_WINDOW_TICKS \
(((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)
_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");
_Static_assert(GPS_NAV_EPOCH_HOLDOFF_MS >=
GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS,
"NAV holdoff must cover the capture window");
static unsigned int gps_mask_count(uint32_t mask)
{
unsigned int count = 0u;
while(mask != 0u)
{
count += mask & 1u;
mask >>= 1u;
}
return count;
}
static const char* gps_satellite_state_name(gps_satellite_state_t state)
{
@@ -232,37 +256,66 @@ static bool gps_take_timer_maintenance_pending(void)
return pending;
}
bool gps_take_nav_solve_period_ready(void)
static void gps_nav_discard_active_snapshot(bool reset_assembly)
{
bool tracking_shadow_pending = false;
bool acquisition_shadow_pending = false;
memset(&gps_nav_active_snapshot, 0, sizeof(gps_nav_active_snapshot));
if(reset_assembly)
{
gps_nav_epoch_assembly_reset(&gps_nav_epoch_assembly);
}
}
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)
{
gps_runtime_diagnostics.nav_epoch_incomplete_total++;
gps_nav_discard_active_snapshot(reset_assembly);
return;
}
if(gps_nav_completed_snapshot_pending)
{
gps_runtime_diagnostics.nav_epoch_snapshot_overwrite_total++;
}
gps_nav_active_snapshot.channel_mask = channel_mask;
gps_nav_completed_snapshot = gps_nav_active_snapshot;
gps_nav_completed_snapshot_pending = true;
gps_nav_discard_active_snapshot(reset_assembly);
}
bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot)
{
if(snapshot == NULL)
{
return false;
}
uint64_t irq_state = bsp_irq_save();
for(unsigned int i = 0 ; i < GPS_TRCK_IP_COUNT ; i++)
uint64_t now_local_ticks = bsp_read64(BSP_CLINT_MTIME);
if(gps_nav_epoch_assembly_expired(&gps_nav_epoch_assembly,
now_local_ticks))
{
if(trk_shadow[i].drop_pending || trk_shadow[i].nav_bits_pending ||
trk_shadow[i].nav_solve_pending || trk_shadow[i].misc_pending)
gps_runtime_diagnostics.nav_epoch_incomplete_total++;
if(gps_nav_epoch_assembly_usable(&gps_nav_epoch_assembly,
MIN_NEEDED_SATELLITE))
{
tracking_shadow_pending = true;
break;
gps_nav_publish_active_snapshot(
gps_nav_epoch_assembly.received_mask, true);
}
else
{
gps_nav_discard_active_snapshot(true);
}
}
for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
{
if(acq_shadow_pending[i])
{
acquisition_shadow_pending = true;
break;
}
}
bool pipeline_idle = (gps_event_head == gps_event_tail) &&
!tracking_shadow_pending && !acquisition_shadow_pending;
bool ready = gps_nav_solve_period_pending && pipeline_idle;
bool ready = gps_nav_completed_snapshot_pending;
if(ready)
{
/* The complete epoch is handed to the foreground exactly once. Events
* arriving while WLS runs start the next assembly. */
gps_nav_solve_period_pending = false;
gps_nav_epoch_assembly_reset(&gps_nav_epoch_assembly);
*snapshot = gps_nav_completed_snapshot;
gps_nav_completed_snapshot_pending = false;
}
bsp_irq_restore(irq_state);
return ready;
@@ -313,8 +366,10 @@ static void gps_runtime_state_init(void)
gps_plic_repeat_count = 0;
gps_acq_schedule_pending = true;
gps_timer_maintenance_pending = true;
gps_nav_solve_period_pending = false;
gps_nav_epoch_assembly_reset(&gps_nav_epoch_assembly);
memset(&gps_nav_active_snapshot, 0, sizeof(gps_nav_active_snapshot));
memset(&gps_nav_completed_snapshot, 0, sizeof(gps_nav_completed_snapshot));
gps_nav_completed_snapshot_pending = false;
memset((void*)&gps_runtime_diagnostics, 0, sizeof(gps_runtime_diagnostics));
gps_runtime_diagnostics.last_connected_prn = GPS_PRN_INVALID;
gps_runtime_diagnostics.last_lost_prn = GPS_PRN_INVALID;
@@ -708,9 +763,8 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
event.rem_code_phase = trck_ip->REM_CODE_PHASE_REG;
event.corrected_abs_sample = gps_correct_abs_sample(event.abs_sample_raw, event.rem_code_phase);
event.nav_solve_timer = trck_ip->NAV_SOLVE_TIMER;
event.nav_solve_event_count = (status_reg & (1u << GPS_TRCK_NAV_SOLVE_FLAG_POS)) ? 1u : 0u;
event.local_timestamp_ticks = local_timestamp_ticks;
if(event.nav_solve_event_count != 0u)
if((status_reg & (1u << GPS_TRCK_NAV_SOLVE_FLAG_POS)) != 0u)
{
event.pps_capture_valid = gps_pps_read_capture(trck_ip_no,
&event.pps_counter,
@@ -741,6 +795,9 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
if(status_reg & (1u << GPS_TRCK_STOP_UNLOCK_POS))
{
shadow->drop_event = event;
shadow->drop_event.status &=
~((1u << GPS_TRCK_NAV_BITS_RCV_POS) |
(1u << GPS_TRCK_NAV_SOLVE_FLAG_POS));
shadow->drop_pending = true;
}
if(status_reg & (1u << GPS_TRCK_NAV_BITS_RCV_POS))
@@ -750,12 +807,17 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
shadow->nav_bits_overwrite_count++;
}
shadow->nav_bits_event = event;
shadow->nav_bits_event.status &=
~((1u << GPS_TRCK_STOP_UNLOCK_POS) |
(1u << GPS_TRCK_NAV_SOLVE_FLAG_POS));
shadow->nav_bits_pending = true;
}
if(status_reg & (1u << GPS_TRCK_NAV_SOLVE_FLAG_POS))
{
shadow->nav_solve_event = event;
shadow->nav_solve_event_count++;
shadow->nav_solve_event.status &=
~((1u << GPS_TRCK_STOP_UNLOCK_POS) |
(1u << GPS_TRCK_NAV_BITS_RCV_POS));
shadow->nav_solve_pending = true;
}
if((status_reg & ((1u << GPS_TRCK_STOP_UNLOCK_POS) |
@@ -763,6 +825,10 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
(1u << GPS_TRCK_NAV_SOLVE_FLAG_POS))) == 0u)
{
shadow->misc_event = event;
shadow->misc_event.status &=
~((1u << GPS_TRCK_STOP_UNLOCK_POS) |
(1u << GPS_TRCK_NAV_BITS_RCV_POS) |
(1u << GPS_TRCK_NAV_SOLVE_FLAG_POS));
shadow->misc_pending = true;
}
}
@@ -912,10 +978,17 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event)
unsigned char trck_ip_no = event->index;
unsigned int dropped_prn = event->prn;
if(gps_nav_epoch_assembly_remove_channel(
&gps_nav_epoch_assembly, trck_ip_no))
bool epoch_complete = gps_nav_epoch_assembly_remove_channel(
&gps_nav_epoch_assembly, trck_ip_no);
gps_nav_active_snapshot.channel_mask &= ~(1u << trck_ip_no);
if(epoch_complete)
{
gps_nav_solve_period_pending = true;
gps_nav_publish_active_snapshot(
gps_nav_epoch_assembly.received_mask, true);
}
else if(!gps_nav_epoch_assembly.active)
{
gps_nav_discard_active_snapshot(false);
}
if(gps_prn_valid(dropped_prn))
@@ -944,6 +1017,42 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event)
trck_ip_no, dropped_prn, event->status, event->control);
}
static bool gps_how_candidate_consistent(
int first_week,
double first_tow_sec,
uint32_t first_raw_sample,
double first_corrected_sample,
int second_week,
double second_tow_sec,
uint32_t second_raw_sample,
double second_corrected_sample)
{
return gps_time_anchors_consistent(
first_week, first_tow_sec,
first_raw_sample, first_corrected_sample,
second_week, second_tow_sec,
second_raw_sample, second_corrected_sample,
SAMPLES_PER_CODE * 1000.0,
GPS_HOW_CONTINUITY_MAX_GAP_SEC,
GPS_HOW_CONTINUITY_TOLERANCE_SEC);
}
static void gps_commit_how_anchor(volatile gps_ephem_ll_t* node,
int week,
double tow_sec,
const gps_isr_event_t* event,
bool week_valid)
{
node->gps_how_tow.week = week;
node->gps_how_tow.sec = tow_sec;
node->gps_how_abs_sample = event->abs_sample_raw;
node->gps_how_corrected_abs_sample = event->corrected_abs_sample;
node->gps_how_tick = event->tick;
node->gps_how_anchor_valid = true;
node->gps_how_week_valid = week_valid;
node->gps_pending_how_valid = false;
}
static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
{
unsigned char trck_ip_no = event->index;
@@ -980,39 +1089,89 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
unsigned char subframe_id = parse_subframe((unsigned int*)gps_subframe,
(gps_frame_t*)gps_frame_parsed,
&how_tow_sec);
if((subframe_id >= 1u) && (subframe_id <= 5u))
if((subframe_id >= 1u) && (subframe_id <= 5u) &&
(how_tow_sec < (unsigned int)GPS_TIME_WEEK_SECONDS))
{
node->gps_subframe_mask |= (1u << subframe_id);
int fallback_week = gps_frame_parsed->gps_ephems.tow.week;
if(subframe_id == 1u)
{
/* WN is ten bits in LNAV. Keeping it in the same modulo domain as
* the ephemeris is sufficient for satellite-time differences. */
fallback_week = (int)((gps_subframe[2] >> 20) & 0x3ffu);
fallback_week = gps_time_extend_lnav_week(
node->gps_how_anchor_valid,
node->gps_how_tow.week,
(gps_subframe[2] >> 20) & 0x3ffu);
}
/* A subframe-1 WN is authoritative in its native 10-bit domain. On
* other subframes, resolve the HOW seconds rollover from the previous
* anchor. This also upgrades a pre-SF1 temporary week immediately. */
/* Subframe 1 supplies the modulo-1024 week. Other subframes resolve
* HOW rollover from the previous monotonic anchor. */
int anchor_week = (subframe_id == 1u) ? fallback_week :
gps_time_resolve_how_week(node->gps_how_anchor_valid,
node->gps_how_tow.week,
node->gps_how_tow.sec,
fallback_week,
(double)how_tow_sec);
node->gps_how_tow.week = anchor_week;
node->gps_how_tow.sec = (double)how_tow_sec;
node->gps_how_abs_sample = event->abs_sample_raw;
node->gps_how_corrected_abs_sample = event->corrected_abs_sample;
node->gps_how_tick = event->tick;
node->gps_how_anchor_valid = true;
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
"HOW anchor: trk=%u prn=%u week=%d tow=%.3f abs=%u corrected=%.6f\r\n",
trck_ip_no, event->prn, anchor_week, (double)how_tow_sec,
event->abs_sample_raw, event->corrected_abs_sample);
/* Before the first SF1, fallback_week is only a placeholder. Validate
* TOW/sample continuity in that temporary domain, then atomically
* upgrade to the parity-checked LNAV week carried by SF1. */
int continuity_week =
((subframe_id == 1u) && !node->gps_how_week_valid &&
node->gps_how_anchor_valid) ?
node->gps_how_tow.week : anchor_week;
bool accept_anchor = !node->gps_how_anchor_valid ||
gps_how_candidate_consistent(
node->gps_how_tow.week, node->gps_how_tow.sec,
node->gps_how_abs_sample,
node->gps_how_corrected_abs_sample,
continuity_week, (double)how_tow_sec,
event->abs_sample_raw, event->corrected_abs_sample);
bool recovered_anchor = false;
if(!accept_anchor && node->gps_pending_how_valid)
{
accept_anchor = gps_how_candidate_consistent(
node->gps_pending_how_tow.week,
node->gps_pending_how_tow.sec,
node->gps_pending_how_abs_sample,
node->gps_pending_how_corrected_abs_sample,
anchor_week, (double)how_tow_sec,
event->abs_sample_raw, event->corrected_abs_sample);
recovered_anchor = accept_anchor;
}
if(accept_anchor)
{
gps_commit_how_anchor(node, anchor_week,
(double)how_tow_sec, event,
node->gps_how_week_valid ||
(subframe_id == 1u));
GPS_LOG(recovered_anchor ? DEBUG_WARNING : DEBUG_TRACE,
GPS_LOG_CAT_NAV,
"%s HOW anchor: trk=%u prn=%u week=%d tow=%.3f abs=%u corrected=%.6f\r\n",
recovered_anchor ? "recovered" : "accepted",
trck_ip_no, event->prn, anchor_week,
(double)how_tow_sec, event->abs_sample_raw,
event->corrected_abs_sample);
}
else
{
node->gps_pending_how_tow.week = anchor_week;
node->gps_pending_how_tow.sec = (double)how_tow_sec;
node->gps_pending_how_abs_sample = event->abs_sample_raw;
node->gps_pending_how_corrected_abs_sample =
event->corrected_abs_sample;
node->gps_pending_how_valid = true;
gps_runtime_diagnostics.how_anchor_rejected_total++;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"HOW discontinuity rejected: trk=%u prn=%u current=%d/%.3f candidate=%d/%.3f rejects=%u\r\n",
trck_ip_no, event->prn,
node->gps_how_tow.week, node->gps_how_tow.sec,
anchor_week, (double)how_tow_sec,
gps_runtime_diagnostics.how_anchor_rejected_total);
}
}
else
{
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_TRK, "invalid subframe id: trk=%u subframe=%u\r\n", trck_ip_no, subframe_id);
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_TRK,
"invalid subframe/HOW: trk=%u subframe=%u tow=%u\r\n",
trck_ip_no, subframe_id, how_tow_sec);
}
gps_mark_node_fresh(node, event->status, event->tick);
@@ -1032,32 +1191,58 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
volatile gps_ephem_ll_t* node = get_element_by_ip_number(&tracking_node, trck_ip_no);
if(node != NULL)
{
unsigned int nav_solve_event_count = event->nav_solve_event_count;
unsigned int captured_epoch_count = 1u;
if(event->pps_capture_valid)
{
uint32_t previous_capture_sequence = node->gps_last_pps_capture_sequence;
if(previous_capture_sequence != 0u)
{
nav_solve_event_count = event->pps_capture_sequence - previous_capture_sequence;
if(nav_solve_event_count == 0u)
uint32_t sequence_delta =
event->pps_capture_sequence - previous_capture_sequence;
if(sequence_delta == 0u)
{
return;
}
if(nav_solve_event_count > event->nav_solve_event_count)
bool sequence_consistent =
event->pps_counter > node->gps_last_nav_solve_pps_counter;
if(sequence_consistent)
{
gps_runtime_diagnostics.nav_epoch_coalesced_total +=
nav_solve_event_count - event->nav_solve_event_count;
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);
}
if(sequence_consistent)
{
captured_epoch_count = sequence_delta;
if(sequence_delta > 1u)
{
gps_runtime_diagnostics.nav_epoch_coalesced_total +=
sequence_delta - 1u;
}
}
else
{
gps_runtime_diagnostics.pps_sequence_discontinuity_total++;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"recovered %u coalesced nav-solve epochs: trk=%u prn=%u\r\n",
nav_solve_event_count, trck_ip_no, event->prn);
"PPS capture sequence discontinuity: trk=%u prn=%u previous=%u current=%u total=%u\r\n",
trck_ip_no, event->prn,
previous_capture_sequence,
event->pps_capture_sequence,
gps_runtime_diagnostics.pps_sequence_discontinuity_total);
}
}
}
else if(node->gps_last_pps_capture_sequence != 0u)
{
/* Keep the sequence baseline moving on the rare APB capture race. A
* later valid capture still recovers any epochs coalesced meanwhile. */
node->gps_last_pps_capture_sequence += nav_solve_event_count;
/* Capture sequencing is transport metadata. Rebase it even if the
* decoded HOW below is unavailable or rejected. */
node->gps_last_nav_solve_pps_counter = event->pps_counter;
node->gps_last_pps_capture_sequence =
event->pps_capture_sequence;
}
if(!node->gps_how_anchor_valid)
@@ -1091,56 +1276,94 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
gps_frame_parsed->gps_ephems.tow.week = tow_week;
gps_frame_parsed->gps_ephems.tow.sec = tow_sec;
gps_frame_parsed->gps_ephems.abs_sample = event->corrected_abs_sample;
node->gps_nav_solve_sequence += nav_solve_event_count;
/* A one-entry hardware mailbox carries only the newest measurement.
* Missed capture pulses are diagnostic counts, not synthetic samples. */
node->gps_nav_solve_sequence++;
node->gps_last_nav_solve_tow.week = tow_week;
node->gps_last_nav_solve_tow.sec = tow_sec;
node->gps_last_nav_solve_abs_sample = event->abs_sample_raw;
node->gps_last_nav_solve_corrected_abs_sample = event->corrected_abs_sample;
node->gps_last_nav_solve_local_ticks = event->local_timestamp_ticks;
if(event->pps_capture_valid)
{
node->gps_last_nav_solve_pps_counter = event->pps_counter;
node->gps_last_pps_capture_sequence = event->pps_capture_sequence;
}
gps_mark_node_fresh(node, event->status, event->tick);
uint64_t epoch_id;
if(!gps_nav_epoch_id(tow_week, tow_sec,
GPS_NAV_SOLVE_PERIOD_MS, &epoch_id))
if(!event->pps_capture_valid)
{
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"nav solve has invalid epoch id: trk=%u week=%d tow=%.6f\r\n",
trck_ip_no, tow_week, tow_sec);
"nav solve omitted after incoherent PPS capture read: trk=%u prn=%u\r\n",
trck_ip_no, event->prn);
return;
}
uint64_t previous_epoch = gps_nav_epoch_assembly.epoch_id;
uint32_t previous_expected = gps_nav_epoch_assembly.expected_mask;
uint32_t previous_received = gps_nav_epoch_assembly.received_mask;
gps_nav_epoch_event_result_t assembly_result =
gps_nav_epoch_assembly_push(
&gps_nav_epoch_assembly, epoch_id, trck_ip_no,
gps_nav_ready_channel_mask());
&gps_nav_epoch_assembly,
event->pps_counter,
event->local_timestamp_ticks,
trck_ip_no,
gps_nav_ready_channel_mask(),
GPS_NAV_CAPTURE_PERIOD_TICKS,
GPS_NAV_CAPTURE_WINDOW_TICKS,
GPS_NAV_HOLDOFF_LOCAL_TICKS);
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);
}
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,
(unsigned long)event->pps_counter, trck_ip_no);
}
if((assembly_result == GPS_NAV_EPOCH_EVENT_ACCEPTED) ||
(assembly_result == GPS_NAV_EPOCH_EVENT_RESTARTED) ||
(assembly_result == GPS_NAV_EPOCH_EVENT_COMPLETE))
{
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.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] =
event->abs_sample_raw;
gps_nav_active_snapshot.corrected_abs_sample[trck_ip_no] =
event->corrected_abs_sample;
gps_nav_active_snapshot.pps_counter[trck_ip_no] =
event->pps_counter;
gps_nav_active_snapshot.pps_capture_sequence[trck_ip_no] =
event->pps_capture_sequence;
gps_nav_active_snapshot.local_timestamp_ticks[trck_ip_no] =
event->local_timestamp_ticks;
}
if(assembly_result == GPS_NAV_EPOCH_EVENT_COMPLETE)
{
gps_nav_solve_period_pending = true;
gps_nav_publish_active_snapshot(
gps_nav_epoch_assembly.received_mask, true);
}
else if(assembly_result == GPS_NAV_EPOCH_EVENT_STALE)
{
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"stale nav epoch interrupt ignored: trk=%u epoch=%lu active=%lu\r\n",
trck_ip_no, (unsigned long)epoch_id,
(unsigned long)gps_nav_epoch_assembly.epoch_id);
"stale NAV capture ignored: trk=%u counter=%lu active_min=%lu\r\n",
trck_ip_no, (unsigned long)event->pps_counter,
(unsigned long)gps_nav_epoch_assembly.minimum_capture_counter);
}
else if(assembly_result == GPS_NAV_EPOCH_EVENT_RESTARTED)
else if(assembly_result == GPS_NAV_EPOCH_EVENT_OUTLIER)
{
/* The previous snapshot is no longer the active assembly. Do not
* let an already-pending foreground hand-off consume mixed nodes. */
gps_nav_solve_period_pending = false;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"NAV epoch restarted by newer interrupt: old=%lu expected=0x%x received=0x%x new=%lu first_trk=%u\r\n",
(unsigned long)previous_epoch,
previous_expected, previous_received,
(unsigned long)epoch_id, trck_ip_no);
"NAV capture outlier ignored: trk=%u counter=%lu active=[%lu,%lu]\r\n",
trck_ip_no, (unsigned long)event->pps_counter,
(unsigned long)gps_nav_epoch_assembly.minimum_capture_counter,
(unsigned long)gps_nav_epoch_assembly.maximum_capture_counter);
}
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_TRK,
"nav solve: trk=%u abs_raw=%u rem=%u abs=%.6f timer=%u count=%u dt=%.6f tow=%f seq=%u\r\n",
@@ -1149,7 +1372,7 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
event->rem_code_phase,
gps_frame_parsed->gps_ephems.abs_sample,
event->nav_solve_timer,
nav_solve_event_count,
captured_epoch_count,
tow_delta_sec,
gps_frame_parsed->gps_ephems.tow.sec,
node->gps_nav_solve_sequence);
@@ -1243,7 +1466,6 @@ static void gps_process_shadow_tracking_events(void)
}
if(shadow.nav_solve_pending)
{
shadow.nav_solve_event.nav_solve_event_count = shadow.nav_solve_event_count;
gps_process_tracking_event(&shadow.nav_solve_event);
}
if(shadow.misc_pending)
soc/software/gps_system/src/gps/gps_ip.c
+86 -132
View File
@@ -570,7 +570,6 @@ static void gps_enable_nav_solve_for_snapshot_locked(const unsigned char snapsho
if(!gps_channel_nav_solve_enabled(ip_no))
{
gps_set_channel_nav_solve_mode(ip_no);
node->gps_nav_solve_consumed_sequence = node->gps_nav_solve_sequence;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK,
"tracking channel %u enabled nav solve; nav bits remain enabled\r\n",
ip_no);
@@ -599,9 +598,7 @@ static unsigned int gps_collect_tracked_channels_locked(
static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT],
bool require_nav_solve_mode,
bool require_nav_sequence)
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT])
{
unsigned int count = 0;
@@ -611,12 +608,7 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
{
unsigned char ip_no = iterator->tracking_ip_no;
gps_ephemeris_t* eph = (gps_ephemeris_t*)iterator->data_p;
if(gps_node_ready_for_position(iterator) &&
(!require_nav_solve_mode || gps_channel_nav_solve_enabled(ip_no)) &&
(!require_nav_sequence ||
((eph != NULL) &&
(iterator->gps_nav_solve_sequence !=
iterator->gps_nav_solve_consumed_sequence))))
if(gps_node_ready_for_position(iterator) && (eph != NULL))
{
if(gps_snapshot_has_svid(snapshot, count, eph->SVID))
{
@@ -626,19 +618,10 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
continue;
}
snapshot[count] = *eph;
if(require_nav_sequence)
if(snapshot_raw_samples != NULL)
{
/* Nav bits may update the shared ephemeris between nav-solve
* epochs. Position measurements must use the timing latched by
* the nav-solve event, not the shared nav-bit fields. */
snapshot[count].tow = iterator->gps_last_nav_solve_tow;
snapshot[count].abs_sample =
iterator->gps_last_nav_solve_corrected_abs_sample;
if(snapshot_raw_samples != NULL)
{
snapshot_raw_samples[count] =
iterator->gps_last_nav_solve_abs_sample;
}
snapshot_raw_samples[count] =
iterator->gps_last_nav_solve_abs_sample;
}
snapshot_ips[count] = ip_no;
count++;
@@ -651,21 +634,70 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
return count;
}
static unsigned int gps_collect_completed_epoch_locked(
const gps_nav_epoch_snapshot_t* epoch,
gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
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])
{
if(epoch == NULL)
{
return 0u;
}
unsigned int count = 0u;
for(unsigned char ip_no = 0u ; ip_no < GPS_TRCK_IP_COUNT ; ip_no++)
{
if((epoch->channel_mask & (1u << ip_no)) == 0u)
{
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]) ||
gps_snapshot_has_svid(snapshot, count, eph->SVID))
{
continue;
}
snapshot[count] = *eph;
snapshot[count].tow = epoch->tow[ip_no];
snapshot[count].abs_sample =
epoch->corrected_abs_sample[ip_no];
snapshot_ips[count] = ip_no;
snapshot_pps_counters[count] =
epoch->pps_counter[ip_no];
snapshot_pps_sequences[count] =
epoch->pps_capture_sequence[ip_no];
snapshot_local_timestamps[count] =
epoch->local_timestamp_ticks[ip_no];
snapshot_raw_samples[count] =
epoch->raw_sample[ip_no];
count++;
}
return count;
}
static unsigned int gps_select_coherent_nav_snapshot(
gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
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],
unsigned int candidate_count,
uint32_t* selected_mask_out)
unsigned int candidate_count)
{
if((candidate_count == 0u) ||
(candidate_count > GPS_NAV_EPOCH_MAX_CANDIDATES))
{
*selected_mask_out = 0u;
return 0u;
}
@@ -683,7 +715,6 @@ static unsigned int gps_select_coherent_nav_snapshot(
GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC,
&selection))
{
*selected_mask_out = 0u;
return 0u;
}
@@ -702,7 +733,6 @@ static unsigned int gps_select_coherent_nav_snapshot(
{
snapshot[destination] = snapshot[source];
snapshot_ips[destination] = snapshot_ips[source];
snapshot_sequences[destination] = snapshot_sequences[source];
snapshot_pps_counters[destination] =
snapshot_pps_counters[source];
snapshot_pps_sequences[destination] =
@@ -720,28 +750,9 @@ static unsigned int gps_select_coherent_nav_snapshot(
destination++;
}
*selected_mask_out = selection.selected_mask;
return destination;
}
static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
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],
unsigned int satellite_count)
{
for(unsigned int i = 0 ; i < satellite_count ; i++)
{
volatile gps_ephem_ll_t* node = get_element_by_ip_number(&tracking_node, snapshot_ips[i]);
snapshot_sequences[i] = (node != NULL) ? node->gps_nav_solve_sequence : 0u;
snapshot_pps_counters[i] = (node != NULL) ? node->gps_last_nav_solve_pps_counter : 0u;
snapshot_pps_sequences[i] = (node != NULL) ? node->gps_last_pps_capture_sequence : 0u;
snapshot_local_timestamps[i] =
(node != NULL) ? node->gps_last_nav_solve_local_ticks : 0u;
}
}
static uint64_t gps_nav_median_local_timestamp(
const uint64_t timestamps[GPS_TRCK_IP_COUNT], unsigned int count)
{
@@ -765,48 +776,6 @@ static uint64_t gps_nav_median_local_timestamp(
return (valid_count != 0u) ? sorted[valid_count / 2u] : 0u;
}
static void gps_consume_nav_sequences_locked(const unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
const uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
unsigned int satellite_count)
{
for(unsigned int i = 0 ; i < satellite_count ; i++)
{
volatile gps_ephem_ll_t* node = get_element_by_ip_number(&tracking_node, snapshot_ips[i]);
if(node != NULL)
{
/* Consume only the epoch copied into this solution. A newer sequence,
* if present, remains different from consumed and is solved next. */
node->gps_nav_solve_consumed_sequence = snapshot_sequences[i];
}
}
}
static void gps_consume_rejected_nav_sequences_locked(
const unsigned char candidate_ips[GPS_TRCK_IP_COUNT],
const uint32_t candidate_sequences[GPS_TRCK_IP_COUNT],
unsigned int candidate_count,
uint32_t selected_mask)
{
for(unsigned int i = 0u ; i < candidate_count ; i++)
{
if((selected_mask & (1u << i)) != 0u)
{
continue;
}
volatile gps_ephem_ll_t* node =
get_element_by_ip_number(&tracking_node, candidate_ips[i]);
if(node != NULL)
{
/*
* Consume exactly the rejected snapshot. If an interrupt has
* already produced a newer sequence, it remains different from
* this consumed value and is preserved for the next epoch.
*/
node->gps_nav_solve_consumed_sequence = candidate_sequences[i];
}
}
}
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],
@@ -1088,14 +1057,13 @@ bool calculate_position(gps_nav_solution_t* solution_out,
bool result_ready = false;
gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT];
unsigned char snapshot_ips[GPS_TRCK_IP_COUNT];
uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT];
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];
gps_nav_epoch_snapshot_t completed_epoch;
unsigned int satellite_count = 0;
unsigned int candidate_count = 0;
uint32_t selected_candidate_mask = 0u;
bool run_nav = false;
bool initial_run = false;
unsigned int ready_count;
@@ -1110,13 +1078,14 @@ bool calculate_position(gps_nav_solution_t* solution_out,
#ifdef EKF_ENABLE
(void)g_ekf;
#endif
bool nav_period_ready = gps_take_nav_solve_period_ready();
bool nav_period_ready =
gps_take_nav_epoch_snapshot(&completed_epoch);
uint64_t nav_period_start_cycles = nav_period_ready ? gps_read_cycle_counter() : 0u;
uint64_t irq_state = bsp_irq_save();
ready_count = gps_collect_nav_snapshot_locked(
snapshot, snapshot_ips, NULL, false, false);
snapshot, snapshot_ips, NULL);
tracked_count = gps_collect_tracked_channels_locked(
GPS_DEBUG_ENABLED(DEBUG_CHANNELS) ? tracked_prns : NULL);
@@ -1131,13 +1100,11 @@ bool calculate_position(gps_nav_solution_t* solution_out,
gps_enable_nav_solve_for_snapshot_locked(snapshot_ips, ready_count);
if(nav_period_ready)
{
candidate_count = gps_collect_nav_snapshot_locked(
snapshot, snapshot_ips, snapshot_raw_samples, true, true);
gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences,
snapshot_pps_counters,
snapshot_pps_sequences,
snapshot_local_timestamps,
candidate_count);
candidate_count = gps_collect_completed_epoch_locked(
&completed_epoch,
snapshot, snapshot_ips, snapshot_pps_counters,
snapshot_pps_sequences,
snapshot_local_timestamps, snapshot_raw_samples);
initial_run = !gps_initial_position_valid;
}
}
@@ -1146,22 +1113,11 @@ bool calculate_position(gps_nav_solution_t* solution_out,
if(nav_period_ready && (candidate_count != 0u))
{
/*
* Preserve the pre-compaction identity of rejected candidates so their
* exact sequence can be retired without touching a newer interrupt.
*/
unsigned char candidate_ips[GPS_TRCK_IP_COUNT];
uint32_t candidate_sequences[GPS_TRCK_IP_COUNT];
memcpy(candidate_ips, snapshot_ips,
candidate_count * sizeof(candidate_ips[0]));
memcpy(candidate_sequences, snapshot_sequences,
candidate_count * sizeof(candidate_sequences[0]));
satellite_count = gps_select_coherent_nav_snapshot(
snapshot, snapshot_ips, snapshot_sequences,
snapshot_pps_counters, snapshot_pps_sequences,
snapshot, snapshot_ips, snapshot_pps_counters,
snapshot_pps_sequences,
snapshot_local_timestamps, snapshot_raw_samples,
candidate_count, &selected_candidate_mask);
candidate_count);
coherent_nav_count = satellite_count;
run_nav = satellite_count >= MIN_NEEDED_SATELLITE;
@@ -1169,11 +1125,6 @@ bool calculate_position(gps_nav_solution_t* solution_out,
{
unsigned int excluded_count = candidate_count - satellite_count;
gps_nav_epoch_excluded_total += excluded_count;
uint64_t consume_irq_state = bsp_irq_save();
gps_consume_rejected_nav_sequences_locked(
candidate_ips, candidate_sequences, candidate_count,
selected_candidate_mask);
bsp_irq_restore(consume_irq_state);
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,
@@ -1181,10 +1132,6 @@ bool calculate_position(gps_nav_solution_t* solution_out,
}
else if(!run_nav)
{
uint64_t consume_irq_state = bsp_irq_save();
gps_consume_rejected_nav_sequences_locked(
candidate_ips, candidate_sequences, candidate_count, 0u);
bsp_irq_restore(consume_irq_state);
if(candidate_count >= MIN_NEEDED_SATELLITE)
{
gps_nav_epoch_reject_total++;
@@ -1202,8 +1149,15 @@ bool calculate_position(gps_nav_solution_t* solution_out,
(coherent_nav_count != last_nav_status_coherent_count) ||
((now - last_nav_status_log_tick) >= GPS_NAV_STATUS_LOG_PERIOD_TICKS))
{
GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV, "tracked channels=%u ready channels=%u coherent_nav=%u\r\n",
tracked_count, ready_count, coherent_nav_count);
gps_runtime_diagnostics_t status_diagnostics;
gps_get_runtime_diagnostics(&status_diagnostics);
GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV,
"tracked channels=%u ready channels=%u coherent_nav=%u incomplete=%u snapshot_overwrite=%u pps_discontinuity=%u how_reject=%u\r\n",
tracked_count, ready_count, coherent_nav_count,
status_diagnostics.nav_epoch_incomplete_total,
status_diagnostics.nav_epoch_snapshot_overwrite_total,
status_diagnostics.pps_sequence_discontinuity_total,
status_diagnostics.how_anchor_rejected_total);
last_nav_status_tracked_count = tracked_count;
last_nav_status_ready_count = ready_count;
last_nav_status_coherent_count = coherent_nav_count;
@@ -1241,13 +1195,8 @@ bool calculate_position(gps_nav_solution_t* solution_out,
result_ready = true;
}
}
/* A completed hardware epoch is attempted once. Retrying the same
* measurements after a WLS failure would couple solution cadence to an
* unrelated foreground wake-up. */
uint64_t consume_irq_state = bsp_irq_save();
gps_consume_nav_sequences_locked(
snapshot_ips, snapshot_sequences, satellite_count);
bsp_irq_restore(consume_irq_state);
/* The completed snapshot is handed out exactly once. Retrying after a
* WLS failure would couple solution cadence to a foreground wake-up. */
uint64_t foreground_cycles = gps_read_cycle_counter() - nav_period_start_cycles;
uint64_t period_budget_cycles = (GPS_PROFILE_CPU_HZ * GPS_NAV_SOLVE_PERIOD_MS) / 1000u;
bool period_overrun = foreground_cycles >= period_budget_cycles;
@@ -1260,6 +1209,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
"fde=%u valid=%u budget_cycles=%lu overrun=%u math_corr=%.6lf "
"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",
GPS_NAV_SOLVE_PERIOD_MS,
(unsigned long)GPS_PROFILE_CPU_HZ,
@@ -1283,6 +1233,10 @@ bool calculate_position(gps_nav_solution_t* solution_out,
runtime_diagnostics.plic_drain_limit_total,
runtime_diagnostics.nav_epoch_coalesced_total,
runtime_diagnostics.satellite_lost_total,
runtime_diagnostics.pps_sequence_discontinuity_total,
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;
soc/software/gps_system/src/gps/gps_nav_epoch.c
+73 -40
View File
@@ -5,7 +5,6 @@
#define GPS_WEEK_SECONDS 604800.0
#define GPS_HALF_WEEK_SECONDS 302400.0
#define GPS_WEEK_MILLISECONDS 604800000ULL
static double gps_nav_epoch_tow_delta(
const gps_nav_epoch_key_t* first,
@@ -180,40 +179,16 @@ bool gps_nav_epoch_select(
return selection->selected_count != 0u;
}
bool gps_nav_epoch_id(
int week,
double tow_sec,
unsigned int period_ms,
uint64_t* epoch_id)
{
if((epoch_id == NULL) || (week < 0) || (period_ms == 0u) ||
((GPS_WEEK_MILLISECONDS % period_ms) != 0u) ||
!isfinite(tow_sec) || (tow_sec < 0.0) ||
(tow_sec >= GPS_WEEK_SECONDS))
{
return false;
}
uint64_t epochs_per_week = GPS_WEEK_MILLISECONDS / period_ms;
double epoch_in_week = (tow_sec * 1000.0) / (double)period_ms;
uint64_t rounded_epoch = (uint64_t)llround(epoch_in_week);
if(rounded_epoch == epochs_per_week)
{
week++;
rounded_epoch = 0u;
}
*epoch_id = ((uint64_t)(unsigned int)week * epochs_per_week) +
rounded_epoch;
return true;
}
void gps_nav_epoch_assembly_reset(gps_nav_epoch_assembly_t* assembly)
{
if(assembly == NULL)
{
return;
}
assembly->epoch_id = 0u;
assembly->first_capture_counter = 0u;
assembly->minimum_capture_counter = 0u;
assembly->maximum_capture_counter = 0u;
assembly->deadline_local_ticks = 0u;
assembly->expected_mask = 0u;
assembly->received_mask = 0u;
assembly->active = false;
@@ -230,12 +205,21 @@ bool gps_nav_epoch_assembly_complete(
gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
gps_nav_epoch_assembly_t* assembly,
uint64_t epoch_id,
uint64_t capture_counter,
uint64_t local_timestamp_ticks,
unsigned int channel,
uint32_t ready_mask)
uint32_t ready_mask,
uint64_t capture_period_ticks,
uint64_t sync_window_ticks,
uint64_t holdoff_local_ticks)
{
if((assembly == NULL) ||
(channel >= GPS_NAV_EPOCH_MAX_CANDIDATES))
(channel >= GPS_NAV_EPOCH_MAX_CANDIDATES) ||
(capture_counter == 0u) ||
(capture_period_ticks == 0u) ||
(sync_window_ticks == 0u) ||
(sync_window_ticks >= (capture_period_ticks / 2u)) ||
(holdoff_local_ticks == 0u))
{
return GPS_NAV_EPOCH_EVENT_INVALID;
}
@@ -252,20 +236,40 @@ gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
bool restarted = false;
if(assembly->active)
{
if(epoch_id < assembly->epoch_id)
uint64_t minimum = assembly->minimum_capture_counter;
uint64_t maximum = assembly->maximum_capture_counter;
uint64_t next_minimum =
(capture_counter < minimum) ? capture_counter : minimum;
uint64_t next_maximum =
(capture_counter > maximum) ? capture_counter : maximum;
if((next_maximum - next_minimum) > sync_window_ticks)
{
return GPS_NAV_EPOCH_EVENT_STALE;
}
if(epoch_id > assembly->epoch_id)
{
gps_nav_epoch_assembly_reset(assembly);
restarted = true;
uint64_t epoch_separation = capture_period_ticks / 2u;
if((capture_counter > maximum) &&
((capture_counter - maximum) >= epoch_separation))
{
gps_nav_epoch_assembly_reset(assembly);
restarted = true;
}
else if((capture_counter < minimum) &&
((minimum - capture_counter) >= epoch_separation))
{
return GPS_NAV_EPOCH_EVENT_STALE;
}
else
{
return GPS_NAV_EPOCH_EVENT_OUTLIER;
}
}
}
if(!assembly->active)
{
assembly->epoch_id = epoch_id;
assembly->first_capture_counter = capture_counter;
assembly->minimum_capture_counter = capture_counter;
assembly->maximum_capture_counter = capture_counter;
assembly->deadline_local_ticks =
local_timestamp_ticks + holdoff_local_ticks;
assembly->expected_mask = ready_mask;
assembly->received_mask = 0u;
assembly->active = true;
@@ -280,6 +284,14 @@ gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
return GPS_NAV_EPOCH_EVENT_DUPLICATE;
}
if(capture_counter < assembly->minimum_capture_counter)
{
assembly->minimum_capture_counter = capture_counter;
}
if(capture_counter > assembly->maximum_capture_counter)
{
assembly->maximum_capture_counter = capture_counter;
}
assembly->received_mask |= channel_mask;
if(gps_nav_epoch_assembly_complete(assembly))
{
@@ -310,3 +322,24 @@ bool gps_nav_epoch_assembly_remove_channel(
}
return gps_nav_epoch_assembly_complete(assembly);
}
bool gps_nav_epoch_assembly_usable(
const gps_nav_epoch_assembly_t* assembly,
unsigned int minimum_channels)
{
if((assembly == NULL) || !assembly->active ||
(minimum_channels == 0u))
{
return false;
}
return gps_nav_epoch_mask_count(assembly->received_mask) >=
minimum_channels;
}
bool gps_nav_epoch_assembly_expired(
const gps_nav_epoch_assembly_t* assembly,
uint64_t now_local_ticks)
{
return (assembly != NULL) && assembly->active &&
(now_local_ticks >= assembly->deadline_local_ticks);
}
soc/software/gps_system/src/gps/gps_time_anchor.c
+69
View File
@@ -42,6 +42,28 @@ int gps_time_resolve_how_week(bool previous_valid,
return week;
}
int gps_time_extend_lnav_week(bool previous_valid,
int previous_week,
unsigned int lnav_week)
{
int week = (int)(lnav_week & 0x3ffu);
if(!previous_valid || (previous_week < 0))
{
return week;
}
int candidate = (previous_week & ~0x3ff) | week;
if((candidate - previous_week) > 512)
{
candidate -= 1024;
}
else if((previous_week - candidate) > 512)
{
candidate += 1024;
}
return candidate;
}
bool gps_time_from_sample_anchor(int anchor_week,
double anchor_tow_sec,
uint32_t anchor_raw_sample,
@@ -86,3 +108,50 @@ bool gps_time_from_sample_anchor(int anchor_week,
}
return true;
}
bool gps_time_anchors_consistent(int first_week,
double first_tow_sec,
uint32_t first_raw_sample,
double first_corrected_sample,
int second_week,
double second_tow_sec,
uint32_t second_raw_sample,
double second_corrected_sample,
double samples_per_second,
double max_gap_sec,
double tolerance_sec)
{
if((first_week < 0) || (second_week < 0) ||
!isfinite(first_tow_sec) || !isfinite(second_tow_sec) ||
!isfinite(first_corrected_sample) ||
!isfinite(second_corrected_sample) ||
(first_tow_sec < 0.0) ||
(first_tow_sec >= GPS_TIME_WEEK_SECONDS) ||
(second_tow_sec < 0.0) ||
(second_tow_sec >= GPS_TIME_WEEK_SECONDS) ||
!(samples_per_second > 0.0) || !(max_gap_sec > 0.0) ||
!(tolerance_sec >= 0.0))
{
return false;
}
int32_t raw_delta = (int32_t)(second_raw_sample - first_raw_sample);
double first_fraction =
first_corrected_sample - (double)first_raw_sample;
double second_fraction =
second_corrected_sample - (double)second_raw_sample;
double sample_elapsed =
((double)raw_delta + second_fraction - first_fraction) /
samples_per_second;
if(!isfinite(sample_elapsed) || (sample_elapsed < -0.001) ||
(sample_elapsed > max_gap_sec))
{
return false;
}
double gps_elapsed =
((double)(second_week - first_week) * GPS_TIME_WEEK_SECONDS) +
second_tow_sec - first_tow_sec;
return isfinite(gps_elapsed) &&
(fabs(gps_elapsed - sample_elapsed) <= tolerance_sec);
}
soc/software/gps_system/src/gps/linked_list.c
+6 -1
View File
@@ -66,8 +66,13 @@ bool insert_element(volatile gps_ephem_ll_t** node,
new_node->gps_how_corrected_abs_sample = 0.0;
new_node->gps_how_tick = 0u;
new_node->gps_how_anchor_valid = false;
new_node->gps_how_week_valid = false;
new_node->gps_pending_how_tow.week = 0;
new_node->gps_pending_how_tow.sec = 0.0;
new_node->gps_pending_how_abs_sample = 0u;
new_node->gps_pending_how_corrected_abs_sample = 0.0;
new_node->gps_pending_how_valid = false;
new_node->gps_nav_solve_sequence = 0u;
new_node->gps_nav_solve_consumed_sequence = 0u;
new_node->gps_last_nav_solve_tow.week = 0;
new_node->gps_last_nav_solve_tow.sec = 0.0;
new_node->gps_last_nav_solve_abs_sample = 0u;