fix(gps): make acquisition and reacquisition deterministic
This commit is contained in:
@@ -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())
|
||||
{
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user