From 9ff895600b8ceb9b0e0cc6f9f6acd2febf55e229 Mon Sep 17 00:00:00 2001 From: onur Date: Thu, 23 Jul 2026 23:20:23 +0300 Subject: [PATCH] Keep fused location streaming through GPS rejects --- soc/software/Makefile | 6 +- .../tests/platform_scenario_matrix_test.c | 42 ++++++++++++-- soc/software/gps_system/Makefile | 6 +- soc/software/gps_system/gps_system.c | 58 +++++++++++++------ .../header/adapters/gps_system_uart.h | 3 + .../header/fusion/navigation_fusion.h | 1 + .../gps_system/header/gps/gps_ip_defines.h | 4 +- soc/software/gps_system/header/gps_system.h | 1 + .../gps_system/src/adapters/gps_system_uart.c | 29 ++++++++++ .../gps_system/src/fusion/navigation_fusion.c | 2 + .../src/gps/gps_interrupt_handlers.c | 3 +- soc/software/gps_system/src/gps/gps_ip.c | 26 ++++----- soc/software/gps_system/src/imu/imu_sensor.c | 7 ++- .../gps_system/tests/test_gps_nav_epoch.c | 31 ++++++++++ 14 files changed, 173 insertions(+), 46 deletions(-) diff --git a/soc/software/Makefile b/soc/software/Makefile index 1da3c51..e49f758 100644 --- a/soc/software/Makefile +++ b/soc/software/Makefile @@ -13,7 +13,9 @@ GPS_DEBUG ?= 1 GPS_DEBUG_LEVEL ?= 4 BSP_UART_CLOCK_HZ ?= 100000000 BSP_UART_BAUDRATE ?= 115200 +GPS_SYSTEM_UART_BAUDRATE ?= 460800 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_BASE ?= 0x80000000 BSP_DTIM_SIZE ?= 0x00100000 @@ -38,7 +40,9 @@ help: @echo " APP_FLASH_OFFSET=$(APP_FLASH_OFFSET)" @echo " BSP_UART_CLOCK_HZ=$(BSP_UART_CLOCK_HZ)" @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 " GPS_SYSTEM_CLINT_TIMEBASE_HZ=$(GPS_SYSTEM_CLINT_TIMEBASE_HZ)" @echo " BSP_DTIM_BASE=$(BSP_DTIM_BASE)" @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 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: @test -d "$(APP)" diff --git a/soc/software/fusion/tests/platform_scenario_matrix_test.c b/soc/software/fusion/tests/platform_scenario_matrix_test.c index 03ec94a..9fe0719 100644 --- a/soc/software/fusion/tests/platform_scenario_matrix_test.c +++ b/soc/software/fusion/tests/platform_scenario_matrix_test.c @@ -474,14 +474,14 @@ static void print_metrics(scenario_kind_t scenario, unsigned int gps_ms, static int run_matrix(void) { 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; 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 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, 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"); 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"); return 0; } @@ -512,6 +512,33 @@ static int run_matrix(void) int main(int argc, char** argv) { 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)) { 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 imu_ms = (unsigned int)strtoul(argv[4], NULL, 10); 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))) { - 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; } FILE* log_file = fopen(argv[5], "w"); @@ -546,6 +573,9 @@ int main(int argc, char** argv) return metrics.finite ? 0 : 1; } 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; } diff --git a/soc/software/gps_system/Makefile b/soc/software/gps_system/Makefile index 764821f..2ec82d0 100644 --- a/soc/software/gps_system/Makefile +++ b/soc/software/gps_system/Makefile @@ -8,9 +8,11 @@ GPS_DEBUG ?= 1 GPS_DEBUG_LEVEL ?= 4 # The current Genesys2 Rocket/fabric domain that clocks the UART is 50 MHz. -# Keep the requested 115200 baud physical rather than producing 57600 baud -# with the common 100 MHz fallback divider. +# The 50 Hz fused-location stream plus GPS diagnostics does not fit safely in +# the polling UART budget at 115200 baud. 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. # Accepted values: auto, tdk, st (stm is an alias for st). diff --git a/soc/software/gps_system/gps_system.c b/soc/software/gps_system/gps_system.c index 4664130..1e4236d 100644 --- a/soc/software/gps_system/gps_system.c +++ b/soc/software/gps_system/gps_system.c @@ -18,6 +18,9 @@ #error "GPS_ESKF_ENABLE must be 0 or 1" #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) { 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. */ } -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; } unsigned int processed = 0u; 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++; } return processed; @@ -78,21 +95,33 @@ int main(void) system.fusion_enabled, system.fusion_initialized, system.imu_initialized, system.imu_running); + bool imu_backlog_may_remain = false; for(;;) { - /* GPS, IMU data-ready and the maintenance timer wake this loop. */ - asm volatile("wfi"); + /* 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. */ + asm volatile("wfi"); + } - /* IMU prediction runs at the sensor's own timestamped rate. */ - (void)feed_pending_imu_to_eskf(&system); + /* Limit pre-GPS work to one sample for low event latency. */ + (void)feed_pending_imu_to_eskf( + &system, IMU_SAMPLES_BEFORE_GPS_SERVICE); /* GPS remains an independent receiver: events first, then coherent WLS. */ gps_receiver_process_pending_events(); system.gps_epoch_ready = gps_receiver_solve_ready_epoch( &system.gps_solution, &system.gps_measurement_ticks); - /* WLS may take long enough for more IMU samples to arrive. */ - (void)feed_pending_imu_to_eskf(&system); + /* WLS may take long enough for more IMU samples to arrive. Drain a + * 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) { @@ -114,15 +143,6 @@ int main(void) /* Delayed GPS update is applied at its capture timestamp; newer * IMU history is replayed to produce the current fused state. */ (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); - } } } diff --git a/soc/software/gps_system/header/adapters/gps_system_uart.h b/soc/software/gps_system/header/adapters/gps_system_uart.h index 315bc95..2730c1c 100644 --- a/soc/software/gps_system/header/adapters/gps_system_uart.h +++ b/soc/software/gps_system/header/adapters/gps_system_uart.h @@ -14,6 +14,9 @@ void gps_system_uart_publish_location( double height_m, 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( bool fusion_enabled, bool fusion_initialized, diff --git a/soc/software/gps_system/header/fusion/navigation_fusion.h b/soc/software/gps_system/header/fusion/navigation_fusion.h index ad5e5d0..8e695f2 100644 --- a/soc/software/gps_system/header/fusion/navigation_fusion.h +++ b/soc/software/gps_system/header/fusion/navigation_fusion.h @@ -11,6 +11,7 @@ typedef struct { uint64_t timestamp_ticks; + uint64_t last_accepted_gps_ticks; double position_enu_m[3]; double velocity_enu_mps[3]; double latitude_deg; diff --git a/soc/software/gps_system/header/gps/gps_ip_defines.h b/soc/software/gps_system/header/gps/gps_ip_defines.h index dc91e8b..3ffa8bd 100644 --- a/soc/software/gps_system/header/gps/gps_ip_defines.h +++ b/soc/software/gps_system/header/gps/gps_ip_defines.h @@ -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_INTR_LINE(index) (BSP_GPS_ACQ_IRQ + ((unsigned int)(index) * BSP_GPS_ACQ_IRQ_STRIDE)) #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 #ifndef GPS_NAV_SOLVE_TIMER_TICKS #define GPS_NAV_SOLVE_TIMER_TICKS 50u diff --git a/soc/software/gps_system/header/gps_system.h b/soc/software/gps_system/header/gps_system.h index a438b9a..a59e652 100644 --- a/soc/software/gps_system/header/gps_system.h +++ b/soc/software/gps_system/header/gps_system.h @@ -17,6 +17,7 @@ typedef struct { imu_sample_t imu_sample; navigation_fusion_output_t fused_solution; uint64_t gps_measurement_ticks; + uint64_t last_fused_location_tick; uint64_t last_status_tick; bool gps_epoch_ready; bool fusion_enabled; diff --git a/soc/software/gps_system/src/adapters/gps_system_uart.c b/soc/software/gps_system/src/adapters/gps_system_uart.c index 0848011..d58faa7 100644 --- a/soc/software/gps_system/src/adapters/gps_system_uart.c +++ b/soc/software/gps_system/src/adapters/gps_system_uart.c @@ -7,6 +7,8 @@ #include "gps/gps_receiver.h" #include "imu/imu_sensor.h" +#define GPS_AIDED_MAX_AGE_MS 1000u + void gps_system_uart_publish_location( const char* source, double latitude_deg, @@ -27,6 +29,33 @@ void gps_system_uart_publish_location( 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( bool fusion_enabled, bool fusion_initialized, diff --git a/soc/software/gps_system/src/fusion/navigation_fusion.c b/soc/software/gps_system/src/fusion/navigation_fusion.c index 6918a10..f5c5abf 100644 --- a/soc/software/gps_system/src/fusion/navigation_fusion.c +++ b/soc/software/gps_system/src/fusion/navigation_fusion.c @@ -123,6 +123,8 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output) } const fusion_runtime_t* runtime = &fusion_engine.current; 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, sizeof(output->position_enu_m)); memcpy(output->velocity_enu_mps, runtime->filter.velocity_mps, diff --git a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c index 0c39ff8..863fdac 100644 --- a/soc/software/gps_system/src/gps/gps_interrupt_handlers.c +++ b/soc/software/gps_system/src/gps/gps_interrupt_handlers.c @@ -114,9 +114,10 @@ static uint32_t gps_nav_ready_channel_mask(void) uint32_t status = trck_ips[i]->STATUS_REG; uint32_t control = trck_ips[i]->CONTROL_REG; bool busy = (status & (1u << GPS_TRCK_BUSY_POS)) != 0u; + bool locked = (status & (1u << GPS_TRCK_LOCK_POS)) != 0u; bool nav_irq_enabled = (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; } diff --git a/soc/software/gps_system/src/gps/gps_ip.c b/soc/software/gps_system/src/gps/gps_ip.c index 9b3d3a8..294b53a 100644 --- a/soc/software/gps_system/src/gps/gps_ip.c +++ b/soc/software/gps_system/src/gps/gps_ip.c @@ -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 count = 0; 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) { @@ -1099,11 +1099,11 @@ bool calculate_position(gps_nav_solution_t* solution_out, bool run_nav = false; bool initial_run = false; unsigned int ready_count; - unsigned int busy_count; + unsigned int tracked_count; unsigned int tracked_prns[GPS_TRCK_IP_COUNT]; unsigned int coherent_nav_count = 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_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( 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); 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(); - if((busy_count != last_nav_status_busy_count) || + if((tracked_count != last_nav_status_tracked_count) || (ready_count != last_nav_status_ready_count) || (coherent_nav_count != last_nav_status_coherent_count) || ((now - last_nav_status_log_tick) >= GPS_NAV_STATUS_LOG_PERIOD_TICKS)) { - GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV, "busy channels=%u ready channels=%u coherent_nav=%u\r\n", - busy_count, ready_count, coherent_nav_count); - last_nav_status_busy_count = busy_count; + GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV, "tracked channels=%u ready channels=%u coherent_nav=%u\r\n", + tracked_count, ready_count, coherent_nav_count); + last_nav_status_tracked_count = tracked_count; last_nav_status_ready_count = ready_count; last_nav_status_coherent_count = coherent_nav_count; last_nav_status_log_tick = now; } - (void)busy_count; + (void)tracked_count; 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; 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( snapshot_local_timestamps, satellite_count); if(measurement_timestamp != 0u) @@ -1304,8 +1304,8 @@ bool calculate_position(gps_nav_solution_t* solution_out, if(GPS_DEBUG_ENABLED(DEBUG_CHANNELS)) { GPS_LOG(DEBUG_CHANNELS, GPS_LOG_CAT_TRK, - "tracked satellites=%u prns=[", busy_count); - for(unsigned int i = 0 ; i < busy_count ; i++) + "tracked satellites=%u prns=[", tracked_count); + for(unsigned int i = 0 ; i < tracked_count ; i++) { PRINT_DEBUG(DEBUG_CHANNELS, "%s%u", (i == 0u) ? "" : ",", tracked_prns[i]); diff --git a/soc/software/gps_system/src/imu/imu_sensor.c b/soc/software/gps_system/src/imu/imu_sensor.c index 3dbae1c..291fb18 100644 --- a/soc/software/gps_system/src/imu/imu_sensor.c +++ b/soc/software/gps_system/src/imu/imu_sensor.c @@ -216,12 +216,13 @@ bool imu_sensor_init(const imu_sensor_config_t* config) #elif defined(IMU_SENSOR_BUILD_ST) return try_st(config); #else - /* WHO_AM_I probing is read-only until a matching backend is found. */ - if(try_tdk(config)) + /* WHO_AM_I probing is read-only until a matching backend is found. + * Prefer the Genesys2 STEVAL/LSM6DSOX path when both devices respond. */ + if(try_st(config)) { return true; } - return try_st(config); + return try_tdk(config); #endif } diff --git a/soc/software/gps_system/tests/test_gps_nav_epoch.c b/soc/software/gps_system/tests/test_gps_nav_epoch.c index eaf89d1..7907f25 100644 --- a/soc/software/gps_system/tests/test_gps_nav_epoch.c +++ b/soc/software/gps_system/tests/test_gps_nav_epoch.c @@ -83,6 +83,36 @@ static void assembly_waits_for_every_ready_channel(void) "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) { gps_nav_epoch_assembly_t assembly; @@ -458,6 +488,7 @@ int main(void) { epoch_id_rounding_and_week_wrap(); assembly_waits_for_every_ready_channel(); + busy_unlocked_channel_does_not_block_epoch(); assembly_handles_duplicates_and_epoch_order(); assembly_disconnect_and_reconnect(); channel_join_does_not_expand_active_epoch();