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 b65da46..ea0f13e 100644 --- a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c +++ b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c @@ -48,6 +48,7 @@ typedef struct { } gps_tracking_shadow_t; static volatile bool tracking_allocated[GPS_TRCK_IP_COUNT]; +static unsigned char tracking_not_busy_polls[GPS_TRCK_IP_COUNT]; static volatile gps_acq_engine_state_t acq_engine_state[GPS_ACQ_IP_COUNT]; static volatile unsigned char acq_engine_prn[GPS_ACQ_IP_COUNT]; static volatile uint64_t acq_engine_start_tick[GPS_ACQ_IP_COUNT]; @@ -81,6 +82,7 @@ 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_TRK_NOT_BUSY_CONFIRM_POLLS 2u #define GPS_NAV_CAPTURE_PERIOD_TICKS \ (((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_SOLVE_PERIOD_MS) / 1000u) #define GPS_NAV_CAPTURE_WINDOW_TICKS \ @@ -385,6 +387,7 @@ static void gps_runtime_state_init(void) for(unsigned int i = 0 ; i < GPS_TRCK_IP_COUNT ; i++) { tracking_allocated[i] = false; + tracking_not_busy_polls[i] = 0u; memset(&trk_shadow[i], 0, sizeof(trk_shadow[i])); gps_clear_tracking_slot((unsigned char)i); } @@ -682,6 +685,7 @@ bool assign_tracking_from_result(const gps_acq_result_t* result) !trck_ip_exists(&tracking_node, i)) { tracking_allocated[i] = true; + tracking_not_busy_polls[i] = 0u; gps_clear_tracking_slot(i); gps_ephemeris_t cached_ephemeris; if(gps_ephemeris_cache_restore( @@ -1030,6 +1034,7 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event) } tracking_allocated[trck_ip_no] = false; + tracking_not_busy_polls[trck_ip_no] = 0u; if(trck_ip_exists(&tracking_node, trck_ip_no) && !delete_by_ip_number(&tracking_node, trck_ip_no)) { @@ -1731,6 +1736,73 @@ static void gps_mark_stale_measurements(void) } } +static unsigned int gps_prn_for_tracking_channel( + unsigned char trck_ip_no, + volatile gps_ephem_ll_t* node) +{ + if((node != NULL) && (node->data_p != NULL) && + gps_prn_valid(node->data_p->SVID)) + { + return node->data_p->SVID; + } + + unsigned int hardware_prn = trck_ips[trck_ip_no]->PRN_NUMBER; + if(gps_prn_valid(hardware_prn)) + { + return hardware_prn; + } + + for(unsigned int prn = 0u ; prn < GPS_PRN_COUNT ; prn++) + { + if(satellite_tracking_ip[prn] == trck_ip_no) + { + return prn; + } + } + return GPS_PRN_INVALID; +} + +static void gps_reconcile_stopped_tracking_channels(void) +{ + for(unsigned char i = 0u ; i < GPS_TRCK_IP_COUNT ; i++) + { + volatile gps_ephem_ll_t* node = + get_element_by_ip_number(&tracking_node, i); + bool allocated = tracking_allocated[i] || (node != NULL); + uint32_t status = trck_ips[i]->STATUS_REG; + if(!allocated || + ((status & (1u << GPS_TRCK_BUSY_POS)) != 0u)) + { + tracking_not_busy_polls[i] = 0u; + continue; + } + + if(tracking_not_busy_polls[i] < + GPS_TRK_NOT_BUSY_CONFIRM_POLLS) + { + tracking_not_busy_polls[i]++; + } + if(tracking_not_busy_polls[i] < + GPS_TRK_NOT_BUSY_CONFIRM_POLLS) + { + continue; + } + + gps_isr_event_t stopped_event; + memset(&stopped_event, 0, sizeof(stopped_event)); + stopped_event.type = GPS_ISR_EVENT_TRK_STATUS; + stopped_event.index = i; + stopped_event.status = status; + stopped_event.control = trck_ips[i]->CONTROL_REG; + stopped_event.prn = gps_prn_for_tracking_channel(i, node); + stopped_event.tick = gps_get_systick(); + GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_TRK, + "tracking stopped without loss IRQ: trk=%u prn=%u status=0x%x\r\n", + i, stopped_event.prn, status); + gps_process_tracking_drop(&stopped_event); + } +} + void gps_background_task(void) { gps_poll_acq_pending_completions(); @@ -1738,6 +1810,7 @@ void gps_background_task(void) gps_process_isr_events(); if(gps_take_timer_maintenance_pending()) { + gps_reconcile_stopped_tracking_channels(); gps_mark_stale_measurements(); gps_check_acq_stuck_busy(); gps_log_acq_status(); diff --git a/soc/software/gps_system/tests/test_gps_receiver_hardware_sim.c b/soc/software/gps_system/tests/test_gps_receiver_hardware_sim.c index d59fc94..56fd62b 100644 --- a/soc/software/gps_system/tests/test_gps_receiver_hardware_sim.c +++ b/soc/software/gps_system/tests/test_gps_receiver_hardware_sim.c @@ -15,13 +15,17 @@ typedef struct { gps_ephemeris_cache_t cache; bool visible[GPS_ACQ_MANAGER_PRN_COUNT]; bool tracked[GPS_ACQ_MANAGER_PRN_COUNT]; + bool hardware_busy[GPS_ACQ_MANAGER_PRN_COUNT]; uint64_t invisible_since[GPS_ACQ_MANAGER_PRN_COUNT]; + unsigned int not_busy_polls[GPS_ACQ_MANAGER_PRN_COUNT]; bool engine_busy; bool drop_completion_irq; + bool drop_loss_irq; unsigned int engine_prn; uint64_t engine_start; uint64_t tick; unsigned int polled_completions; + unsigned int polled_losses; unsigned int cache_restores; } receiver_sim_t; @@ -100,6 +104,8 @@ static void sim_complete_engine(receiver_sim_t* sim) gps_acq_manager_mark_acquired(&sim->manager, prn); gps_acq_manager_mark_tracking(&sim->manager, prn); sim->tracked[prn] = true; + sim->hardware_busy[prn] = true; + sim->not_busy_polls[prn] = 0u; sim->invisible_since[prn] = 0u; gps_ephemeris_t restored; if(gps_ephemeris_cache_restore( @@ -119,6 +125,14 @@ 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->not_busy_polls[prn] = 0u; + gps_acq_manager_mark_lost( + &sim->manager, prn, sim->tick); +} + static void sim_update_tracking(receiver_sim_t* sim) { for(unsigned int prn = 0u; @@ -140,9 +154,25 @@ static void sim_update_tracking(receiver_sim_t* sim) else if((sim->tick - sim->invisible_since[prn]) >= SIM_UNLOCK_TICKS) { - sim->tracked[prn] = false; - gps_acq_manager_mark_lost( - &sim->manager, prn, sim->tick); + sim->hardware_busy[prn] = false; + if(!sim->drop_loss_irq) + { + sim_mark_tracking_lost(sim, prn); + } + } + + if(!sim->tracked[prn]) + { + continue; + } + if(sim->hardware_busy[prn]) + { + sim->not_busy_polls[prn] = 0u; + } + else if(++sim->not_busy_polls[prn] >= 2u) + { + sim->polled_losses++; + sim_mark_tracking_lost(sim, prn); } } } @@ -307,6 +337,36 @@ static void test_missed_completion_interrupt_is_polled(void) GPS_ACQ_PRN_TRACKING); } +static void test_missed_tracking_loss_interrupt_is_reconciled(void) +{ + receiver_sim_t sim; + sim_init(&sim); + sim.visible[0] = true; + sim_start_engine(&sim); + sim_run(&sim, SIM_SEARCH_TICKS); + assert(sim.tracked[0]); + + sim.drop_loss_irq = true; + sim.visible[0] = false; + sim_run(&sim, SIM_UNLOCK_TICKS + 2u); + assert(!sim.tracked[0]); + assert(sim.polled_losses == 1u); + assert(gps_acq_manager_state(&sim.manager, 0u) == + GPS_ACQ_PRN_HOT); + + sim.visible[0] = true; + sim.drop_loss_irq = false; + unsigned int return_tick = (unsigned int)sim.tick; + while(!sim.tracked[0] && + ((sim.tick - return_tick) <= (2u * SIM_SEARCH_TICKS))) + { + sim_step(&sim); + } + assert(sim.tracked[0]); + assert((sim.tick - return_tick) <= (2u * SIM_SEARCH_TICKS)); + assert(sim.cache_restores == 1u); +} + int main(void) { test_cold_boot_and_short_signal_gap(); @@ -314,6 +374,7 @@ int main(void) test_long_outage_hot_reacquisition_and_cached_nav(); test_hot_failures_do_not_starve_cold_search(); test_missed_completion_interrupt_is_polled(); + test_missed_tracking_loss_interrupt_is_reconciled(); puts("gps receiver hardware-simulation scenarios passed"); return 0; }