diff --git a/soc/software/Makefile b/soc/software/Makefile index e49f758..5536a1f 100644 --- a/soc/software/Makefile +++ b/soc/software/Makefile @@ -16,6 +16,7 @@ BSP_UART_BAUDRATE ?= 115200 GPS_SYSTEM_UART_BAUDRATE ?= 460800 BSP_CLINT_TIMEBASE_HZ ?= 1000000 GPS_SYSTEM_CLINT_TIMEBASE_HZ ?= 500000 +GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES ?= 5 # BSP_DTIM_* mirrors the FPGA DTIM_* knobs and is used by C code plus linker scripts. BSP_DTIM_BASE ?= 0x80000000 BSP_DTIM_SIZE ?= 0x00100000 @@ -43,6 +44,7 @@ help: @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 " GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES=$(GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES)" @echo " BSP_DTIM_BASE=$(BSP_DTIM_BASE)" @echo " BSP_DTIM_SIZE=$(BSP_DTIM_SIZE)" @@ -61,7 +63,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="$(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 + @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)" IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES="$(GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all app: @test -d "$(APP)" diff --git a/soc/software/gps_system/Makefile b/soc/software/gps_system/Makefile index 2ec82d0..68866d2 100644 --- a/soc/software/gps_system/Makefile +++ b/soc/software/gps_system/Makefile @@ -44,6 +44,9 @@ endif IMU_ACCEL_FS_G ?= 0 IMU_GYRO_FS_DPS ?= 0 GPS_ESKF_ENABLE ?= 1 +# UART telemetry cadence only; ESKF prediction still runs for every IMU sample. +# At the default 50 Hz ODR, 5 samples produce one corrected record every 100 ms. +IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES ?= 5 APP_SRCS := \ gps_system.c \ @@ -81,6 +84,7 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \ -DIMU_ACCEL_FS_G=$(IMU_ACCEL_FS_G) \ -DIMU_GYRO_FS_DPS=$(IMU_GYRO_FS_DPS) \ -DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \ + -DIMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=$(IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES) \ -DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS) LDLIBS += -lm diff --git a/soc/software/gps_system/README.md b/soc/software/gps_system/README.md index ca5ef3d..b35a4a3 100644 --- a/soc/software/gps_system/README.md +++ b/soc/software/gps_system/README.md @@ -198,9 +198,9 @@ bagimsiz yuksek-ODR zamanlama gerekirse sensor FIFO timestamp'i kullanilmalidir. ### Level-1 konum UART protokolu -Her gecerli GPS navigation epoch'unda level 1'de iki konum satiri art arda -yayinlanir. Ilk satir WLS GPS konumu, ikinci satir o anda mevcut IMU+ESKF -duzeltilmis konumdur: +Her gecerli GPS navigation epoch'unda ham WLS GPS konumu yayinlanir. ESKF +duzeltilmis konumu ise GPS epoch'undan bagimsiz olarak IMU ornek sayaci +uzerinden yayinlanir: ```text [LOCATION] source=gps lat=... lon=... height=... gdop=... pdop=... hdop=... vdop=... tdop=... tracked=... used=... @@ -208,10 +208,28 @@ duzeltilmis konumdur: ``` `tracked`, donanimda calisan tracking kanal sayisini; `used`, ilgili WLS -cozumunde kullanilan uydu sayisini belirtir. DOP alanlari ayni GPS epoch'unun -geometrisidir ve iki satirda ayni tasinir. ESKF henuz kalibrasyon/origin -asamasindaysa yalniz GPS satiri yayinlanir; ESKF valid olduktan sonra her GPS -konumu geldiginde iki satir da yayinlanir. Eski `[NAV] position` ve periyodik +cozumunde kullanilan uydu sayisini belirtir. ESKF henuz kalibrasyon/origin +asamasindaysa yalniz GPS satiri yayinlanir. Origin kurulduktan sonra GPS +reddedilse veya gecici olarak kesilse de corrected konum IMU ile ilerlemeye +devam eder. + +Varsayilan corrected UART periyodu bes IMU ornegidir. 50 Hz sensor hedefinde +bu yaklasik 100 ms/10 Hz demektir. Bu bolucu yalniz UART logunu seyreltir; +sensor okuma ve ESKF prediction her IMU orneginde calismaya devam eder: + +```sh +# Varsayilan: 20 ms x 5 ~= 100 ms +make -C soc/software/gps_system IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=5 + +# 20 ms x 2 ~= 40 ms +make -C soc/software/gps_system IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=2 + +# Parent software hedefi uzerinden ayni ayar +make -C soc/software gps-system GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES=5 +``` + +Deger en az `1` olmalidir. Secilen bolucu startup satirinda +`publish_every=` alaniyla gorulur. Eski `[NAV] position` ve periyodik `[FUSION] lat/lon` ayrintilari daha yuksek debug seviyesindedir; level 1 parser'inin ayni konumu birden fazla kez gormesi engellenir. diff --git a/soc/software/gps_system/gps_system.c b/soc/software/gps_system/gps_system.c index 1e4236d..c9b1ae8 100644 --- a/soc/software/gps_system/gps_system.c +++ b/soc/software/gps_system/gps_system.c @@ -18,6 +18,14 @@ #error "GPS_ESKF_ENABLE must be 0 or 1" #endif +#ifndef IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES +#define IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES 5u +#endif + +#if (IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES < 1) +#error "IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES must be at least one" +#endif + #define IMU_SAMPLES_BEFORE_GPS_SERVICE 1u #define IMU_SAMPLES_AFTER_GPS_SERVICE 3u @@ -67,10 +75,20 @@ static unsigned int feed_pending_imu_to_eskf( (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; + system->fused_publish_sample_count++; + bool first_location = + (system->last_fused_location_tick == 0u); + bool publish_due = + (system->fused_publish_sample_count >= + IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES); + if(first_location || publish_due) + { + gps_system_uart_publish_fused_location( + &system->fused_solution); + system->last_fused_location_tick = + system->fused_solution.timestamp_ticks; + system->fused_publish_sample_count = 0u; + } } processed++; } @@ -93,7 +111,8 @@ int main(void) gps_receiver_start_acquisition(); gps_system_uart_publish_startup( system.fusion_enabled, system.fusion_initialized, - system.imu_initialized, system.imu_running); + system.imu_initialized, system.imu_running, + IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES); bool imu_backlog_may_remain = false; for(;;) 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 2730c1c..189e643 100644 --- a/soc/software/gps_system/header/adapters/gps_system_uart.h +++ b/soc/software/gps_system/header/adapters/gps_system_uart.h @@ -21,7 +21,8 @@ void gps_system_uart_publish_startup( bool fusion_enabled, bool fusion_initialized, bool imu_initialized, - bool imu_running); + bool imu_running, + unsigned int fused_publish_every_samples); void gps_system_uart_publish_periodic_fusion_status( navigation_fusion_output_t* output, diff --git a/soc/software/gps_system/header/gps_system.h b/soc/software/gps_system/header/gps_system.h index a59e652..0f3bdc4 100644 --- a/soc/software/gps_system/header/gps_system.h +++ b/soc/software/gps_system/header/gps_system.h @@ -19,6 +19,7 @@ typedef struct { uint64_t gps_measurement_ticks; uint64_t last_fused_location_tick; uint64_t last_status_tick; + uint32_t fused_publish_sample_count; bool gps_epoch_ready; bool fusion_enabled; bool imu_initialized; 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 d58faa7..d5abfe1 100644 --- a/soc/software/gps_system/src/adapters/gps_system_uart.c +++ b/soc/software/gps_system/src/adapters/gps_system_uart.c @@ -47,8 +47,8 @@ void gps_system_uart_publish_fused_location( (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. */ + /* The application rate-limits this compact record by IMU sample count, + * leaving UART headroom without reducing the ESKF prediction rate. */ 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", @@ -60,13 +60,15 @@ void gps_system_uart_publish_startup( bool fusion_enabled, bool fusion_initialized, bool imu_initialized, - bool imu_running) + bool imu_running, + unsigned int fused_publish_every_samples) { 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 " - "running=%u target_hz=%lu actual_hz=%lu nav_ms=%u timebase_hz=%lu\r\n", + "running=%u target_hz=%lu actual_hz=%lu publish_every=%u " + "nav_ms=%u timebase_hz=%lu\r\n", fusion_enabled ? 1u : 0u, fusion_initialized ? 1u : 0u, imu_initialized ? 1u : 0u, @@ -75,6 +77,7 @@ void gps_system_uart_publish_startup( imu_running ? 1u : 0u, (unsigned long)imu.requested_odr_hz, (unsigned long)imu.configured_odr_hz, + fused_publish_every_samples, GPS_NAV_SOLVE_PERIOD_MS, (unsigned long)BSP_CLINT_TIMEBASE_HZ); }