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

Keep fused location streaming through GPS rejects

Browse Source
This commit is contained in:
onur
2026-07-23 23:20:23 +03:00
parent fc242b2883
commit 9ff895600b
14 changed files with 173 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/Makefile
+5 -1
View File
@@ -13,7 +13,9 @@ GPS_DEBUG ?= 1
GPS_DEBUG_LEVEL ?= 4 GPS_DEBUG_LEVEL ?= 4
BSP_UART_CLOCK_HZ ?= 100000000 BSP_UART_CLOCK_HZ ?= 100000000
BSP_UART_BAUDRATE ?= 115200 BSP_UART_BAUDRATE ?= 115200
GPS_SYSTEM_UART_BAUDRATE ?= 460800
BSP_CLINT_TIMEBASE_HZ ?= 1000000 BSP_CLINT_TIMEBASE_HZ ?= 1000000
GPS_SYSTEM_CLINT_TIMEBASE_HZ ?= 500000
# BSP_DTIM_* mirrors the FPGA DTIM_* knobs and is used by C code plus linker scripts. # BSP_DTIM_* mirrors the FPGA DTIM_* knobs and is used by C code plus linker scripts.
BSP_DTIM_BASE ?= 0x80000000 BSP_DTIM_BASE ?= 0x80000000
BSP_DTIM_SIZE ?= 0x00100000 BSP_DTIM_SIZE ?= 0x00100000
@@ -38,7 +40,9 @@ help:
@echo " APP_FLASH_OFFSET=$(APP_FLASH_OFFSET)" @echo " APP_FLASH_OFFSET=$(APP_FLASH_OFFSET)"
@echo " BSP_UART_CLOCK_HZ=$(BSP_UART_CLOCK_HZ)" @echo " BSP_UART_CLOCK_HZ=$(BSP_UART_CLOCK_HZ)"
@echo " BSP_UART_BAUDRATE=$(BSP_UART_BAUDRATE)" @echo " BSP_UART_BAUDRATE=$(BSP_UART_BAUDRATE)"
@echo " GPS_SYSTEM_UART_BAUDRATE=$(GPS_SYSTEM_UART_BAUDRATE)"
@echo " BSP_CLINT_TIMEBASE_HZ=$(BSP_CLINT_TIMEBASE_HZ)" @echo " BSP_CLINT_TIMEBASE_HZ=$(BSP_CLINT_TIMEBASE_HZ)"
@echo " GPS_SYSTEM_CLINT_TIMEBASE_HZ=$(GPS_SYSTEM_CLINT_TIMEBASE_HZ)"
@echo " BSP_DTIM_BASE=$(BSP_DTIM_BASE)" @echo " BSP_DTIM_BASE=$(BSP_DTIM_BASE)"
@echo " BSP_DTIM_SIZE=$(BSP_DTIM_SIZE)" @echo " BSP_DTIM_SIZE=$(BSP_DTIM_SIZE)"
@@ -57,7 +61,7 @@ steval-mki217v1-test:
@make -C steval_mki217v1_test TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/steval_mki217v1_test" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all @make -C steval_mki217v1_test TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/steval_mki217v1_test" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
gps-system: gps-system:
@make -C gps_system TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/gps_system" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all @make -C gps_system TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/gps_system" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(GPS_SYSTEM_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(GPS_SYSTEM_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
app: app:
@test -d "$(APP)" @test -d "$(APP)"
soc/software/fusion/tests/platform_scenario_matrix_test.c
+36 -6
View File
@@ -474,14 +474,14 @@ static void print_metrics(scenario_kind_t scenario, unsigned int gps_ms,
static int run_matrix(void) static int run_matrix(void)
{ {
static const unsigned int gps_periods_ms[] = {200u, 500u, 1000u}; static const unsigned int gps_periods_ms[] = {200u, 500u, 1000u};
static const unsigned int imu_periods_ms[] = {19u, 20u, 200u, 300u}; static const unsigned int imu_periods_ms[] = {19u, 20u, 100u, 200u, 300u};
bool passed = true; bool passed = true;
printf("Four-satellite asynchronous platform matrix; 3 s outage and 2 s coherent GNSS fault\n"); printf("Four-satellite asynchronous platform matrix; 3 s outage and 2 s coherent GNSS fault\n");
for(unsigned int scenario = 0u; scenario < SCENARIO_COUNT; scenario++) for(unsigned int scenario = 0u; scenario < SCENARIO_COUNT; scenario++)
{ {
for(unsigned int gps_index = 0u; gps_index < 3u; gps_index++) for(unsigned int gps_index = 0u; gps_index < 3u; gps_index++)
{ {
for(unsigned int imu_index = 0u; imu_index < 4u; imu_index++) for(unsigned int imu_index = 0u; imu_index < 5u; imu_index++)
{ {
metrics_t metrics = run_case((scenario_kind_t)scenario, metrics_t metrics = run_case((scenario_kind_t)scenario,
gps_periods_ms[gps_index], imu_periods_ms[imu_index], NULL); gps_periods_ms[gps_index], imu_periods_ms[imu_index], NULL);
@@ -504,7 +504,7 @@ static int run_matrix(void)
fprintf(stderr, "FAIL: platform scenario matrix contract.\n"); fprintf(stderr, "FAIL: platform scenario matrix contract.\n");
return 1; return 1;
} }
printf("PASS: 19/20 ms production rates stayed bounded; 200/300 ms coarse IMU " printf("PASS: 19/20 ms production rates stayed bounded; 100/200/300 ms coarse IMU "
"rates remained finite and their dynamic accuracy loss is reported, not hidden.\n"); "rates remained finite and their dynamic accuracy loss is reported, not hidden.\n");
return 0; return 0;
} }
@@ -512,6 +512,33 @@ static int run_matrix(void)
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
if(argc == 1) return run_matrix(); if(argc == 1) return run_matrix();
if((argc == 6) && (strcmp(argv[1], "--benchmark") == 0))
{
scenario_kind_t scenario;
if(!parse_scenario(argv[2], &scenario))
{
fprintf(stderr, "unknown scenario: %s\n", argv[2]);
return 2;
}
unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10);
unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10);
unsigned int iterations = (unsigned int)strtoul(argv[5], NULL, 10);
if((iterations == 0u) ||
!((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) ||
!((imu_ms == 19u) || (imu_ms == 20u) || (imu_ms == 100u) ||
(imu_ms == 200u) || (imu_ms == 300u)))
{
fprintf(stderr, "supported GPS: 200/500/1000 ms; "
"IMU: 19/20/100/200/300 ms; iterations > 0\n");
return 2;
}
metrics_t metrics;
for(unsigned int iteration = 0u; iteration < iterations; iteration++)
metrics = run_case(scenario, gps_ms, imu_ms, NULL);
print_metrics(scenario, gps_ms, imu_ms, &metrics);
printf("completed %u benchmark iterations\n", iterations);
return metrics.finite ? 0 : 1;
}
if((argc == 6) && (strcmp(argv[1], "--log") == 0)) if((argc == 6) && (strcmp(argv[1], "--log") == 0))
{ {
scenario_kind_t scenario; scenario_kind_t scenario;
@@ -523,10 +550,10 @@ int main(int argc, char** argv)
unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10); unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10);
unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10); unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10);
if(!((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) || if(!((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) ||
!((imu_ms == 19u) || (imu_ms == 20u) || !((imu_ms == 19u) || (imu_ms == 20u) || (imu_ms == 100u) ||
(imu_ms == 200u) || (imu_ms == 300u))) (imu_ms == 200u) || (imu_ms == 300u)))
{ {
fprintf(stderr, "supported GPS: 200/500/1000 ms; IMU: 19/20/200/300 ms\n"); fprintf(stderr, "supported GPS: 200/500/1000 ms; IMU: 19/20/100/200/300 ms\n");
return 2; return 2;
} }
FILE* log_file = fopen(argv[5], "w"); FILE* log_file = fopen(argv[5], "w");
@@ -546,6 +573,9 @@ int main(int argc, char** argv)
return metrics.finite ? 0 : 1; return metrics.finite ? 0 : 1;
} }
fprintf(stderr, "usage: %s [--log stationary|pedestrian|car|aircraft " fprintf(stderr, "usage: %s [--log stationary|pedestrian|car|aircraft "
"200|500|1000 19|20|200|300 output.log]\n", argv[0]); "200|500|1000 19|20|100|200|300 output.log]\n"
" %s [--benchmark stationary|pedestrian|car|aircraft "
"200|500|1000 19|20|100|200|300 iterations]\n",
argv[0], argv[0]);
return 2; return 2;
} }
soc/software/gps_system/Makefile
+4 -2
View File
@@ -8,9 +8,11 @@ GPS_DEBUG ?= 1
GPS_DEBUG_LEVEL ?= 4 GPS_DEBUG_LEVEL ?= 4
# The current Genesys2 Rocket/fabric domain that clocks the UART is 50 MHz. # The current Genesys2 Rocket/fabric domain that clocks the UART is 50 MHz.
# Keep the requested 115200 baud physical rather than producing 57600 baud # The 50 Hz fused-location stream plus GPS diagnostics does not fit safely in
# with the common 100 MHz fallback divider. # the polling UART budget at 115200 baud.
BSP_UART_CLOCK_HZ ?= 50000000 BSP_UART_CLOCK_HZ ?= 50000000
BSP_UART_BAUDRATE ?= 460800
BSP_CLINT_TIMEBASE_HZ ?= 500000
# Select one sensor backend, or keep the default read-only WHO_AM_I probe. # Select one sensor backend, or keep the default read-only WHO_AM_I probe.
# Accepted values: auto, tdk, st (stm is an alias for st). # Accepted values: auto, tdk, st (stm is an alias for st).
soc/software/gps_system/gps_system.c
+37 -17
View File
@@ -18,6 +18,9 @@
#error "GPS_ESKF_ENABLE must be 0 or 1" #error "GPS_ESKF_ENABLE must be 0 or 1"
#endif #endif
#define IMU_SAMPLES_BEFORE_GPS_SERVICE 1u
#define IMU_SAMPLES_AFTER_GPS_SERVICE 3u
static void initialize_sensor_fusion(gps_system_state_t* system) static void initialize_sensor_fusion(gps_system_state_t* system)
{ {
system->fusion_enabled = (GPS_ESKF_ENABLE != 0); system->fusion_enabled = (GPS_ESKF_ENABLE != 0);
@@ -43,18 +46,32 @@ static void initialize_sensor_fusion(gps_system_state_t* system)
true); /* Current prototype is the PX4-equivalent unarmed bench mode. */ true); /* Current prototype is the PX4-equivalent unarmed bench mode. */
} }
static unsigned int feed_pending_imu_to_eskf(gps_system_state_t* system) static unsigned int feed_pending_imu_to_eskf(
gps_system_state_t* system,
unsigned int max_samples)
{ {
if(!system->fusion_initialized) if(!system->fusion_initialized || (max_samples == 0u))
{ {
return 0u; return 0u;
} }
unsigned int processed = 0u; unsigned int processed = 0u;
imu_sensor_service(); imu_sensor_service();
while(imu_sensor_take_measurement(&system->imu_sample)) while((processed < max_samples) &&
imu_sensor_take_measurement(&system->imu_sample))
{ {
(void)navigation_fusion_push_imu(&system->imu_sample); bool accepted = navigation_fusion_push_imu(&system->imu_sample);
if(accepted &&
navigation_fusion_get_output(&system->fused_solution) &&
system->fused_solution.global_valid &&
(system->fused_solution.timestamp_ticks !=
system->last_fused_location_tick))
{
gps_system_uart_publish_fused_location(
&system->fused_solution);
system->last_fused_location_tick =
system->fused_solution.timestamp_ticks;
}
processed++; processed++;
} }
return processed; return processed;
@@ -78,21 +95,33 @@ int main(void)
system.fusion_enabled, system.fusion_initialized, system.fusion_enabled, system.fusion_initialized,
system.imu_initialized, system.imu_running); system.imu_initialized, system.imu_running);
bool imu_backlog_may_remain = false;
for(;;) for(;;)
{
/* Skip WFI only while draining a bounded IMU backlog. Every batch
* still services GPS before more prediction work, so a continuously
* non-empty IMU queue cannot starve navigation epochs. */
if(!imu_backlog_may_remain)
{ {
/* GPS, IMU data-ready and the maintenance timer wake this loop. */ /* GPS, IMU data-ready and the maintenance timer wake this loop. */
asm volatile("wfi"); asm volatile("wfi");
}
/* IMU prediction runs at the sensor's own timestamped rate. */ /* Limit pre-GPS work to one sample for low event latency. */
(void)feed_pending_imu_to_eskf(&system); (void)feed_pending_imu_to_eskf(
&system, IMU_SAMPLES_BEFORE_GPS_SERVICE);
/* GPS remains an independent receiver: events first, then coherent WLS. */ /* GPS remains an independent receiver: events first, then coherent WLS. */
gps_receiver_process_pending_events(); gps_receiver_process_pending_events();
system.gps_epoch_ready = gps_receiver_solve_ready_epoch( system.gps_epoch_ready = gps_receiver_solve_ready_epoch(
&system.gps_solution, &system.gps_measurement_ticks); &system.gps_solution, &system.gps_measurement_ticks);
/* WLS may take long enough for more IMU samples to arrive. */ /* WLS may take long enough for more IMU samples to arrive. Drain a
(void)feed_pending_imu_to_eskf(&system); * small batch, then return to GPS service before continuing. */
unsigned int imu_after_gps = feed_pending_imu_to_eskf(
&system, IMU_SAMPLES_AFTER_GPS_SERVICE);
imu_backlog_may_remain =
(imu_after_gps == IMU_SAMPLES_AFTER_GPS_SERVICE);
if(system.gps_epoch_ready && system.gps_solution.solution_valid) if(system.gps_epoch_ready && system.gps_solution.solution_valid)
{ {
@@ -114,15 +143,6 @@ int main(void)
/* Delayed GPS update is applied at its capture timestamp; newer /* Delayed GPS update is applied at its capture timestamp; newer
* IMU history is replayed to produce the current fused state. */ * IMU history is replayed to produce the current fused state. */
(void)navigation_fusion_push_gps(&system.gps_for_fusion); (void)navigation_fusion_push_gps(&system.gps_for_fusion);
if(navigation_fusion_get_output(&system.fused_solution) &&
system.fused_solution.global_valid)
{
gps_system_uart_publish_location("imu_corrected",
system.fused_solution.latitude_deg,
system.fused_solution.longitude_deg,
system.fused_solution.height_m,
&system.gps_solution);
}
} }
} }
soc/software/gps_system/header/adapters/gps_system_uart.h
+3
View File
@@ -14,6 +14,9 @@ void gps_system_uart_publish_location(
double height_m, double height_m,
const gps_nav_solution_t* gps); const gps_nav_solution_t* gps);
void gps_system_uart_publish_fused_location(
const navigation_fusion_output_t* output);
void gps_system_uart_publish_startup( void gps_system_uart_publish_startup(
bool fusion_enabled, bool fusion_enabled,
bool fusion_initialized, bool fusion_initialized,
soc/software/gps_system/header/fusion/navigation_fusion.h
+1
View File
@@ -11,6 +11,7 @@
typedef struct { typedef struct {
uint64_t timestamp_ticks; uint64_t timestamp_ticks;
uint64_t last_accepted_gps_ticks;
double position_enu_m[3]; double position_enu_m[3];
double velocity_enu_mps[3]; double velocity_enu_mps[3];
double latitude_deg; double latitude_deg;
soc/software/gps_system/header/gps/gps_ip_defines.h
+3 -1
View File
@@ -43,7 +43,9 @@
#define GPS_ACQ_IP_BASE_ADDR(index) (BSP_GPS_ACQ_BASE + ((uintptr_t)(index) * BSP_GPS_ACQ_STRIDE)) #define GPS_ACQ_IP_BASE_ADDR(index) (BSP_GPS_ACQ_BASE + ((uintptr_t)(index) * BSP_GPS_ACQ_STRIDE))
#define GPS_ACQ_INTR_LINE(index) (BSP_GPS_ACQ_IRQ + ((unsigned int)(index) * BSP_GPS_ACQ_IRQ_STRIDE)) #define GPS_ACQ_INTR_LINE(index) (BSP_GPS_ACQ_IRQ + ((unsigned int)(index) * BSP_GPS_ACQ_IRQ_STRIDE))
#define GPS_EVENT_QUEUE_SIZE 64u #define GPS_EVENT_QUEUE_SIZE 64u
#define GPS_SYSTICK_HZ 20u /* Genesys2 CLINT is 500 kHz. Keep foreground maintenance at
* 10 Hz; the tracking IP's 20 ms NAV timer is independent. */
#define GPS_SYSTICK_HZ 10u
#define GPS_NAV_SOLVE_TIMER_TICK_MS 20u #define GPS_NAV_SOLVE_TIMER_TICK_MS 20u
#ifndef GPS_NAV_SOLVE_TIMER_TICKS #ifndef GPS_NAV_SOLVE_TIMER_TICKS
#define GPS_NAV_SOLVE_TIMER_TICKS 50u #define GPS_NAV_SOLVE_TIMER_TICKS 50u
soc/software/gps_system/header/gps_system.h
+1
View File
@@ -17,6 +17,7 @@ typedef struct {
imu_sample_t imu_sample; imu_sample_t imu_sample;
navigation_fusion_output_t fused_solution; navigation_fusion_output_t fused_solution;
uint64_t gps_measurement_ticks; uint64_t gps_measurement_ticks;
uint64_t last_fused_location_tick;
uint64_t last_status_tick; uint64_t last_status_tick;
bool gps_epoch_ready; bool gps_epoch_ready;
bool fusion_enabled; bool fusion_enabled;
soc/software/gps_system/src/adapters/gps_system_uart.c
+29
View File
@@ -7,6 +7,8 @@
#include "gps/gps_receiver.h" #include "gps/gps_receiver.h"
#include "imu/imu_sensor.h" #include "imu/imu_sensor.h"
#define GPS_AIDED_MAX_AGE_MS 1000u
void gps_system_uart_publish_location( void gps_system_uart_publish_location(
const char* source, const char* source,
double latitude_deg, double latitude_deg,
@@ -27,6 +29,33 @@ void gps_system_uart_publish_location(
gps->tracked_satellite_count, gps->used_satellite_count); gps->tracked_satellite_count, gps->used_satellite_count);
} }
void gps_system_uart_publish_fused_location(
const navigation_fusion_output_t* output)
{
if((output == NULL) || !output->global_valid)
{
return;
}
uint64_t age_ticks = output->timestamp_ticks >= output->last_accepted_gps_ticks
? output->timestamp_ticks - output->last_accepted_gps_ticks : 0u;
uint64_t age_ms_wide =
age_ticks / (BSP_CLINT_TIMEBASE_HZ / 1000u);
uint32_t age_ms = age_ms_wide > UINT32_MAX
? UINT32_MAX : (uint32_t)age_ms_wide;
const char* mode =
(output->gps_accepted > 0u) && (age_ms <= GPS_AIDED_MAX_AGE_MS)
? "gps_aided" : "dead_reckoning";
/* At 50 Hz this compact record uses about 14% of the 460800-baud link,
* leaving headroom for navigation work and lower-rate diagnostics. */
GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
"source=imu_corrected mode=%s lat=%.9lf lon=%.9lf "
"height=%.2lf gps_age_ms=%lu\r\n",
mode, output->latitude_deg, output->longitude_deg,
output->height_m, (unsigned long)age_ms);
}
void gps_system_uart_publish_startup( void gps_system_uart_publish_startup(
bool fusion_enabled, bool fusion_enabled,
bool fusion_initialized, bool fusion_initialized,
soc/software/gps_system/src/fusion/navigation_fusion.c
+2
View File
@@ -123,6 +123,8 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output)
} }
const fusion_runtime_t* runtime = &fusion_engine.current; const fusion_runtime_t* runtime = &fusion_engine.current;
output->timestamp_ticks = runtime->filter_timestamp_ticks; output->timestamp_ticks = runtime->filter_timestamp_ticks;
output->last_accepted_gps_ticks =
runtime->last_accepted_position_ticks;
memcpy(output->position_enu_m, runtime->filter.position_m, memcpy(output->position_enu_m, runtime->filter.position_m,
sizeof(output->position_enu_m)); sizeof(output->position_enu_m));
memcpy(output->velocity_enu_mps, runtime->filter.velocity_mps, memcpy(output->velocity_enu_mps, runtime->filter.velocity_mps,
soc/software/gps_system/src/gps/gps_interrupt_handlers.c
+2 -1
View File
@@ -114,9 +114,10 @@ static uint32_t gps_nav_ready_channel_mask(void)
uint32_t status = trck_ips[i]->STATUS_REG; uint32_t status = trck_ips[i]->STATUS_REG;
uint32_t control = trck_ips[i]->CONTROL_REG; uint32_t control = trck_ips[i]->CONTROL_REG;
bool busy = (status & (1u << GPS_TRCK_BUSY_POS)) != 0u; bool busy = (status & (1u << GPS_TRCK_BUSY_POS)) != 0u;
bool locked = (status & (1u << GPS_TRCK_LOCK_POS)) != 0u;
bool nav_irq_enabled = bool nav_irq_enabled =
(control & (1u << GPS_TRCK_NAV_SOLVE_INTR_EN_POS)) != 0u; (control & (1u << GPS_TRCK_NAV_SOLVE_INTR_EN_POS)) != 0u;
if(tracking_allocated[i] && busy && nav_irq_enabled) if(tracking_allocated[i] && busy && locked && nav_irq_enabled)
{ {
ready_mask |= 1u << i; ready_mask |= 1u << i;
} }
soc/software/gps_system/src/gps/gps_ip.c
+13 -13
View File
@@ -579,13 +579,13 @@ static void gps_enable_nav_solve_for_snapshot_locked(const unsigned char snapsho
} }
} }
static unsigned int gps_collect_busy_tracking_locked( static unsigned int gps_collect_tracked_channels_locked(
unsigned int tracked_prns[GPS_TRCK_IP_COUNT]) unsigned int tracked_prns[GPS_TRCK_IP_COUNT])
{ {
unsigned int count = 0; unsigned int count = 0;
for(unsigned char i = 0 ; i < GPS_TRCK_IP_COUNT ; i++) for(unsigned char i = 0 ; i < GPS_TRCK_IP_COUNT ; i++)
{ {
if(trck_ips[i]->STATUS_REG & (1u << GPS_TRCK_BUSY_POS)) if(gps_tracking_ip_alive(i))
{ {
if(tracked_prns != NULL) if(tracked_prns != NULL)
{ {
@@ -1099,11 +1099,11 @@ bool calculate_position(gps_nav_solution_t* solution_out,
bool run_nav = false; bool run_nav = false;
bool initial_run = false; bool initial_run = false;
unsigned int ready_count; unsigned int ready_count;
unsigned int busy_count; unsigned int tracked_count;
unsigned int tracked_prns[GPS_TRCK_IP_COUNT]; unsigned int tracked_prns[GPS_TRCK_IP_COUNT];
unsigned int coherent_nav_count = 0; unsigned int coherent_nav_count = 0;
static uint64_t last_nav_status_log_tick = 0; static uint64_t last_nav_status_log_tick = 0;
static unsigned int last_nav_status_busy_count = GPS_TRCK_IP_COUNT + 1u; static unsigned int last_nav_status_tracked_count = GPS_TRCK_IP_COUNT + 1u;
static unsigned int last_nav_status_ready_count = GPS_TRCK_IP_COUNT + 1u; static unsigned int last_nav_status_ready_count = GPS_TRCK_IP_COUNT + 1u;
static unsigned int last_nav_status_coherent_count = GPS_TRCK_IP_COUNT + 1u; static unsigned int last_nav_status_coherent_count = GPS_TRCK_IP_COUNT + 1u;
@@ -1117,7 +1117,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
ready_count = gps_collect_nav_snapshot_locked( ready_count = gps_collect_nav_snapshot_locked(
snapshot, snapshot_ips, NULL, false, false); snapshot, snapshot_ips, NULL, false, false);
busy_count = gps_collect_busy_tracking_locked( tracked_count = gps_collect_tracked_channels_locked(
GPS_DEBUG_ENABLED(DEBUG_CHANNELS) ? tracked_prns : NULL); GPS_DEBUG_ENABLED(DEBUG_CHANNELS) ? tracked_prns : NULL);
if(ready_count < MIN_NEEDED_SATELLITE) if(ready_count < MIN_NEEDED_SATELLITE)
@@ -1197,19 +1197,19 @@ bool calculate_position(gps_nav_solution_t* solution_out,
} }
uint64_t now = gps_get_systick(); uint64_t now = gps_get_systick();
if((busy_count != last_nav_status_busy_count) || if((tracked_count != last_nav_status_tracked_count) ||
(ready_count != last_nav_status_ready_count) || (ready_count != last_nav_status_ready_count) ||
(coherent_nav_count != last_nav_status_coherent_count) || (coherent_nav_count != last_nav_status_coherent_count) ||
((now - last_nav_status_log_tick) >= GPS_NAV_STATUS_LOG_PERIOD_TICKS)) ((now - last_nav_status_log_tick) >= GPS_NAV_STATUS_LOG_PERIOD_TICKS))
{ {
GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV, "busy channels=%u ready channels=%u coherent_nav=%u\r\n", GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV, "tracked channels=%u ready channels=%u coherent_nav=%u\r\n",
busy_count, ready_count, coherent_nav_count); tracked_count, ready_count, coherent_nav_count);
last_nav_status_busy_count = busy_count; last_nav_status_tracked_count = tracked_count;
last_nav_status_ready_count = ready_count; last_nav_status_ready_count = ready_count;
last_nav_status_coherent_count = coherent_nav_count; last_nav_status_coherent_count = coherent_nav_count;
last_nav_status_log_tick = now; last_nav_status_log_tick = now;
} }
(void)busy_count; (void)tracked_count;
if(run_nav) if(run_nav)
{ {
@@ -1231,7 +1231,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles; uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles;
if(nav_ok) if(nav_ok)
{ {
nav_solution.tracked_satellite_count = busy_count; nav_solution.tracked_satellite_count = tracked_count;
uint64_t measurement_timestamp = gps_nav_median_local_timestamp( uint64_t measurement_timestamp = gps_nav_median_local_timestamp(
snapshot_local_timestamps, satellite_count); snapshot_local_timestamps, satellite_count);
if(measurement_timestamp != 0u) if(measurement_timestamp != 0u)
@@ -1304,8 +1304,8 @@ bool calculate_position(gps_nav_solution_t* solution_out,
if(GPS_DEBUG_ENABLED(DEBUG_CHANNELS)) if(GPS_DEBUG_ENABLED(DEBUG_CHANNELS))
{ {
GPS_LOG(DEBUG_CHANNELS, GPS_LOG_CAT_TRK, GPS_LOG(DEBUG_CHANNELS, GPS_LOG_CAT_TRK,
"tracked satellites=%u prns=[", busy_count); "tracked satellites=%u prns=[", tracked_count);
for(unsigned int i = 0 ; i < busy_count ; i++) for(unsigned int i = 0 ; i < tracked_count ; i++)
{ {
PRINT_DEBUG(DEBUG_CHANNELS, "%s%u", PRINT_DEBUG(DEBUG_CHANNELS, "%s%u",
(i == 0u) ? "" : ",", tracked_prns[i]); (i == 0u) ? "" : ",", tracked_prns[i]);
soc/software/gps_system/src/imu/imu_sensor.c
+4 -3
View File
@@ -216,12 +216,13 @@ bool imu_sensor_init(const imu_sensor_config_t* config)
#elif defined(IMU_SENSOR_BUILD_ST) #elif defined(IMU_SENSOR_BUILD_ST)
return try_st(config); return try_st(config);
#else #else
/* WHO_AM_I probing is read-only until a matching backend is found. */ /* WHO_AM_I probing is read-only until a matching backend is found.
if(try_tdk(config)) * Prefer the Genesys2 STEVAL/LSM6DSOX path when both devices respond. */
if(try_st(config))
{ {
return true; return true;
} }
return try_st(config); return try_tdk(config);
#endif #endif
} }
soc/software/gps_system/tests/test_gps_nav_epoch.c
+31
View File
@@ -83,6 +83,36 @@ static void assembly_waits_for_every_ready_channel(void)
"assembly lost a ready-channel interrupt"); "assembly lost a ready-channel interrupt");
} }
static void busy_unlocked_channel_does_not_block_epoch(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
/* Model the board failure: channels 0..8 are allocated and busy, but
* channel 4 has lost lock and therefore cannot produce this NAV epoch. */
const uint32_t allocated_busy_mask = 0x1ffu;
const uint32_t locked_mask = allocated_busy_mask & ~(1u << 4);
const uint32_t ready_mask = allocated_busy_mask & locked_mask;
for(unsigned int channel = 0u ; channel < 9u ; channel++)
{
if((ready_mask & (1u << channel)) == 0u)
{
continue;
}
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 1500u, channel, ready_mask);
bool last = channel == 8u;
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) == last,
"busy unlocked channel blocked a complete locked-channel epoch");
}
CHECK(assembly.expected_mask == 0x1efu,
"busy unlocked channel entered the ready mask");
CHECK(assembly.received_mask == assembly.expected_mask,
"locked channels did not complete the epoch");
}
static void assembly_handles_duplicates_and_epoch_order(void) static void assembly_handles_duplicates_and_epoch_order(void)
{ {
gps_nav_epoch_assembly_t assembly; gps_nav_epoch_assembly_t assembly;
@@ -458,6 +488,7 @@ int main(void)
{ {
epoch_id_rounding_and_week_wrap(); epoch_id_rounding_and_week_wrap();
assembly_waits_for_every_ready_channel(); assembly_waits_for_every_ready_channel();
busy_unlocked_channel_does_not_block_epoch();
assembly_handles_duplicates_and_epoch_order(); assembly_handles_duplicates_and_epoch_order();
assembly_disconnect_and_reconnect(); assembly_disconnect_and_reconnect();
channel_join_does_not_expand_active_epoch(); channel_join_does_not_expand_active_epoch();