fix(gps): make acquisition and reacquisition deterministic

This commit is contained in:
2026-07-24 20:13:33 +03:00
parent e57b19b4ee
commit 86dfbeafac
18 changed files with 1274 additions and 241 deletions
+4
View File
@@ -6,6 +6,7 @@ TARGET := gps_system
# UART image.
GPS_DEBUG ?= 1
GPS_DEBUG_LEVEL ?= 4
GPS_BUILD_ID ?= $(shell git describe --always --dirty --abbrev=12 2>/dev/null || printf unknown)
# The current Genesys2 Rocket/fabric domain that clocks the UART is 50 MHz.
# The 50 Hz fused-location stream plus GPS diagnostics does not fit safely in
@@ -65,6 +66,8 @@ APP_SRCS := \
src/adapters/gps_fusion_publisher.c \
src/gps/gps_ip.c \
src/gps/gps_interrupt_handlers.c \
src/gps/gps_acq_manager.c \
src/gps/gps_ephemeris_cache.c \
src/gps/gps_time_anchor.c \
src/gps/gps_nav_epoch.c \
src/gps/gps_pps.c \
@@ -80,6 +83,7 @@ include ../common/app.mk
CFLAGS += -Iheader
CFLAGS += $(IMU_BACKEND_CFLAG) \
-DGPS_BUILD_ID=\"$(GPS_BUILD_ID)\" \
-DIMU_ODR_HZ=$(IMU_ODR_HZ) \
-DIMU_ACCEL_FS_G=$(IMU_ACCEL_FS_G) \
-DIMU_GYRO_FS_DPS=$(IMU_GYRO_FS_DPS) \
@@ -0,0 +1,83 @@
#ifndef GPS_ACQ_MANAGER_H
#define GPS_ACQ_MANAGER_H
#include <stdbool.h>
#include <stdint.h>
#define GPS_ACQ_MANAGER_PRN_COUNT 32u
#define GPS_ACQ_MANAGER_INVALID_PRN 0xffu
typedef enum {
GPS_ACQ_PRN_DISABLED = 0,
GPS_ACQ_PRN_COLD,
GPS_ACQ_PRN_HOT,
GPS_ACQ_PRN_ACQUIRING,
GPS_ACQ_PRN_ACQUIRED,
GPS_ACQ_PRN_TRACKING,
GPS_ACQ_PRN_STALE
} gps_acq_prn_state_t;
typedef struct {
uint64_t retry_delay_ticks;
uint64_t hot_retry_delay_ticks;
uint64_t hot_window_ticks;
unsigned int max_hot_streak;
} gps_acq_manager_config_t;
typedef struct {
gps_acq_prn_state_t state;
uint64_t last_attempt_tick;
uint64_t hot_until_tick;
bool has_attempted;
bool attempt_was_hot;
} gps_acq_prn_entry_t;
typedef struct {
gps_acq_manager_config_t config;
gps_acq_prn_entry_t prn[GPS_ACQ_MANAGER_PRN_COUNT];
unsigned int cold_cursor;
unsigned int hot_cursor;
unsigned int hot_streak;
} gps_acq_manager_t;
void gps_acq_manager_init(
gps_acq_manager_t* manager,
const gps_acq_manager_config_t* config);
bool gps_acq_manager_enable_cold(
gps_acq_manager_t* manager,
unsigned int prn);
gps_acq_prn_state_t gps_acq_manager_state(
const gps_acq_manager_t* manager,
unsigned int prn);
const char* gps_acq_manager_state_name(gps_acq_prn_state_t state);
void gps_acq_manager_mark_acquired(
gps_acq_manager_t* manager,
unsigned int prn);
void gps_acq_manager_mark_tracking(
gps_acq_manager_t* manager,
unsigned int prn);
void gps_acq_manager_mark_stale(
gps_acq_manager_t* manager,
unsigned int prn);
void gps_acq_manager_mark_lost(
gps_acq_manager_t* manager,
unsigned int prn,
uint64_t now);
void gps_acq_manager_mark_failed(
gps_acq_manager_t* manager,
unsigned int prn,
uint64_t now);
bool gps_acq_manager_start_next(
gps_acq_manager_t* manager,
uint64_t now,
unsigned int* prn_out,
bool* hot_out);
unsigned int gps_acq_manager_count_state(
const gps_acq_manager_t* manager,
gps_acq_prn_state_t state);
unsigned int gps_acq_manager_count_searchable(
const gps_acq_manager_t* manager);
#endif
@@ -0,0 +1,37 @@
#ifndef GPS_EPHEMERIS_CACHE_H
#define GPS_EPHEMERIS_CACHE_H
#include <stdbool.h>
#include <stdint.h>
#include "gps/gps_ip_defines.h"
typedef struct {
gps_ephemeris_t ephemeris;
uint64_t committed_tick;
bool valid;
} gps_ephemeris_cache_entry_t;
typedef struct {
gps_ephemeris_cache_entry_t entry[GPS_PRN_COUNT];
uint64_t max_age_ticks;
} gps_ephemeris_cache_t;
void gps_ephemeris_cache_init(
gps_ephemeris_cache_t* cache,
uint64_t max_age_ticks);
bool gps_ephemeris_cache_store(
gps_ephemeris_cache_t* cache,
unsigned int prn,
const gps_ephemeris_t* ephemeris,
uint64_t now);
bool gps_ephemeris_cache_restore(
gps_ephemeris_cache_t* cache,
unsigned int prn,
uint64_t now,
gps_ephemeris_t* ephemeris_out);
void gps_ephemeris_cache_invalidate(
gps_ephemeris_cache_t* cache,
unsigned int prn);
#endif
@@ -14,6 +14,8 @@
uint32_t how_anchor_rejected_total;
uint32_t satellite_connected_total;
uint32_t satellite_lost_total;
uint32_t ephemeris_cache_store_total;
uint32_t ephemeris_cache_restore_total;
uint32_t last_connected_prn;
uint32_t last_lost_prn;
uint64_t last_connected_tick;
@@ -42,6 +44,9 @@
gps_external_irq_handler_t handler);
void gps_irq_handler(void) GPS_IRQ_HANDLER_ATTR;
void gps_process_isr_events(void);
void gps_acq_configure_prns(
const unsigned char* prns,
unsigned int count);
void gps_acq_scheduler_run(void);
void gps_background_task(void);
bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot);
@@ -9,7 +9,11 @@
#include "gps/matrixLib.h"
#include "gps/gps_macros.h"
#ifdef __riscv
#define GPS_IRQ_HANDLER_ATTR __attribute__((interrupt("machine"), aligned(4)))
#else
#define GPS_IRQ_HANDLER_ATTR
#endif
@@ -58,7 +62,14 @@
#define GPS_NAV_ENABLE_FDE 0u
#define GPS_PROFILE_CPU_HZ 50000000ULL
#define GPS_ACQ_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ / 2u)
#define GPS_ACQ_STUCK_WARN_TICKS (GPS_SYSTICK_HZ * 2u)
#define GPS_ACQ_HOT_RETRY_DELAY_TICKS (GPS_SYSTICK_HZ * 30u)
#define GPS_ACQ_REACQ_PRIORITY_TICKS \
(GPS_SYSTICK_HZ * 2u * 60u * 60u)
#define GPS_ACQ_MAX_HOT_STREAK 4u
#define GPS_ACQ_STUCK_WARN_TICKS (GPS_SYSTICK_HZ * 8u)
#define GPS_EPHEMERIS_CACHE_MAX_AGE_TICKS \
(GPS_SYSTICK_HZ * 2u * 60u * 60u)
#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_NAV_ABS_SAMPLE_SYNC_WINDOW_MS 25u
@@ -223,15 +234,6 @@
uint32_t REM_CODE_PHASE_REG; // 'h4c
}gps_trck_t;
typedef enum {
GPS_SAT_NOT_SEEN = 0,
GPS_SAT_ACQUIRING,
GPS_SAT_ACQUIRED,
GPS_SAT_TRACKING,
GPS_SAT_LOST,
GPS_SAT_STALE
} gps_satellite_state_t;
typedef enum {
GPS_ACQ_ENGINE_IDLE = 0,
GPS_ACQ_ENGINE_BUSY
@@ -458,9 +460,13 @@
uint64_t gps_get_systick(void);
static inline uint64_t gps_read_cycle_counter(void)
{
#ifdef __riscv
uint64_t cycles;
asm volatile("rdcycle %0" : "=r"(cycles));
return cycles;
#else
return 0u;
#endif
}
bool gps_get_ionoutc(ionoutc_t* ionoutc);
void gps_background_task(void);
@@ -5,8 +5,8 @@
/*
* Verbosity is cumulative:
* 1: navigation result (position and all DOP values) plus tracked
* satellite count/PRN summary
* 1: navigation result (position and all DOP values), tracked
* satellite summary and low-rate acquisition/reacquisition status
* 2: reserved for additional status detail
* 3: state transitions, warnings/errors and integrity summary
* 4: performance, NAV satellite and low-level diagnostic detail
@@ -29,13 +29,7 @@ typedef struct gps_ephem_ll {
volatile bool gps_measurement_valid;
} gps_ephem_ll_t;
typedef struct gps_acq_ll {
unsigned int prn_no;
struct gps_acq_ll* next;
} gps_acq_ll_t;
extern volatile gps_ephem_ll_t* tracking_node;
extern volatile gps_acq_ll_t* acquisition_node;
bool insert_element(volatile gps_ephem_ll_t** node,
volatile gps_ephemeris_t* data_p,
@@ -45,8 +39,4 @@ bool trck_ip_exists(volatile gps_ephem_ll_t** node, unsigned char trck_ip_no);
volatile gps_ephem_ll_t* get_element_by_ip_number(volatile gps_ephem_ll_t** node,
unsigned char trck_ip_no);
bool insert_acq_element(volatile gps_acq_ll_t** node, unsigned int prn_no);
bool delete_acq_element_by_prn_number(volatile gps_acq_ll_t** node, unsigned int prn_no);
bool acq_prn_exists(volatile gps_acq_ll_t** node, unsigned int prn_no);
#endif
@@ -9,6 +9,10 @@
#define GPS_AIDED_MAX_AGE_MS 1000u
#ifndef GPS_BUILD_ID
#define GPS_BUILD_ID "unknown"
#endif
void gps_system_uart_publish_location(
const char* source,
double latitude_deg,
@@ -66,9 +70,10 @@ void gps_system_uart_publish_startup(
imu_sensor_diagnostics_t imu;
imu_sensor_get_diagnostics(&imu);
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
"init eskf_enabled=%u eskf_ready=%u imu=%u sensor=%s who=0x%02x "
"init build=%s eskf_enabled=%u eskf_ready=%u imu=%u sensor=%s who=0x%02x "
"running=%u target_hz=%lu actual_hz=%lu publish_every=%u "
"nav_ms=%u timebase_hz=%lu\r\n",
GPS_BUILD_ID,
fusion_enabled ? 1u : 0u,
fusion_initialized ? 1u : 0u,
imu_initialized ? 1u : 0u,
@@ -0,0 +1,289 @@
#include "gps/gps_acq_manager.h"
#include <stddef.h>
#include <string.h>
static bool gps_acq_manager_prn_valid(unsigned int prn)
{
return prn < GPS_ACQ_MANAGER_PRN_COUNT;
}
static bool gps_acq_manager_retry_due(
const gps_acq_manager_t* manager,
const gps_acq_prn_entry_t* entry,
uint64_t now,
bool hot)
{
uint64_t retry_delay = hot
? manager->config.hot_retry_delay_ticks
: manager->config.retry_delay_ticks;
return !entry->has_attempted ||
((now - entry->last_attempt_tick) >=
retry_delay);
}
static bool gps_acq_manager_hot_active(
const gps_acq_prn_entry_t* entry,
uint64_t now)
{
return (entry->state == GPS_ACQ_PRN_HOT) &&
(now <= entry->hot_until_tick);
}
static bool gps_acq_manager_find(
const gps_acq_manager_t* manager,
uint64_t now,
bool hot,
unsigned int cursor,
unsigned int* prn_out)
{
for(unsigned int offset = 0u;
offset < GPS_ACQ_MANAGER_PRN_COUNT;
offset++)
{
unsigned int prn =
(cursor + offset) % GPS_ACQ_MANAGER_PRN_COUNT;
const gps_acq_prn_entry_t* entry = &manager->prn[prn];
bool candidate = hot
? gps_acq_manager_hot_active(entry, now)
: ((entry->state == GPS_ACQ_PRN_COLD) ||
((entry->state == GPS_ACQ_PRN_HOT) &&
(now > entry->hot_until_tick)));
if(candidate &&
gps_acq_manager_retry_due(
manager, entry, now, hot))
{
*prn_out = prn;
return true;
}
}
return false;
}
void gps_acq_manager_init(
gps_acq_manager_t* manager,
const gps_acq_manager_config_t* config)
{
if((manager == NULL) || (config == NULL))
{
return;
}
memset(manager, 0, sizeof(*manager));
manager->config = *config;
if(manager->config.max_hot_streak == 0u)
{
manager->config.max_hot_streak = 1u;
}
}
bool gps_acq_manager_enable_cold(
gps_acq_manager_t* manager,
unsigned int prn)
{
if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
{
return false;
}
gps_acq_prn_entry_t* entry = &manager->prn[prn];
if(entry->state == GPS_ACQ_PRN_DISABLED)
{
entry->state = GPS_ACQ_PRN_COLD;
return true;
}
return entry->state == GPS_ACQ_PRN_COLD;
}
gps_acq_prn_state_t gps_acq_manager_state(
const gps_acq_manager_t* manager,
unsigned int prn)
{
if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
{
return GPS_ACQ_PRN_DISABLED;
}
return manager->prn[prn].state;
}
const char* gps_acq_manager_state_name(gps_acq_prn_state_t state)
{
switch(state)
{
case GPS_ACQ_PRN_DISABLED:
return "disabled";
case GPS_ACQ_PRN_COLD:
return "cold";
case GPS_ACQ_PRN_HOT:
return "hot";
case GPS_ACQ_PRN_ACQUIRING:
return "acquiring";
case GPS_ACQ_PRN_ACQUIRED:
return "acquired";
case GPS_ACQ_PRN_TRACKING:
return "tracking";
case GPS_ACQ_PRN_STALE:
return "stale";
default:
return "unknown";
}
}
void gps_acq_manager_mark_acquired(
gps_acq_manager_t* manager,
unsigned int prn)
{
if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
{
return;
}
manager->prn[prn].state = GPS_ACQ_PRN_ACQUIRED;
manager->prn[prn].attempt_was_hot = false;
}
void gps_acq_manager_mark_tracking(
gps_acq_manager_t* manager,
unsigned int prn)
{
if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
{
return;
}
manager->prn[prn].state = GPS_ACQ_PRN_TRACKING;
manager->prn[prn].attempt_was_hot = false;
manager->prn[prn].hot_until_tick = 0u;
}
void gps_acq_manager_mark_stale(
gps_acq_manager_t* manager,
unsigned int prn)
{
if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
{
return;
}
if(manager->prn[prn].state == GPS_ACQ_PRN_TRACKING)
{
manager->prn[prn].state = GPS_ACQ_PRN_STALE;
}
}
void gps_acq_manager_mark_lost(
gps_acq_manager_t* manager,
unsigned int prn,
uint64_t now)
{
if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
{
return;
}
gps_acq_prn_entry_t* entry = &manager->prn[prn];
bool already_hot = entry->state == GPS_ACQ_PRN_HOT;
entry->state = GPS_ACQ_PRN_HOT;
entry->attempt_was_hot = false;
entry->hot_until_tick = now + manager->config.hot_window_ticks;
if(!already_hot)
{
/* A real tracking loss gets one immediate reacquisition attempt.
* Later failures observe the hot retry backoff. */
entry->has_attempted = false;
}
}
void gps_acq_manager_mark_failed(
gps_acq_manager_t* manager,
unsigned int prn,
uint64_t now)
{
if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
{
return;
}
gps_acq_prn_entry_t* entry = &manager->prn[prn];
entry->last_attempt_tick = now;
entry->has_attempted = true;
entry->state =
entry->attempt_was_hot && (now <= entry->hot_until_tick)
? GPS_ACQ_PRN_HOT : GPS_ACQ_PRN_COLD;
entry->attempt_was_hot = false;
}
bool gps_acq_manager_start_next(
gps_acq_manager_t* manager,
uint64_t now,
unsigned int* prn_out,
bool* hot_out)
{
if((manager == NULL) || (prn_out == NULL))
{
return false;
}
unsigned int hot_prn = GPS_ACQ_MANAGER_INVALID_PRN;
unsigned int cold_prn = GPS_ACQ_MANAGER_INVALID_PRN;
bool have_hot = gps_acq_manager_find(
manager, now, true, manager->hot_cursor, &hot_prn);
bool have_cold = gps_acq_manager_find(
manager, now, false, manager->cold_cursor, &cold_prn);
bool choose_hot = have_hot &&
(!have_cold ||
(manager->hot_streak < manager->config.max_hot_streak));
unsigned int prn;
if(choose_hot)
{
prn = hot_prn;
manager->hot_cursor =
(prn + 1u) % GPS_ACQ_MANAGER_PRN_COUNT;
manager->hot_streak++;
}
else if(have_cold)
{
prn = cold_prn;
manager->cold_cursor =
(prn + 1u) % GPS_ACQ_MANAGER_PRN_COUNT;
manager->hot_streak = 0u;
}
else
{
return false;
}
gps_acq_prn_entry_t* entry = &manager->prn[prn];
entry->attempt_was_hot = choose_hot;
entry->last_attempt_tick = now;
entry->has_attempted = true;
entry->state = GPS_ACQ_PRN_ACQUIRING;
*prn_out = prn;
if(hot_out != NULL)
{
*hot_out = choose_hot;
}
return true;
}
unsigned int gps_acq_manager_count_state(
const gps_acq_manager_t* manager,
gps_acq_prn_state_t state)
{
if(manager == NULL)
{
return 0u;
}
unsigned int count = 0u;
for(unsigned int prn = 0u;
prn < GPS_ACQ_MANAGER_PRN_COUNT;
prn++)
{
if(manager->prn[prn].state == state)
{
count++;
}
}
return count;
}
unsigned int gps_acq_manager_count_searchable(
const gps_acq_manager_t* manager)
{
return gps_acq_manager_count_state(manager, GPS_ACQ_PRN_COLD) +
gps_acq_manager_count_state(manager, GPS_ACQ_PRN_HOT);
}
@@ -0,0 +1,68 @@
#include "gps/gps_ephemeris_cache.h"
#include <stddef.h>
#include <string.h>
void gps_ephemeris_cache_init(
gps_ephemeris_cache_t* cache,
uint64_t max_age_ticks)
{
if(cache == NULL)
{
return;
}
memset(cache, 0, sizeof(*cache));
cache->max_age_ticks = max_age_ticks;
}
bool gps_ephemeris_cache_store(
gps_ephemeris_cache_t* cache,
unsigned int prn,
const gps_ephemeris_t* ephemeris,
uint64_t now)
{
if((cache == NULL) || (ephemeris == NULL) ||
(prn >= GPS_PRN_COUNT) ||
!ephemeris->ephemeris_valid ||
(ephemeris->SVID != prn))
{
return false;
}
cache->entry[prn].ephemeris = *ephemeris;
cache->entry[prn].committed_tick = now;
cache->entry[prn].valid = true;
return true;
}
bool gps_ephemeris_cache_restore(
gps_ephemeris_cache_t* cache,
unsigned int prn,
uint64_t now,
gps_ephemeris_t* ephemeris_out)
{
if((cache == NULL) || (ephemeris_out == NULL) ||
(prn >= GPS_PRN_COUNT))
{
return false;
}
gps_ephemeris_cache_entry_t* entry = &cache->entry[prn];
if(!entry->valid ||
((now - entry->committed_tick) > cache->max_age_ticks))
{
entry->valid = false;
return false;
}
*ephemeris_out = entry->ephemeris;
return true;
}
void gps_ephemeris_cache_invalidate(
gps_ephemeris_cache_t* cache,
unsigned int prn)
{
if((cache == NULL) || (prn >= GPS_PRN_COUNT))
{
return;
}
memset(&cache->entry[prn], 0, sizeof(cache->entry[prn]));
}
@@ -1,4 +1,6 @@
#include "gps/linked_list.h"
#include "gps/gps_acq_manager.h"
#include "gps/gps_ephemeris_cache.h"
#include "gps/gps_interrupt_handlers.h"
#include "gps/Navigation.h"
#include "gps/gps_nav_epoch.h"
@@ -56,10 +58,10 @@ static volatile bool acq_shadow_pending[GPS_ACQ_IP_COUNT];
static gps_acq_result_t acq_shadow_result[GPS_ACQ_IP_COUNT];
static gps_tracking_shadow_t trk_shadow[GPS_TRCK_IP_COUNT];
static volatile gps_satellite_state_t satellite_state[GPS_PRN_COUNT];
static volatile unsigned char satellite_tracking_ip[GPS_PRN_COUNT];
static volatile uint64_t satellite_last_acq_tick[GPS_PRN_COUNT];
static gps_acq_result_t satellite_acq_result[GPS_PRN_COUNT];
static gps_acq_manager_t gps_acq_manager;
static gps_ephemeris_cache_t gps_ephemeris_cache;
static gps_isr_event_t gps_event_queue[GPS_EVENT_QUEUE_SIZE];
static volatile unsigned int gps_event_head = 0;
@@ -74,7 +76,7 @@ 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 uint64_t gps_last_acq_status_log_tick = 0u;
static unsigned int gps_external_irq_id = 0u;
static gps_external_irq_handler_t gps_external_irq_handler = NULL;
@@ -104,27 +106,6 @@ static unsigned int gps_mask_count(uint32_t mask)
return count;
}
static const char* gps_satellite_state_name(gps_satellite_state_t state)
{
switch(state)
{
case GPS_SAT_NOT_SEEN:
return "not_seen";
case GPS_SAT_ACQUIRING:
return "acquiring";
case GPS_SAT_ACQUIRED:
return "acquired";
case GPS_SAT_TRACKING:
return "tracking";
case GPS_SAT_LOST:
return "lost";
case GPS_SAT_STALE:
return "stale";
default:
return "unknown";
}
}
static double gps_correct_abs_sample(uint32_t abs_sample, uint32_t rem_code_phase)
{
return (double)abs_sample - ((double)rem_code_phase / CODE_FREQ_BASIS);
@@ -353,6 +334,14 @@ static void gps_set_tracking_nav_bits_mode(unsigned char trck_ip_no)
static void gps_runtime_state_init(void)
{
const gps_acq_manager_config_t acq_config = {
.retry_delay_ticks = GPS_ACQ_RETRY_DELAY_TICKS,
.hot_retry_delay_ticks =
GPS_ACQ_HOT_RETRY_DELAY_TICKS,
.hot_window_ticks = GPS_ACQ_REACQ_PRIORITY_TICKS,
.max_hot_streak = GPS_ACQ_MAX_HOT_STREAK
};
gps_event_head = 0;
gps_event_tail = 0;
gps_event_overflow_count = 0;
@@ -367,9 +356,13 @@ static void gps_runtime_state_init(void)
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;
gps_acq_round_robin_prn = 0;
gps_last_acq_status_log_tick = 0u;
gps_external_irq_id = 0u;
gps_external_irq_handler = NULL;
gps_acq_manager_init(&gps_acq_manager, &acq_config);
gps_ephemeris_cache_init(
&gps_ephemeris_cache,
GPS_EPHEMERIS_CACHE_MAX_AGE_TICKS);
for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
{
@@ -385,9 +378,7 @@ static void gps_runtime_state_init(void)
for(unsigned int i = 0 ; i < GPS_PRN_COUNT ; i++)
{
satellite_state[i] = GPS_SAT_NOT_SEEN;
satellite_tracking_ip[i] = GPS_PRN_INVALID;
satellite_last_acq_tick[i] = 0;
memset(&satellite_acq_result[i], 0, sizeof(satellite_acq_result[i]));
}
@@ -400,6 +391,25 @@ static void gps_runtime_state_init(void)
}
void gps_acq_configure_prns(
const unsigned char* prns,
unsigned int count)
{
if(prns == NULL)
{
return;
}
for(unsigned int i = 0u ; i < count ; i++)
{
if(gps_prn_valid(prns[i]))
{
(void)gps_acq_manager_enable_cold(
&gps_acq_manager, prns[i]);
}
}
gps_acq_schedule_pending = true;
}
bool gps_interrupt_attach_external_source(
unsigned int irq_id,
gps_external_irq_handler_t handler)
@@ -598,18 +608,22 @@ static void gps_mark_satellite_tracking(unsigned int prn_no, unsigned char trck_
{
if(gps_prn_valid(prn_no))
{
gps_satellite_state_t previous_state = satellite_state[prn_no];
gps_acq_prn_state_t previous_state =
gps_acq_manager_state(&gps_acq_manager, prn_no);
unsigned char previous_ip = satellite_tracking_ip[prn_no];
satellite_state[prn_no] = GPS_SAT_TRACKING;
gps_acq_manager_mark_tracking(&gps_acq_manager, prn_no);
satellite_tracking_ip[prn_no] = trck_ip_no;
if((previous_state != GPS_SAT_TRACKING) || (previous_ip != trck_ip_no))
if((previous_state != GPS_ACQ_PRN_TRACKING) ||
(previous_ip != trck_ip_no))
{
gps_runtime_diagnostics.satellite_connected_total++;
gps_runtime_diagnostics.last_connected_prn = prn_no;
gps_runtime_diagnostics.last_connected_tick = gps_get_systick();
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK,
"satellite connected: prn=%u trk=%u previous=%s previous_trk=%u\r\n",
prn_no, trck_ip_no, gps_satellite_state_name(previous_state), previous_ip);
prn_no, trck_ip_no,
gps_acq_manager_state_name(previous_state),
previous_ip);
}
}
}
@@ -618,18 +632,22 @@ static void gps_mark_satellite_lost(unsigned int prn_no)
{
if(gps_prn_valid(prn_no))
{
gps_satellite_state_t previous_state = satellite_state[prn_no];
gps_acq_prn_state_t previous_state =
gps_acq_manager_state(&gps_acq_manager, prn_no);
unsigned char previous_ip = satellite_tracking_ip[prn_no];
satellite_state[prn_no] = GPS_SAT_LOST;
uint64_t now = gps_get_systick();
gps_acq_manager_mark_lost(&gps_acq_manager, prn_no, now);
satellite_tracking_ip[prn_no] = GPS_PRN_INVALID;
if(previous_state != GPS_SAT_LOST)
if(previous_state != GPS_ACQ_PRN_HOT)
{
gps_runtime_diagnostics.satellite_lost_total++;
gps_runtime_diagnostics.last_lost_prn = prn_no;
gps_runtime_diagnostics.last_lost_tick = gps_get_systick();
gps_runtime_diagnostics.last_lost_tick = now;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK,
"satellite lost: prn=%u previous=%s previous_trk=%u\r\n",
prn_no, gps_satellite_state_name(previous_state), previous_ip);
prn_no,
gps_acq_manager_state_name(previous_state),
previous_ip);
}
}
}
@@ -652,7 +670,6 @@ bool assign_tracking_from_result(const gps_acq_result_t* result)
if(active && (trk->PRN_NUMBER == prn_no))
{
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_TRK, "acquired prn %u is already being tracked\r\n", prn_no);
(void)delete_acq_element_by_prn_number(&acquisition_node, prn_no);
gps_mark_satellite_tracking(prn_no, i);
return false;
}
@@ -666,13 +683,25 @@ bool assign_tracking_from_result(const gps_acq_result_t* result)
{
tracking_allocated[i] = true;
gps_clear_tracking_slot(i);
gps_ephemeris_t cached_ephemeris;
if(gps_ephemeris_cache_restore(
&gps_ephemeris_cache, prn_no,
gps_get_systick(), &cached_ephemeris))
{
gps_frames_parsed[i].gps_ephems =
cached_ephemeris;
gps_runtime_diagnostics
.ephemeris_cache_restore_total++;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
"restored ephemeris cache: prn=%u trk=%u iode=%u\r\n",
prn_no, i, cached_ephemeris.iode);
}
gps_set_tracking_nav_bits_mode(i);
trck_ips[i]->PRN_NUMBER = result->prn;
trck_ips[i]->CODE_PHASE = result->code_phase;
trck_ips[i]->DOPPLER_REG = result->doppler;
trck_ips[i]->CONTROL_REG |= (1u << GPS_TRCK_NAV_BITS_INTR_EN_POS);
trck_ips[i]->CONTROL_REG |= (1u << GPS_TRCK_START_POS);
(void)delete_acq_element_by_prn_number(&acquisition_node, prn_no);
gps_mark_satellite_tracking(prn_no, i);
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK, "tracking started: prn=%u trk=%u code=%u doppler=%u\r\n",
prn_no, i, result->code_phase, result->doppler);
@@ -850,19 +879,16 @@ static void gps_process_acq_result(const gps_acq_result_t* result)
return;
}
satellite_last_acq_tick[prn] = result->tick;
if(!result->acquired)
{
if(satellite_state[prn] == GPS_SAT_ACQUIRING)
{
satellite_state[prn] = GPS_SAT_NOT_SEEN;
}
gps_acq_manager_mark_failed(
&gps_acq_manager, prn, result->tick);
GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_ACQ, "satellite search ended: engine=%u prn=%u status=0x%x result=not_acquired\r\n",
result->engine_id, prn, result->status);
return;
}
satellite_state[prn] = GPS_SAT_ACQUIRED;
gps_acq_manager_mark_acquired(&gps_acq_manager, prn);
satellite_acq_result[prn] = *result;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_ACQ,
"acquired: engine=%u prn=%u code=%u doppler=%u peak=%u\r\n",
@@ -870,8 +896,7 @@ static void gps_process_acq_result(const gps_acq_result_t* result)
if(gps_tracking_prn_active(prn))
{
(void)delete_acq_element_by_prn_number(&acquisition_node, prn);
satellite_state[prn] = GPS_SAT_TRACKING;
gps_acq_manager_mark_tracking(&gps_acq_manager, prn);
return;
}
@@ -962,8 +987,16 @@ static void gps_mark_node_fresh(volatile gps_ephem_ll_t* node, unsigned int stat
((status & (1u << GPS_TRCK_STOP_UNLOCK_POS)) == 0u);
if((node->data_p != NULL) && gps_prn_valid(node->data_p->SVID))
{
satellite_state[node->data_p->SVID] = node->gps_measurement_valid ? GPS_SAT_TRACKING : GPS_SAT_STALE;
satellite_tracking_ip[node->data_p->SVID] = node->tracking_ip_no;
unsigned int prn = node->data_p->SVID;
if(node->gps_measurement_valid)
{
gps_mark_satellite_tracking(prn, node->tracking_ip_no);
}
else
{
gps_acq_manager_mark_stale(&gps_acq_manager, prn);
satellite_tracking_ip[prn] = node->tracking_ip_no;
}
}
}
@@ -987,7 +1020,6 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event)
if(gps_prn_valid(dropped_prn))
{
(void)insert_acq_element(&acquisition_node, dropped_prn);
gps_mark_satellite_lost(dropped_prn);
}
@@ -1083,6 +1115,19 @@ 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 <= 3u) &&
(gps_frame_parsed->pending_ephemeris_mask == 0u) &&
gps_ephemeris_cache_store(
&gps_ephemeris_cache, event->prn,
(const gps_ephemeris_t*)&gps_frame_parsed->gps_ephems,
event->tick))
{
gps_runtime_diagnostics.ephemeris_cache_store_total++;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
"stored ephemeris cache: prn=%u trk=%u iode=%u\r\n",
event->prn, trck_ip_no,
gps_frame_parsed->gps_ephems.iode);
}
if((subframe_id >= 1u) && (subframe_id <= 5u) &&
(how_tow_sec < (unsigned int)GPS_TIME_WEEK_SECONDS))
{
@@ -1169,10 +1214,6 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
}
gps_mark_node_fresh(node, event->status, event->tick);
if(gps_prn_valid(event->prn))
{
gps_mark_satellite_tracking(event->prn, trck_ip_no);
}
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_TRK, "nav bits: trk=%u prn=%u mask=0x%x status=0x%x\r\n",
trck_ip_no, event->prn, node->gps_subframe_mask, event->status);
@@ -1412,7 +1453,7 @@ static void gps_process_tracking_event(const gps_isr_event_t* event)
}
else if(gps_prn_valid(event->prn))
{
(void)insert_acq_element(&acquisition_node, event->prn);
gps_mark_satellite_lost(event->prn);
tracking_allocated[event->index] = false;
if(trck_ip_exists(&tracking_node, event->index))
{
@@ -1502,80 +1543,28 @@ static void gps_promote_acquired_satellites(void)
{
for(unsigned int prn = 0 ; prn < GPS_PRN_COUNT ; prn++)
{
if((satellite_state[prn] == GPS_SAT_ACQUIRED) && !gps_tracking_prn_active(prn))
if((gps_acq_manager_state(&gps_acq_manager, prn) ==
GPS_ACQ_PRN_ACQUIRED) &&
!gps_tracking_prn_active(prn))
{
(void)assign_tracking_from_result(&satellite_acq_result[prn]);
}
}
}
static void gps_remove_tracked_prns_from_acq_list(void)
static void gps_reconcile_tracked_prns(void)
{
for(unsigned int prn = 0 ; prn < GPS_PRN_COUNT ; prn++)
{
if(gps_tracking_prn_active(prn))
if(gps_tracking_prn_active(prn) &&
(gps_acq_manager_state(&gps_acq_manager, prn) !=
GPS_ACQ_PRN_STALE))
{
if(acq_prn_exists(&acquisition_node, prn))
{
(void)delete_acq_element_by_prn_number(&acquisition_node, prn);
}
satellite_state[prn] = GPS_SAT_TRACKING;
gps_acq_manager_mark_tracking(&gps_acq_manager, prn);
}
}
}
static bool gps_prn_schedulable(unsigned int prn, uint64_t now)
{
if(!gps_prn_valid(prn))
{
return false;
}
if((satellite_state[prn] == GPS_SAT_ACQUIRING) ||
(satellite_state[prn] == GPS_SAT_ACQUIRED) ||
gps_tracking_prn_active(prn))
{
return false;
}
if((satellite_last_acq_tick[prn] != 0u) &&
((now - satellite_last_acq_tick[prn]) < GPS_ACQ_RETRY_DELAY_TICKS))
{
return false;
}
return true;
}
static bool gps_find_next_acq_prn(uint64_t now, unsigned int* prn_out)
{
gps_acq_ll_t* iterator = (gps_acq_ll_t*)acquisition_node;
gps_acq_ll_t* wrapped_candidate = NULL;
while(iterator != NULL)
{
unsigned int prn = iterator->prn_no;
if(gps_prn_schedulable(prn, now))
{
if(prn >= gps_acq_round_robin_prn)
{
*prn_out = prn;
gps_acq_round_robin_prn = (prn + 1u) % GPS_PRN_COUNT;
return true;
}
if(wrapped_candidate == NULL)
{
wrapped_candidate = iterator;
}
}
iterator = iterator->next;
}
if(wrapped_candidate != NULL)
{
*prn_out = wrapped_candidate->prn_no;
gps_acq_round_robin_prn = (*prn_out + 1u) % GPS_PRN_COUNT;
return true;
}
return false;
}
static bool gps_acq_engine_available(unsigned int engine_id, volatile gps_acq_t* acq_ip)
{
if((engine_id >= GPS_ACQ_IP_COUNT) || (acq_ip == NULL))
@@ -1604,7 +1593,7 @@ static bool gps_acq_engine_available(unsigned int engine_id, volatile gps_acq_t*
void gps_acq_scheduler_run(void)
{
uint64_t now = gps_get_systick();
gps_remove_tracked_prns_from_acq_list();
gps_reconcile_tracked_prns();
gps_promote_acquired_satellites();
for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
@@ -1620,7 +1609,9 @@ void gps_acq_scheduler_run(void)
continue;
}
unsigned int prn;
if(!gps_find_next_acq_prn(now, &prn))
bool hot = false;
if(!gps_acq_manager_start_next(
&gps_acq_manager, now, &prn, &hot))
{
continue;
}
@@ -1629,10 +1620,10 @@ void gps_acq_scheduler_run(void)
acq_engine_prn[i] = (unsigned char)prn;
acq_engine_start_tick[i] = now;
acq_engine_stuck_warned[i] = false;
satellite_state[prn] = GPS_SAT_ACQUIRING;
satellite_last_acq_tick[prn] = now;
acq_start(acq_ip, (unsigned char)prn);
GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_SCHED, "satellite search started: engine=%u prn=%u\r\n", i, prn);
GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_SCHED,
"satellite search started: engine=%u prn=%u priority=%s\r\n",
i, prn, hot ? "reacquire" : "cold");
}
}
@@ -1641,17 +1632,12 @@ static void gps_check_acq_stuck_busy(void)
uint64_t now = gps_get_systick();
for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
{
if((acq_engine_state[i] != GPS_ACQ_ENGINE_BUSY) || acq_engine_stuck_warned[i])
if(acq_engine_state[i] != GPS_ACQ_ENGINE_BUSY)
{
continue;
}
uint64_t age = now - acq_engine_start_tick[i];
if(age <= GPS_ACQ_STUCK_WARN_TICKS)
{
continue;
}
volatile gps_acq_t* acq_ip = gps_get_acq_ip(i);
if(acq_ip == NULL)
{
@@ -1660,15 +1646,68 @@ static void gps_check_acq_stuck_busy(void)
uint32_t control = acq_ip->CONTROL_REG;
uint32_t status = acq_ip->STATUS_REG;
acq_engine_stuck_warned[i] = true;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_ACQ,
"ACQ engine %u busy for %lu ticks prn=%u status=0x%x control=0x%x; check rf_clk/sample_align/acq_end\r\n",
i, (unsigned long)age, acq_engine_prn[i], status, control);
if((age >= GPS_ACQ_STUCK_WARN_TICKS) &&
!acq_engine_stuck_warned[i])
{
acq_engine_stuck_warned[i] = true;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_ACQ,
"ACQ engine %u busy for %lu ticks prn=%u status=0x%x control=0x%x; check rf_clk/sample_align/acq_end\r\n",
i, (unsigned long)age, acq_engine_prn[i],
status, control);
}
(void)status;
(void)control;
}
}
static void gps_log_acq_status(void)
{
uint64_t now = gps_get_systick();
if((gps_last_acq_status_log_tick != 0u) &&
((now - gps_last_acq_status_log_tick) <
GPS_ACQ_STATUS_LOG_PERIOD_TICKS))
{
return;
}
gps_last_acq_status_log_tick = now;
unsigned int engine = GPS_PRN_INVALID;
unsigned int engine_prn = GPS_PRN_INVALID;
uint64_t engine_age_ticks = 0u;
for(unsigned int i = 0u ; i < GPS_ACQ_IP_COUNT ; i++)
{
if(acq_engine_state[i] == GPS_ACQ_ENGINE_BUSY)
{
engine = i;
engine_prn = acq_engine_prn[i];
engine_age_ticks = now - acq_engine_start_tick[i];
break;
}
}
unsigned int tracking = gps_acq_manager_count_state(
&gps_acq_manager, GPS_ACQ_PRN_TRACKING);
unsigned int stale = gps_acq_manager_count_state(
&gps_acq_manager, GPS_ACQ_PRN_STALE);
unsigned int hot = gps_acq_manager_count_state(
&gps_acq_manager, GPS_ACQ_PRN_HOT);
unsigned int cold = gps_acq_manager_count_state(
&gps_acq_manager, GPS_ACQ_PRN_COLD);
unsigned int acquiring = gps_acq_manager_count_state(
&gps_acq_manager, GPS_ACQ_PRN_ACQUIRING);
unsigned int acquired = gps_acq_manager_count_state(
&gps_acq_manager, GPS_ACQ_PRN_ACQUIRED);
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_ACQ,
"state=%s tracked=%u stale=%u hot=%u cold=%u acquiring=%u acquired=%u engine=%u prn=%u age_ms=%lu\r\n",
(tracking >= GPS_NAV_MIN_WLS_SATELLITES)
? "tracking" : "searching",
tracking, stale, hot, cold, acquiring, acquired,
engine, engine_prn,
(unsigned long)((engine_age_ticks * 1000u) /
GPS_SYSTICK_HZ));
}
static void gps_mark_stale_measurements(void)
{
uint64_t now = gps_get_systick();
@@ -1681,7 +1720,9 @@ static void gps_mark_stale_measurements(void)
iterator->gps_measurement_valid = false;
if(iterator->data_p != NULL && gps_prn_valid(iterator->data_p->SVID))
{
satellite_state[iterator->data_p->SVID] = GPS_SAT_STALE;
gps_acq_manager_mark_stale(
&gps_acq_manager,
iterator->data_p->SVID);
}
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_TRK, "stale measurement: trk=%u age_ticks=%lu\r\n",
iterator->tracking_ip_no, (unsigned long)(now - iterator->gps_last_measurement_tick));
@@ -1699,6 +1740,7 @@ void gps_background_task(void)
{
gps_mark_stale_measurements();
gps_check_acq_stuck_busy();
gps_log_acq_status();
}
if(gps_take_acq_schedule_pending())
{
+3 -7
View File
@@ -57,7 +57,6 @@ int unsigned gps_subframes[GPS_TRCK_IP_COUNT][10];
volatile gps_frame_t gps_frames_parsed[GPS_TRCK_IP_COUNT];
volatile gps_ephem_ll_t* tracking_node = NULL;
volatile gps_acq_ll_t* acquisition_node = NULL;
static volatile bool gps_initial_position_valid = false;
static volatile bool gps_ionoutc_valid = false;
@@ -1405,12 +1404,6 @@ void gps_startup_initialization(unsigned int nsp_tick)
unsigned char gps_active_satellites[] = GPS_ACTIVE_PRNS;
for(unsigned char i = 0; i < GPS_ACTIVE_SAT_NUM ; i++)
{
unsigned int prn = (unsigned int) gps_active_satellites[i];
(void)insert_acq_element(&acquisition_node, prn);
}
for(unsigned i = 0 ; i < GPS_TRCK_IP_COUNT ; i++)
{
// stop all working tracking ips & disable interrupts.
@@ -1436,6 +1429,9 @@ void gps_startup_initialization(unsigned int nsp_tick)
}
gps_interrupt_init();
gps_acq_configure_prns(
gps_active_satellites,
GPS_ACTIVE_SAT_NUM);
/* The AFE and its sample timebase are shared by all acquisition/tracking
* channels. Start them exactly once; per-PRN acquisition must not rephase it. */
acq_afe_start(acq_ips[0]);
@@ -127,82 +127,3 @@ bool delete_by_ip_number(volatile gps_ephem_ll_t** node, unsigned char trck_ip_n
}
return false;
}
bool acq_prn_exists(volatile gps_acq_ll_t** node, unsigned int prn_no)
{
if(node == NULL)
{
return false;
}
volatile gps_acq_ll_t* iterator = *node;
while(iterator != NULL)
{
if(iterator->prn_no == prn_no)
{
return true;
}
iterator = iterator->next;
}
return false;
}
bool insert_acq_element(volatile gps_acq_ll_t** node, unsigned int prn_no)
{
if((node == NULL) || acq_prn_exists(node, prn_no))
{
return false;
}
gps_acq_ll_t* new_node = (gps_acq_ll_t*)malloc(sizeof(*new_node));
if(new_node == NULL)
{
return false;
}
new_node->prn_no = prn_no;
new_node->next = NULL;
if(*node == NULL)
{
*node = new_node;
return true;
}
volatile gps_acq_ll_t* tail = *node;
while(tail->next != NULL)
{
tail = tail->next;
}
tail->next = new_node;
return true;
}
bool delete_acq_element_by_prn_number(volatile gps_acq_ll_t** node, unsigned int prn_no)
{
if(node == NULL)
{
return false;
}
volatile gps_acq_ll_t* iterator = *node;
volatile gps_acq_ll_t* previous = NULL;
while(iterator != NULL)
{
if(iterator->prn_no == prn_no)
{
if(previous == NULL)
{
*node = iterator->next;
}
else
{
previous->next = iterator->next;
}
free((void*)iterator);
return true;
}
previous = iterator;
iterator = iterator->next;
}
return false;
}
+44 -2
View File
@@ -4,7 +4,8 @@ CFLAGS ?= -O2 -std=c11 -Wall -Wextra -Werror
ROOT := ..
TARGETS := test_imu_calibration test_gps_nav_epoch \
test_gps_nav_epoch_coalescing test_gps_time_anchor \
test_plic_warm_reset
test_plic_warm_reset test_gps_acq_manager \
test_gps_ephemeris_cache test_gps_receiver_hardware_sim
.PHONY: all run sanitize clean
@@ -25,12 +26,26 @@ test_gps_time_anchor: test_gps_time_anchor.c $(ROOT)/src/gps/gps_time_anchor.c
test_plic_warm_reset: test_plic_warm_reset.c $(ROOT)/../bsp/drivers/plic.c
$(CC) $(CFLAGS) -Ifake_bsp -I$(ROOT)/../bsp/include $^ -o $@
test_gps_acq_manager: test_gps_acq_manager.c $(ROOT)/src/gps/gps_acq_manager.c
$(CC) $(CFLAGS) -I$(ROOT)/header $^ -o $@
test_gps_ephemeris_cache: test_gps_ephemeris_cache.c $(ROOT)/src/gps/gps_ephemeris_cache.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -o $@
test_gps_receiver_hardware_sim: test_gps_receiver_hardware_sim.c \
$(ROOT)/src/gps/gps_acq_manager.c \
$(ROOT)/src/gps/gps_ephemeris_cache.c
$(CC) $(CFLAGS) -Wno-comment -Ifake_bsp -I$(ROOT)/header $^ -o $@
run: $(TARGETS)
./test_imu_calibration
./test_gps_nav_epoch
./test_gps_nav_epoch_coalescing
./test_gps_time_anchor
./test_plic_warm_reset
./test_gps_acq_manager
./test_gps_ephemeris_cache
./test_gps_receiver_hardware_sim
sanitize:
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
@@ -47,7 +62,34 @@ sanitize:
-o test_gps_nav_epoch_coalescing_san
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 ./test_gps_nav_epoch_coalescing_san
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
-fsanitize=address,undefined -fno-omit-frame-pointer \
-I$(ROOT)/header test_gps_acq_manager.c \
$(ROOT)/src/gps/gps_acq_manager.c \
-o test_gps_acq_manager_san
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 ./test_gps_acq_manager_san
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
-fsanitize=address,undefined -fno-omit-frame-pointer \
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
test_gps_ephemeris_cache.c \
$(ROOT)/src/gps/gps_ephemeris_cache.c \
-o test_gps_ephemeris_cache_san
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 ./test_gps_ephemeris_cache_san
$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
-fsanitize=address,undefined -fno-omit-frame-pointer \
-Wno-comment -Ifake_bsp -I$(ROOT)/header \
test_gps_receiver_hardware_sim.c \
$(ROOT)/src/gps/gps_acq_manager.c \
$(ROOT)/src/gps/gps_ephemeris_cache.c \
-o test_gps_receiver_hardware_sim_san
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 \
./test_gps_receiver_hardware_sim_san
clean:
rm -f $(TARGETS) test_gps_nav_epoch_coalescing_repro \
test_gps_nav_epoch_san test_gps_nav_epoch_coalescing_san
test_gps_nav_epoch_san test_gps_nav_epoch_coalescing_san \
test_gps_acq_manager_san test_gps_ephemeris_cache_san \
test_gps_receiver_hardware_sim_san
@@ -2,5 +2,6 @@
#define TEST_BSP_SOC_H
#define BSP_PLIC_BASE 0x0c000000UL
#define BSP_GPS_TRCK_COUNT 9UL
#endif
@@ -0,0 +1,144 @@
#include <assert.h>
#include <stdbool.h>
#include <stdio.h>
#include "gps/gps_acq_manager.h"
static gps_acq_manager_t make_manager(
uint64_t retry_delay,
uint64_t hot_retry_delay,
uint64_t hot_window,
unsigned int max_hot_streak)
{
gps_acq_manager_t manager;
const gps_acq_manager_config_t config = {
.retry_delay_ticks = retry_delay,
.hot_retry_delay_ticks = hot_retry_delay,
.hot_window_ticks = hot_window,
.max_hot_streak = max_hot_streak
};
gps_acq_manager_init(&manager, &config);
return manager;
}
static unsigned int start_next(
gps_acq_manager_t* manager,
uint64_t now,
bool expected_hot)
{
unsigned int prn = GPS_ACQ_MANAGER_INVALID_PRN;
bool hot = !expected_hot;
assert(gps_acq_manager_start_next(manager, now, &prn, &hot));
assert(hot == expected_hot);
return prn;
}
static void test_cold_round_robin_and_retry_from_tick_zero(void)
{
gps_acq_manager_t manager =
make_manager(5u, 5u, 100u, 4u);
assert(gps_acq_manager_enable_cold(&manager, 0u));
assert(gps_acq_manager_enable_cold(&manager, 1u));
assert(gps_acq_manager_enable_cold(&manager, 2u));
assert(start_next(&manager, 0u, false) == 0u);
/* A tick-zero attempt is real and must still observe retry delay. */
gps_acq_manager_mark_failed(&manager, 0u, 0u);
assert(start_next(&manager, 0u, false) == 1u);
gps_acq_manager_mark_failed(&manager, 1u, 0u);
assert(start_next(&manager, 0u, false) == 2u);
gps_acq_manager_mark_failed(&manager, 2u, 0u);
unsigned int ignored;
assert(!gps_acq_manager_start_next(
&manager, 4u, &ignored, NULL));
assert(start_next(&manager, 5u, false) == 0u);
}
static void test_hot_priority_round_robin_and_cold_fairness(void)
{
gps_acq_manager_t manager =
make_manager(1u, 1u, 100u, 2u);
assert(gps_acq_manager_enable_cold(&manager, 0u));
assert(gps_acq_manager_enable_cold(&manager, 1u));
gps_acq_manager_mark_lost(&manager, 7u, 10u);
gps_acq_manager_mark_lost(&manager, 8u, 10u);
assert(start_next(&manager, 10u, true) == 7u);
gps_acq_manager_mark_failed(&manager, 7u, 11u);
assert(start_next(&manager, 11u, true) == 8u);
gps_acq_manager_mark_failed(&manager, 8u, 12u);
/* Two hot attempts force one cold discovery attempt. */
assert(start_next(&manager, 12u, false) == 0u);
gps_acq_manager_mark_failed(&manager, 0u, 13u);
assert(start_next(&manager, 13u, true) == 7u);
}
static void test_expired_hot_becomes_cold_and_tracking_is_excluded(void)
{
gps_acq_manager_t manager =
make_manager(1u, 1u, 10u, 4u);
assert(gps_acq_manager_enable_cold(&manager, 2u));
gps_acq_manager_mark_lost(&manager, 9u, 20u);
unsigned int prn = start_next(&manager, 31u, false);
assert(prn == 2u);
gps_acq_manager_mark_tracking(&manager, prn);
prn = start_next(&manager, 31u, false);
assert(prn == 9u);
gps_acq_manager_mark_acquired(&manager, prn);
unsigned int ignored;
assert(!gps_acq_manager_start_next(
&manager, 100u, &ignored, NULL));
gps_acq_manager_mark_tracking(&manager, 9u);
gps_acq_manager_mark_stale(&manager, 9u);
assert(gps_acq_manager_state(&manager, 9u) ==
GPS_ACQ_PRN_STALE);
gps_acq_manager_mark_lost(&manager, 9u, 101u);
assert(gps_acq_manager_state(&manager, 9u) ==
GPS_ACQ_PRN_HOT);
}
static void test_invalid_inputs_are_safe(void)
{
gps_acq_manager_t manager =
make_manager(1u, 1u, 10u, 0u);
assert(!gps_acq_manager_enable_cold(
&manager, GPS_ACQ_MANAGER_PRN_COUNT));
assert(gps_acq_manager_state(
&manager, GPS_ACQ_MANAGER_PRN_COUNT) ==
GPS_ACQ_PRN_DISABLED);
assert(!gps_acq_manager_start_next(NULL, 0u, NULL, NULL));
assert(gps_acq_manager_count_searchable(NULL) == 0u);
}
static void test_failed_hot_search_is_backed_off(void)
{
gps_acq_manager_t manager =
make_manager(1u, 30u, 100u, 4u);
assert(gps_acq_manager_enable_cold(&manager, 0u));
gps_acq_manager_mark_lost(&manager, 20u, 0u);
assert(start_next(&manager, 0u, true) == 20u);
gps_acq_manager_mark_failed(&manager, 20u, 5u);
assert(start_next(&manager, 5u, false) == 0u);
gps_acq_manager_mark_failed(&manager, 0u, 6u);
assert(start_next(&manager, 7u, false) == 0u);
gps_acq_manager_mark_failed(&manager, 0u, 8u);
assert(start_next(&manager, 35u, true) == 20u);
}
int main(void)
{
test_cold_round_robin_and_retry_from_tick_zero();
test_hot_priority_round_robin_and_cold_fairness();
test_expired_hot_becomes_cold_and_tracking_is_excluded();
test_invalid_inputs_are_safe();
test_failed_hot_search_is_backed_off();
puts("gps acquisition manager tests passed");
return 0;
}
@@ -0,0 +1,81 @@
#include <assert.h>
#include <stdio.h>
#include <string.h>
#include "gps/gps_ephemeris_cache.h"
static gps_ephemeris_t make_ephemeris(
unsigned int prn,
unsigned char iode)
{
gps_ephemeris_t ephemeris;
memset(&ephemeris, 0, sizeof(ephemeris));
ephemeris.SVID = (unsigned char)prn;
ephemeris.iode = iode;
ephemeris.ephemeris_valid = true;
ephemeris.toe.week = 2430;
ephemeris.toe.sec = 123456.0;
return ephemeris;
}
static void test_store_restore_and_expiry(void)
{
gps_ephemeris_cache_t cache;
gps_ephemeris_cache_init(&cache, 100u);
gps_ephemeris_t input = make_ephemeris(4u, 17u);
gps_ephemeris_t output;
memset(&output, 0, sizeof(output));
assert(gps_ephemeris_cache_store(
&cache, 4u, &input, 1000u));
input.iode = 99u;
assert(gps_ephemeris_cache_restore(
&cache, 4u, 1100u, &output));
assert(output.iode == 17u);
assert(output.SVID == 4u);
assert(!gps_ephemeris_cache_restore(
&cache, 4u, 1101u, &output));
/* Expiration invalidates the slot, not only this read. */
assert(!gps_ephemeris_cache_restore(
&cache, 4u, 1001u, &output));
}
static void test_validation_independence_and_invalidate(void)
{
gps_ephemeris_cache_t cache;
gps_ephemeris_cache_init(&cache, 500u);
gps_ephemeris_t prn4 = make_ephemeris(4u, 21u);
gps_ephemeris_t prn19 = make_ephemeris(19u, 44u);
gps_ephemeris_t output;
prn4.ephemeris_valid = false;
assert(!gps_ephemeris_cache_store(
&cache, 4u, &prn4, 0u));
prn4.ephemeris_valid = true;
assert(!gps_ephemeris_cache_store(
&cache, 5u, &prn4, 0u));
assert(!gps_ephemeris_cache_store(
&cache, GPS_PRN_COUNT, &prn4, 0u));
assert(gps_ephemeris_cache_store(
&cache, 4u, &prn4, 20u));
assert(gps_ephemeris_cache_store(
&cache, 19u, &prn19, 30u));
assert(gps_ephemeris_cache_restore(
&cache, 19u, 40u, &output));
assert(output.iode == 44u);
gps_ephemeris_cache_invalidate(&cache, 4u);
assert(!gps_ephemeris_cache_restore(
&cache, 4u, 40u, &output));
assert(gps_ephemeris_cache_restore(
&cache, 19u, 40u, &output));
}
int main(void)
{
test_store_restore_and_expiry();
test_validation_independence_and_invalidate();
puts("gps ephemeris cache tests passed");
return 0;
}
@@ -0,0 +1,319 @@
#include <assert.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include "gps/gps_acq_manager.h"
#include "gps/gps_ephemeris_cache.h"
#define SIM_SEARCH_TICKS 5u
#define SIM_UNLOCK_TICKS 10u
#define SIM_NAV_SUBFRAME_TICKS 6u
typedef struct {
gps_acq_manager_t manager;
gps_ephemeris_cache_t cache;
bool visible[GPS_ACQ_MANAGER_PRN_COUNT];
bool tracked[GPS_ACQ_MANAGER_PRN_COUNT];
uint64_t invisible_since[GPS_ACQ_MANAGER_PRN_COUNT];
bool engine_busy;
bool drop_completion_irq;
unsigned int engine_prn;
uint64_t engine_start;
uint64_t tick;
unsigned int polled_completions;
unsigned int cache_restores;
} receiver_sim_t;
static unsigned int tracked_count(const receiver_sim_t* sim)
{
unsigned int count = 0u;
for(unsigned int prn = 0u;
prn < GPS_ACQ_MANAGER_PRN_COUNT;
prn++)
{
count += sim->tracked[prn] ? 1u : 0u;
}
return count;
}
static void sim_init(receiver_sim_t* sim)
{
memset(sim, 0, sizeof(*sim));
const gps_acq_manager_config_t config = {
.retry_delay_ticks = 1u,
.hot_retry_delay_ticks = 30u,
.hot_window_ticks = 7200u,
.max_hot_streak = 4u
};
gps_acq_manager_init(&sim->manager, &config);
gps_ephemeris_cache_init(&sim->cache, 7200u);
for(unsigned int prn = 0u;
prn < GPS_ACQ_MANAGER_PRN_COUNT;
prn++)
{
assert(gps_acq_manager_enable_cold(
&sim->manager, prn));
}
sim->engine_prn = GPS_ACQ_MANAGER_INVALID_PRN;
}
static void cache_ephemeris(receiver_sim_t* sim, unsigned int prn)
{
gps_ephemeris_t ephemeris;
memset(&ephemeris, 0, sizeof(ephemeris));
ephemeris.SVID = (unsigned char)prn;
ephemeris.iode = (unsigned char)(prn + 10u);
ephemeris.ephemeris_valid = true;
assert(gps_ephemeris_cache_store(
&sim->cache, prn, &ephemeris, sim->tick));
}
static void sim_start_engine(receiver_sim_t* sim)
{
if(sim->engine_busy)
{
return;
}
unsigned int prn;
if(gps_acq_manager_start_next(
&sim->manager, sim->tick, &prn, NULL))
{
sim->engine_busy = true;
sim->engine_prn = prn;
sim->engine_start = sim->tick;
}
}
static void sim_complete_engine(receiver_sim_t* sim)
{
unsigned int prn = sim->engine_prn;
sim->engine_busy = false;
sim->engine_prn = GPS_ACQ_MANAGER_INVALID_PRN;
if(sim->drop_completion_irq)
{
/* The firmware observes BUSY falling even if the PLIC edge is lost. */
sim->polled_completions++;
}
if(sim->visible[prn])
{
gps_acq_manager_mark_acquired(&sim->manager, prn);
gps_acq_manager_mark_tracking(&sim->manager, prn);
sim->tracked[prn] = true;
sim->invisible_since[prn] = 0u;
gps_ephemeris_t restored;
if(gps_ephemeris_cache_restore(
&sim->cache, prn, sim->tick, &restored))
{
sim->cache_restores++;
}
else
{
cache_ephemeris(sim, prn);
}
}
else
{
gps_acq_manager_mark_failed(
&sim->manager, prn, sim->tick);
}
}
static void sim_update_tracking(receiver_sim_t* sim)
{
for(unsigned int prn = 0u;
prn < GPS_ACQ_MANAGER_PRN_COUNT;
prn++)
{
if(!sim->tracked[prn])
{
continue;
}
if(sim->visible[prn])
{
sim->invisible_since[prn] = 0u;
}
else if(sim->invisible_since[prn] == 0u)
{
sim->invisible_since[prn] = sim->tick;
}
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);
}
}
}
static void sim_step(receiver_sim_t* sim)
{
sim->tick++;
sim_update_tracking(sim);
if(sim->engine_busy)
{
uint64_t age = sim->tick - sim->engine_start;
if(age >= SIM_SEARCH_TICKS)
{
sim_complete_engine(sim);
}
}
sim_start_engine(sim);
}
static void sim_run(receiver_sim_t* sim, unsigned int ticks)
{
for(unsigned int i = 0u ; i < ticks ; i++)
{
sim_step(sim);
}
}
static void test_cold_boot_and_short_signal_gap(void)
{
receiver_sim_t sim;
sim_init(&sim);
sim.visible[0] = true;
sim.visible[1] = true;
sim.visible[2] = true;
sim.visible[3] = true;
sim_start_engine(&sim);
while((tracked_count(&sim) < 4u) && (sim.tick < 30u))
{
sim_step(&sim);
}
assert(tracked_count(&sim) == 4u);
assert(sim.tick <= 21u);
memset(sim.visible, 0, sizeof(sim.visible));
sim_run(&sim, SIM_UNLOCK_TICKS - 1u);
assert(tracked_count(&sim) == 4u);
for(unsigned int prn = 0u ; prn < 4u ; prn++)
{
sim.visible[prn] = true;
}
sim_run(&sim, 2u);
assert(tracked_count(&sim) == 4u);
}
static void test_power_on_without_signal_then_visibility(void)
{
receiver_sim_t sim;
sim_init(&sim);
sim_start_engine(&sim);
/* One complete 32-PRN scan with no RF signal must keep progressing. */
sim_run(&sim, GPS_ACQ_MANAGER_PRN_COUNT * SIM_SEARCH_TICKS);
assert(tracked_count(&sim) == 0u);
assert(sim.engine_busy);
sim.visible[29] = true;
uint64_t signal_return_tick = sim.tick;
while(!sim.tracked[29] &&
((sim.tick - signal_return_tick) <=
(GPS_ACQ_MANAGER_PRN_COUNT * SIM_SEARCH_TICKS)))
{
sim_step(&sim);
}
assert(sim.tracked[29]);
assert((sim.tick - signal_return_tick) <=
(GPS_ACQ_MANAGER_PRN_COUNT * SIM_SEARCH_TICKS));
}
static void test_long_outage_hot_reacquisition_and_cached_nav(void)
{
receiver_sim_t sim;
sim_init(&sim);
for(unsigned int prn = 0u ; prn < 4u ; prn++)
{
sim.visible[prn] = true;
}
sim_start_engine(&sim);
while(tracked_count(&sim) < 4u)
{
sim_step(&sim);
}
unsigned int cold_fix_tick = (unsigned int)sim.tick;
memset(sim.visible, 0, sizeof(sim.visible));
sim_run(&sim, SIM_UNLOCK_TICKS + 120u);
assert(tracked_count(&sim) == 0u);
unsigned int return_tick = (unsigned int)sim.tick;
for(unsigned int prn = 0u ; prn < 4u ; prn++)
{
sim.visible[prn] = true;
}
while((tracked_count(&sim) < 4u) &&
((sim.tick - return_tick) < 60u))
{
sim_step(&sim);
}
assert(tracked_count(&sim) == 4u);
assert((sim.tick - return_tick) <= 45u);
assert(sim.cache_restores >= 4u);
/* Cached ephemeris leaves only the next valid LNAV subframe/HOW anchor
* before the channel can contribute to a coherent WLS epoch. */
assert((sim.tick + SIM_NAV_SUBFRAME_TICKS - return_tick) <= 51u);
assert(cold_fix_tick <= 21u);
}
static void test_hot_failures_do_not_starve_cold_search(void)
{
gps_acq_manager_t manager;
const gps_acq_manager_config_t config = {
.retry_delay_ticks = 1u,
.hot_retry_delay_ticks = 1u,
.hot_window_ticks = 100u,
.max_hot_streak = 4u
};
gps_acq_manager_init(&manager, &config);
assert(gps_acq_manager_enable_cold(&manager, 0u));
gps_acq_manager_mark_lost(&manager, 20u, 0u);
gps_acq_manager_mark_lost(&manager, 21u, 0u);
bool cold_seen = false;
for(uint64_t tick = 0u ; tick < 6u ; tick++)
{
unsigned int prn;
bool hot;
assert(gps_acq_manager_start_next(
&manager, tick, &prn, &hot));
if(!hot)
{
assert(prn == 0u);
cold_seen = true;
break;
}
gps_acq_manager_mark_failed(
&manager, prn, tick + 1u);
}
assert(cold_seen);
}
static void test_missed_completion_interrupt_is_polled(void)
{
receiver_sim_t sim;
sim_init(&sim);
sim.visible[0] = true;
sim.drop_completion_irq = true;
sim_start_engine(&sim);
assert(sim.engine_prn == 0u);
sim_run(&sim, SIM_SEARCH_TICKS);
assert(sim.polled_completions == 1u);
assert(sim.tracked[0]);
assert(gps_acq_manager_state(&sim.manager, 0u) ==
GPS_ACQ_PRN_TRACKING);
}
int main(void)
{
test_cold_boot_and_short_signal_gap();
test_power_on_without_signal_then_visibility();
test_long_outage_hot_reacquisition_and_cached_nav();
test_hot_failures_do_not_starve_cold_search();
test_missed_completion_interrupt_is_polled();
puts("gps receiver hardware-simulation scenarios passed");
return 0;
}