From dd6569c0250f3c378479a4bbc9488d3e46df1b8e Mon Sep 17 00:00:00 2001 From: onur Date: Tue, 14 Jul 2026 11:03:13 +0300 Subject: [PATCH] Add generic IMU fusion and PMOD IRQ verification --- .../constraints/genesys2_pmod_irq.xdc | 7 +- .../constraints/genesys2_pmod_pps.xdc | 2 +- hardware/sim/asic/rocket/tb/asic_rocket_tb.sv | 77 ++- hardware/sim/asic/rocket/wave.do | 5 + soc/software/fusion/tests/Makefile | 17 +- .../fusion/tests/imu_quality_synthetic_test.c | 418 +++++++++++++++ soc/software/gps_system/Makefile | 40 +- soc/software/gps_system/README.md | 118 ++++ soc/software/gps_system/gps_test.c | 31 +- .../gps_system/header/fusion_engine.h | 4 + .../gps_system/header/fusion_imu_quality.h | 31 ++ .../gps_system/header/imu_calibration.h | 33 ++ soc/software/gps_system/header/imu_sensor.h | 65 +++ soc/software/gps_system/header/st_lsm6dsox.h | 53 ++ soc/software/gps_system/src/fusion_engine.c | 86 +++ .../gps_system/src/gps_interrupt_handlers.c | 4 +- soc/software/gps_system/src/imu_calibration.c | 113 ++++ soc/software/gps_system/src/imu_sensor.c | 350 ++++++++++++ .../gps_system/src/imu_sensor_profile.c | 74 +++ .../gps_system/src/navigation_fusion.c | 11 +- soc/software/gps_system/src/st_lsm6dsox.c | 503 ++++++++++++++++++ soc/software/gps_system/tests/Makefile | 18 + .../gps_system/tests/test_imu_calibration.c | 78 +++ soc/software/pmod_irq_test/Makefile | 14 + soc/software/pmod_irq_test/README.md | 30 ++ soc/software/pmod_irq_test/main.c | 136 +++++ soc/software/steval_mki217v1_test/Makefile | 2 +- soc/software/steval_mki217v1_test/README.md | 2 +- .../steval_mki217v1_test/header/lsm6dsox.h | 9 + soc/software/steval_mki217v1_test/main.c | 200 ++++++- .../steval_mki217v1_test/src/lsm6dsox.c | 122 ++++- targets/asic/rocket/Makefile | 33 +- targets/asic/rocket/README.md | 40 ++ targets/asic/rocket/tools/gen_asic_wrapper.py | 3 + .../genesys2/tools/gen_genesys2_wrapper.py | 21 +- 35 files changed, 2680 insertions(+), 70 deletions(-) create mode 100644 soc/software/fusion/tests/imu_quality_synthetic_test.c create mode 100644 soc/software/gps_system/header/fusion_imu_quality.h create mode 100644 soc/software/gps_system/header/imu_calibration.h create mode 100644 soc/software/gps_system/header/imu_sensor.h create mode 100644 soc/software/gps_system/header/st_lsm6dsox.h create mode 100644 soc/software/gps_system/src/imu_calibration.c create mode 100644 soc/software/gps_system/src/imu_sensor.c create mode 100644 soc/software/gps_system/src/imu_sensor_profile.c create mode 100644 soc/software/gps_system/src/st_lsm6dsox.c create mode 100644 soc/software/gps_system/tests/Makefile create mode 100644 soc/software/gps_system/tests/test_imu_calibration.c create mode 100644 soc/software/pmod_irq_test/Makefile create mode 100644 soc/software/pmod_irq_test/README.md create mode 100644 soc/software/pmod_irq_test/main.c diff --git a/hardware/fpga/genesys2/constraints/genesys2_pmod_irq.xdc b/hardware/fpga/genesys2/constraints/genesys2_pmod_irq.xdc index 2e7e146..3ffff30 100644 --- a/hardware/fpga/genesys2/constraints/genesys2_pmod_irq.xdc +++ b/hardware/fpga/genesys2/constraints/genesys2_pmod_irq.xdc @@ -1,7 +1,8 @@ -## Genesys2 external sensor interrupt input on PMOD JC. +## Genesys2 external sensor interrupt input on PMOD JB. ## This is independent from the PMOD SPI peripheral. ## -## pmod_irq JC9 +## pmod_irq JB1 -set_property PACKAGE_PIN AK30 [get_ports pmod_irq] +set_property PACKAGE_PIN V29 [get_ports pmod_irq] set_property IOSTANDARD LVCMOS33 [get_ports pmod_irq] +set_property PULLDOWN true [get_ports pmod_irq] diff --git a/hardware/fpga/genesys2/constraints/genesys2_pmod_pps.xdc b/hardware/fpga/genesys2/constraints/genesys2_pmod_pps.xdc index 8499488..002f73e 100644 --- a/hardware/fpga/genesys2/constraints/genesys2_pmod_pps.xdc +++ b/hardware/fpga/genesys2/constraints/genesys2_pmod_pps.xdc @@ -2,7 +2,7 @@ ## ## Existing allocation: ## JC1..JC4, JC7..JC8 user SPI -## JC9 sensor IRQ +## JB1 sensor IRQ ## JC10 GPS PPS (this file) set_property PACKAGE_PIN AK28 [get_ports gps_pps] diff --git a/hardware/sim/asic/rocket/tb/asic_rocket_tb.sv b/hardware/sim/asic/rocket/tb/asic_rocket_tb.sv index bd0b28c..03f4b5e 100644 --- a/hardware/sim/asic/rocket/tb/asic_rocket_tb.sv +++ b/hardware/sim/asic/rocket/tb/asic_rocket_tb.sv @@ -9,6 +9,7 @@ module asic_rocket_tb; reg board_reset = 1'b1; reg gps_rf_clk = 1'b0; reg [3:0] gps_rf_data = 4'h0; + reg pmod_irq = 1'b0; wire core_clk = board_clock; wire core_reset; @@ -34,11 +35,16 @@ module asic_rocket_tb; wire uart_valid; wire [7:0] uart_data; reg simjtag_enable = 1'b0; + reg pmod_irq_rtl_only = 1'b0; reg [7:0] last_uart_byte = 8'h00; integer app_match_count = 0; integer gps_match_count = 0; integer uart_toggle_count = 0; + integer pmod_irq_rise_count = 0; reg main_return_seen = 1'b0; + reg [15:0] main_return_code_q = 16'hffff; + reg pmod_irq_pulse_done = 1'b0; + reg pmod_irq_pending_seen = 1'b0; reg uart_txd_q = 1'b1; wire core_wfi; wire [63:0] main_return_status; @@ -46,6 +52,7 @@ module asic_rocket_tb; initial begin simjtag_enable = $test$plusargs("jtag_rbb_enable"); + pmod_irq_rtl_only = $test$plusargs("pmod_irq_rtl_only"); end always #(CLOCK_HALF_PERIOD_NS) board_clock = ~board_clock; @@ -69,6 +76,25 @@ module asic_rocket_tb; board_reset = 1'b0; end + // Model an asynchronous, active-high sensor interrupt. Four square-wave + // periods deliberately straddle core clock edges and exercise the Rocket + // interrupt synchronizer before reaching PLIC device 11. + initial begin + wait (core_reset === 1'b0); + repeat (32) @(posedge core_clk); + repeat (4) begin + #(3.0) pmod_irq = 1'b1; + repeat (8) @(posedge core_clk); + #(2.0) pmod_irq = 1'b0; + repeat (8) @(posedge core_clk); + end + pmod_irq_pulse_done = 1'b1; + end + + always @(posedge pmod_irq) begin + pmod_irq_rise_count = pmod_irq_rise_count + 1; + end + always @(posedge core_clk) begin uart_txd_q <= uart_txd; if (!core_reset && uart_txd_q !== uart_txd) begin @@ -81,6 +107,7 @@ module asic_rocket_tb; .board_reset(board_reset), .gps_rf_clk(gps_rf_clk), .gps_rf_data(gps_rf_data), + .pmod_irq(pmod_irq), .uart_rxd(1'b1), .uart_txd(uart_txd), .spi_sck(spi_sck), @@ -104,6 +131,19 @@ module asic_rocket_tb; assign main_return_status = dut.u_rocket.tile_prci_domain.element_reset_domain.rockettile.core.csr.reg_mscratch; assign main_return_code = main_return_status[15:0]; + // pmod_irq is source 10 in the zero-based TLPLIC arrays, corresponding to + // architectural PLIC device ID 11. + always @(posedge core_clk) begin + if (core_reset) begin + pmod_irq_pending_seen <= 1'b0; + end else if (dut.u_rocket.plic_domain.plic.pending_10 === 1'b1) begin + if (!pmod_irq_pending_seen) begin + $display("ASIC PMOD IRQ: PLIC device 11 pending observed"); + end + pmod_irq_pending_seen <= 1'b1; + end + end + SimJTAG #(.TICK_DELAY(3)) sim_jtag ( .clock(core_clk), .reset(core_reset), @@ -179,21 +219,54 @@ module asic_rocket_tb; end always @(posedge core_clk) begin - if (!core_reset && gps_match_count == 10) begin + if (!core_reset && gps_match_count == 10 && pmod_irq_pulse_done && + pmod_irq_pending_seen && pmod_irq_rise_count == 4) begin + $display("ASIC PMOD IRQ PASS: rises=%0d pending_seen=%0d", + pmod_irq_rise_count, pmod_irq_pending_seen); $display("ASIC sanity PASS"); $finish; end end + // The RTL-only mode bypasses the intentionally slow SPI-flash boot path. + // It proves the complete hardware route from the asynchronous ASIC input to + // architectural PLIC device 11; sim-pmod-irq retains the CPU/ISR check. + always @(posedge core_clk) begin + if (!core_reset && pmod_irq_rtl_only && pmod_irq_pulse_done) begin + if (pmod_irq_pending_seen && pmod_irq_rise_count == 4) begin + $display("ASIC PMOD IRQ RTL PASS: rises=%0d plic_device=11 pending_seen=%0d", + pmod_irq_rise_count, pmod_irq_pending_seen); + $display("ASIC sanity PASS"); + end else begin + $display("ASIC PMOD IRQ RTL FAIL: rises=%0d plic_device=11 pending_seen=%0d", + pmod_irq_rise_count, pmod_irq_pending_seen); + $display("ASIC sanity FAIL"); + end + $finish; + end + end + always @(posedge core_clk) begin if (core_reset) begin main_return_seen <= 1'b0; + main_return_code_q <= 16'hffff; end else if (!main_return_seen && main_return_status[31:16] == 16'h5a5a) begin main_return_seen <= 1'b1; + main_return_code_q <= main_return_code; $display("C main returned %0d", main_return_code); - if (main_return_code == 16'h0000) begin + end + end + + always @(posedge core_clk) begin + if (!core_reset && main_return_seen && pmod_irq_pulse_done) begin + if (main_return_code_q == 16'h0000 && pmod_irq_pending_seen && + pmod_irq_rise_count == 4) begin + $display("ASIC PMOD IRQ PASS: rises=%0d pending_seen=%0d", + pmod_irq_rise_count, pmod_irq_pending_seen); $display("ASIC sanity PASS"); end else begin + $display("ASIC PMOD IRQ FAIL: rises=%0d pending_seen=%0d", + pmod_irq_rise_count, pmod_irq_pending_seen); $display("ASIC sanity FAIL"); end $finish; diff --git a/hardware/sim/asic/rocket/wave.do b/hardware/sim/asic/rocket/wave.do index 37e3ea9..0a219aa 100644 --- a/hardware/sim/asic/rocket/wave.do +++ b/hardware/sim/asic/rocket/wave.do @@ -17,6 +17,11 @@ add wave -noupdate -expand -group Core /asic_rocket_tb/dut/jtag_tdo_data add wave -noupdate -expand -group Core /asic_rocket_tb/dut/jtag_tdo_driven add wave -noupdate -expand -group Core /asic_rocket_tb/dut/debug_ndreset add wave -noupdate -expand -group Core /asic_rocket_tb/dut/debug_dmactive +add wave -noupdate -expand -group PMOD_IRQ /asic_rocket_tb/pmod_irq +add wave -noupdate -expand -group PMOD_IRQ /asic_rocket_tb/pmod_irq_pulse_done +add wave -noupdate -expand -group PMOD_IRQ /asic_rocket_tb/pmod_irq_pending_seen +add wave -noupdate -expand -group PMOD_IRQ /asic_rocket_tb/dut/rocket_pmod_irq +add wave -noupdate -expand -group PMOD_IRQ /asic_rocket_tb/dut/u_rocket/plic_domain/plic/pending_10 add wave -noupdate -expand -group Flash /asic_rocket_tb/u_flash/sck add wave -noupdate -expand -group Flash /asic_rocket_tb/u_flash/csn add wave -noupdate -expand -group Flash /asic_rocket_tb/u_flash/dq_o diff --git a/soc/software/fusion/tests/Makefile b/soc/software/fusion/tests/Makefile index a127c05..75b775c 100644 --- a/soc/software/fusion/tests/Makefile +++ b/soc/software/fusion/tests/Makefile @@ -5,10 +5,11 @@ LDLIBS ?= -lm GPS_TIME_SOURCE := ../../gps_system/src/gps_time_anchor.c ESKF_SOURCES := ../../gps_system/src/eskf.c ../../gps_system/src/fusion_motion.c ../../gps_system/src/fusion_engine.c ../../gps_system/src/fusion_gps_adapter.c ../../gps_system/src/fusion_bus.c +IMU_PROFILE_SOURCE := ../../gps_system/src/imu_sensor_profile.c .PHONY: all test sanitize profile benchmark clean -all: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test +all: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test imu_quality_synthetic_test standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE) $(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) @@ -19,10 +20,14 @@ eskf_motion_test: eskf_motion_test.c $(ESKF_SOURCES) fusion_synthetic_test: fusion_synthetic_test.c $(ESKF_SOURCES) $(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) -test: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test +imu_quality_synthetic_test: imu_quality_synthetic_test.c $(ESKF_SOURCES) $(IMU_PROFILE_SOURCE) + $(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) + +test: standalone_gps_integrity_test eskf_motion_test fusion_synthetic_test imu_quality_synthetic_test ./standalone_gps_integrity_test ./eskf_motion_test ./fusion_synthetic_test + ./imu_quality_synthetic_test sanitize: $(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \ @@ -37,6 +42,12 @@ sanitize: -o fusion_synthetic_test_san $(LDLIBS) ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ UBSAN_OPTIONS=halt_on_error=1 ./fusion_synthetic_test_san + $(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \ + -fsanitize=address,undefined -fno-omit-frame-pointer \ + -I../../gps_system/header imu_quality_synthetic_test.c $(ESKF_SOURCES) \ + $(IMU_PROFILE_SOURCE) -o imu_quality_synthetic_test_san $(LDLIBS) + ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ + UBSAN_OPTIONS=halt_on_error=1 ./imu_quality_synthetic_test_san profile: $(CC) -std=c11 -O2 -g -pg -Wall -Wextra -Wpedantic \ @@ -51,4 +62,4 @@ benchmark: standalone_gps_integrity_test ./standalone_gps_integrity_test --bench-double 100000 clean: - rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace gmon.out + rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_synthetic_test fusion_synthetic_test_san fusion_synthetic_test_gprof fusion_synthetic_trace imu_quality_synthetic_test imu_quality_synthetic_test_san gmon.out diff --git a/soc/software/fusion/tests/imu_quality_synthetic_test.c b/soc/software/fusion/tests/imu_quality_synthetic_test.c new file mode 100644 index 0000000..53480e4 --- /dev/null +++ b/soc/software/fusion/tests/imu_quality_synthetic_test.c @@ -0,0 +1,418 @@ +#include +#include +#include +#include +#include + +#include "fusion_engine.h" +#include "imu_sensor.h" + +#define QUALITY_PI 3.14159265358979323846264338327950288 +#define QUALITY_TIMEBASE_HZ 1000000u +#define QUALITY_IMU_HZ 200u +#define QUALITY_DURATION_S 30u + +typedef struct { + uint32_t state; +} quality_rng_t; + +typedef struct { + double position_m[3]; + double velocity_mps[3]; + double yaw_rad; +} quality_truth_t; + +typedef struct { + const char* name; + double horizontal_rmse_m; + double final_horizontal_error_m; + double final_speed_mps; + double outage_peak_error_m; + double first_accel_detection_s; + double rotation_detection_s; + double final_stationary_reentry_s; + uint64_t zero_updates; + uint64_t accepted_gps; + unsigned int dynamic_zero_update_count; + unsigned int false_moving_before_motion; + bool state_finite; +} quality_metrics_t; + +static int failures; + +#define CHECK(condition, message) do { \ + if(!(condition)) { \ + fprintf(stderr, "FAIL: %s\n", message); \ + failures++; \ + } \ +} while(0) + +static double square(double value) +{ + return value * value; +} + +static double norm2(double first, double second) +{ + return sqrt(first * first + second * second); +} + +static uint32_t rng_next(quality_rng_t* rng) +{ + uint32_t value = rng->state; + value ^= value << 13; + value ^= value >> 17; + value ^= value << 5; + rng->state = value; + return value; +} + +static double rng_uniform(quality_rng_t* rng) +{ + return ((double)rng_next(rng) + 0.5) / 4294967296.0; +} + +static double rng_gaussian(quality_rng_t* rng) +{ + double first = fmax(rng_uniform(rng), 1.0e-12); + double second = rng_uniform(rng); + return sqrt(-2.0 * log(first)) * cos(2.0 * QUALITY_PI * second); +} + +static void trajectory_input(double time_s, + double acceleration_nav_mps2[3], + double* yaw_rate_rps) +{ + memset(acceleration_nav_mps2, 0, 3u * sizeof(double)); + *yaw_rate_rps = 0.0; + if((time_s >= 3.0) && (time_s < 4.0)) + { + acceleration_nav_mps2[0] = 2.0; + } + else if((time_s >= 6.0) && (time_s < 7.0)) + { + acceleration_nav_mps2[0] = -2.0; + } + else if((time_s >= 12.0) && (time_s < 13.0)) + { + *yaw_rate_rps = 0.5 * QUALITY_PI; + } + else if((time_s >= 17.0) && (time_s < 17.5)) + { + acceleration_nav_mps2[1] = -3.0; + } + else if((time_s >= 19.0) && (time_s < 19.5)) + { + acceleration_nav_mps2[1] = 3.0; + } +} + +static bool truth_is_dynamic(const quality_truth_t* truth, + const double acceleration_nav_mps2[3], + double yaw_rate_rps) +{ + return (fabs(acceleration_nav_mps2[0]) > 0.4) || + (fabs(acceleration_nav_mps2[1]) > 0.4) || + (fabs(yaw_rate_rps) > 0.02) || + (norm2(truth->velocity_mps[0], truth->velocity_mps[1]) > 0.20); +} + +static void truth_step(quality_truth_t* truth, double dt_s, + const double acceleration_nav_mps2[3], + double yaw_rate_rps) +{ + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + truth->position_m[axis] += truth->velocity_mps[axis] * dt_s + + 0.5 * acceleration_nav_mps2[axis] * dt_s * dt_s; + truth->velocity_mps[axis] += acceleration_nav_mps2[axis] * dt_s; + } + truth->yaw_rad += yaw_rate_rps * dt_s; +} + +static void publish_position(fusion_engine_t* engine, uint64_t tick, + const quality_truth_t* truth, + quality_rng_t* gps_rng) +{ + fusion_position_measurement_t gps; + memset(&gps, 0, sizeof(gps)); + gps.timestamp_ticks = tick; + gps.position_enu_m[0] = truth->position_m[0] + 1.5 * rng_gaussian(gps_rng); + gps.position_enu_m[1] = truth->position_m[1] + 1.5 * rng_gaussian(gps_rng); + gps.position_enu_m[2] = truth->position_m[2] + 2.5 * rng_gaussian(gps_rng); + gps.covariance_enu_m2[0] = square(1.5); + gps.covariance_enu_m2[4] = square(1.5); + gps.covariance_enu_m2[8] = square(2.5); + gps.source_quality = 1.0; + gps.source_valid = true; + eskf_update_result_t result = ESKF_UPDATE_INVALID; + CHECK(fusion_engine_push_position(engine, &gps, &result, NULL), + "quality-profile GPS is processed"); + CHECK((result == ESKF_UPDATE_ACCEPTED) || + (result == ESKF_UPDATE_REJECTED_NIS), + "quality-profile GPS returns a defined update result"); +} + +static quality_metrics_t run_profile(imu_sensor_backend_t backend, + uint32_t seed) +{ + static fusion_engine_t engine; + fusion_imu_quality_profile_t profile; + CHECK(imu_sensor_quality_profile_for_backend(backend, &profile), + "sensor fusion quality profile exists"); + fusion_engine_config_t config; + fusion_engine_default_config(&config, QUALITY_TIMEBASE_HZ); + CHECK(fusion_engine_apply_imu_quality(&config, &profile), + "sensor quality profile applies to ESKF config"); + config.calibration_duration_s = 2.0; + config.calibration_min_samples = QUALITY_IMU_HZ; + CHECK(fusion_engine_init(&engine, &config), + "quality-profile fusion engine initializes"); + + fusion_position_measurement_t origin; + memset(&origin, 0, sizeof(origin)); + origin.covariance_enu_m2[0] = square(1.5); + origin.covariance_enu_m2[4] = square(1.5); + origin.covariance_enu_m2[8] = square(2.5); + origin.source_quality = 1.0; + origin.source_valid = true; + CHECK(fusion_engine_push_position(&engine, &origin, NULL, NULL), + "quality-profile initial GPS is retained"); + + quality_rng_t imu_rng = {0x1735a9cdu ^ seed}; + quality_rng_t gps_rng = {0x9b6d42e1u ^ seed}; + quality_truth_t truth = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, 0.0}; + double accel_bias[3] = {0.030, -0.020, 0.050}; + double gyro_bias[3] = {0.0008, -0.0005, 0.0007}; + double quality_factor = (backend == IMU_SENSOR_ST_LSM6DSOX) ? 1.35 : 1.0; + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + accel_bias[axis] *= quality_factor; + gyro_bias[axis] *= quality_factor; + } + + const uint64_t imu_period_ticks = QUALITY_TIMEBASE_HZ / QUALITY_IMU_HZ; + const double dt_s = 1.0 / (double)QUALITY_IMU_HZ; + const double bandwidth_sqrt_hz = sqrt(0.5 * (double)QUALITY_IMU_HZ); + quality_metrics_t metrics; + memset(&metrics, 0, sizeof(metrics)); + metrics.name = profile.name; + metrics.first_accel_detection_s = NAN; + metrics.rotation_detection_s = NAN; + metrics.final_stationary_reentry_s = NAN; + double squared_horizontal_error_sum = 0.0; + uint64_t error_samples = 0u; + uint64_t next_gps_tick = 3000000u; + uint64_t previous_zero_updates = 0u; + + for(uint64_t tick = 0u ; + tick <= (uint64_t)QUALITY_DURATION_S * QUALITY_TIMEBASE_HZ ; + tick += imu_period_ticks) + { + double acceleration_nav[3] = {0.0, 0.0, 0.0}; + double yaw_rate = 0.0; + double time_s = (double)tick / (double)QUALITY_TIMEBASE_HZ; + if(tick != 0u) + { + double midpoint_s = time_s - 0.5 * dt_s; + trajectory_input(midpoint_s, acceleration_nav, &yaw_rate); + truth_step(&truth, dt_s, acceleration_nav, yaw_rate); + } + + /* Inject the profile's bias random walk into the synthetic sensor. */ + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + accel_bias[axis] += profile.accel_bias_random_walk_mps2_sqrt_s[axis] * + sqrt(dt_s) * rng_gaussian(&imu_rng); + gyro_bias[axis] += profile.gyro_bias_random_walk_rps_sqrt_s[axis] * + sqrt(dt_s) * rng_gaussian(&imu_rng); + } + + double midpoint_yaw = truth.yaw_rad - 0.5 * yaw_rate * dt_s; + double cosine = cos(midpoint_yaw); + double sine = sin(midpoint_yaw); + fusion_imu_measurement_t imu; + memset(&imu, 0, sizeof(imu)); + imu.timestamp_ticks = tick; + imu.accel_mps2[0] = cosine * acceleration_nav[0] + + sine * acceleration_nav[1]; + imu.accel_mps2[1] = -sine * acceleration_nav[0] + + cosine * acceleration_nav[1]; + imu.accel_mps2[2] = config.eskf.gravity_mps2; + imu.gyro_rps[2] = yaw_rate; + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + double accel_sigma = + 3.0 * profile.nominal_accel_noise_density_mps2_sqrt_hz[axis] * + bandwidth_sqrt_hz; + double gyro_sigma = + 3.0 * profile.nominal_gyro_noise_density_rps_sqrt_hz[axis] * + bandwidth_sqrt_hz; + imu.accel_mps2[axis] += accel_bias[axis] + + accel_sigma * rng_gaussian(&imu_rng); + imu.gyro_rps[axis] += gyro_bias[axis] + + gyro_sigma * rng_gaussian(&imu_rng); + } + imu.temperature_c = 25.0 + 0.05 * time_s; + imu.sequence = (uint32_t)(tick / imu_period_ticks); + CHECK(fusion_engine_push_imu(&engine, &imu), + "quality-profile IMU is accepted"); + + if((tick >= next_gps_tick) && + !((time_s >= 16.0) && (time_s < 25.0))) + { + publish_position(&engine, tick, &truth, &gps_rng); + next_gps_tick += QUALITY_TIMEBASE_HZ; + } + else if((tick >= next_gps_tick) && (time_s >= 25.0)) + { + /* Skip the 16--25 s events, then resume at the current epoch. */ + publish_position(&engine, tick, &truth, &gps_rng); + next_gps_tick = tick + QUALITY_TIMEBASE_HZ; + } + + if(engine.state != FUSION_ENGINE_RUNNING) + { + continue; + } + bool dynamic = truth_is_dynamic(&truth, + acceleration_nav, yaw_rate); + uint64_t zero_updates = engine.diagnostics.zero_velocity_update_count; + if(dynamic && (zero_updates > previous_zero_updates)) + { + metrics.dynamic_zero_update_count += + (unsigned int)(zero_updates - previous_zero_updates); + } + previous_zero_updates = zero_updates; + if((time_s > 2.2) && (time_s < 3.0) && + (engine.current.motion.state != FUSION_MOTION_STATIONARY)) + { + metrics.false_moving_before_motion++; + } + if(isnan(metrics.first_accel_detection_s) && (time_s >= 3.0) && + (engine.current.motion.state == FUSION_MOTION_MOVING)) + { + metrics.first_accel_detection_s = time_s; + } + if(isnan(metrics.rotation_detection_s) && (time_s >= 12.0) && + (engine.current.motion.state == FUSION_MOTION_MOVING)) + { + metrics.rotation_detection_s = time_s; + } + if(isnan(metrics.final_stationary_reentry_s) && (time_s >= 19.5) && + (engine.current.motion.state == FUSION_MOTION_STATIONARY)) + { + metrics.final_stationary_reentry_s = time_s; + } + + double horizontal_error = norm2( + engine.current.filter.position_m[0] - truth.position_m[0], + engine.current.filter.position_m[1] - truth.position_m[1]); + if(time_s >= 3.0) + { + squared_horizontal_error_sum += square(horizontal_error); + error_samples++; + } + if((time_s >= 16.0) && (time_s < 25.0)) + { + metrics.outage_peak_error_m = + fmax(metrics.outage_peak_error_m, horizontal_error); + } + } + + metrics.horizontal_rmse_m = + (error_samples != 0u) + ? sqrt(squared_horizontal_error_sum / (double)error_samples) + : NAN; + metrics.final_horizontal_error_m = norm2( + engine.current.filter.position_m[0] - truth.position_m[0], + engine.current.filter.position_m[1] - truth.position_m[1]); + metrics.final_speed_mps = eskf_speed_mps(&engine.current.filter); + metrics.zero_updates = engine.diagnostics.zero_velocity_update_count; + metrics.accepted_gps = engine.diagnostics.position_accepted; + metrics.state_finite = eskf_state_is_finite(&engine.current.filter); + return metrics; +} + +static void check_metrics(const quality_metrics_t* metrics, + bool noisier_st_profile) +{ + CHECK(metrics->state_finite, "quality-profile ESKF state remains finite"); + CHECK(metrics->false_moving_before_motion == 0u, + "stationary sensor noise does not create false motion"); + CHECK(metrics->dynamic_zero_update_count == 0u, + "ZUPT and zero-gyro are disabled throughout true motion"); + CHECK(isfinite(metrics->first_accel_detection_s) && + (metrics->first_accel_detection_s <= 3.010), + "sudden acceleration is detected within two samples"); + CHECK(isfinite(metrics->rotation_detection_s) && + (metrics->rotation_detection_s <= 12.010), + "sudden rotation is detected within two samples"); + CHECK(isfinite(metrics->final_stationary_reentry_s) && + (metrics->final_stationary_reentry_s >= 20.5) && + (metrics->final_stationary_reentry_s <= + (noisier_st_profile ? 29.5 : 25.0)), + "stationary re-entry waits for motion hysteresis and then recovers"); + CHECK(metrics->zero_updates > (noisier_st_profile ? 100u : 150u), + "stationary intervals apply rate-limited zero updates"); + CHECK(metrics->accepted_gps >= 15u, + "GPS updates remain usable with the sensor profile"); + CHECK(metrics->horizontal_rmse_m < (noisier_st_profile ? 5.0 : 2.0), + "sensor-profile horizontal RMSE remains bounded"); + CHECK(metrics->outage_peak_error_m < (noisier_st_profile ? 12.0 : 3.0), + "nine-second GPS outage drift remains bounded"); + CHECK(metrics->final_horizontal_error_m < 2.0, + "post-outage final horizontal error remains bounded"); + CHECK(metrics->final_speed_mps < 0.05, + "final stationary speed is near zero"); +} + +int main(void) +{ + fusion_imu_quality_profile_t tdk_profile; + fusion_imu_quality_profile_t st_profile; + CHECK(imu_sensor_quality_profile_for_backend(IMU_SENSOR_TDK_ICM42688, + &tdk_profile), + "TDK profile is valid"); + CHECK(imu_sensor_quality_profile_for_backend(IMU_SENSOR_ST_LSM6DSOX, + &st_profile), + "ST profile is valid"); + CHECK(tdk_profile.nominal_gyro_noise_density_rps_sqrt_hz[0] < + st_profile.nominal_gyro_noise_density_rps_sqrt_hz[0], + "TDK profile models lower nominal gyro noise than ST"); + CHECK(tdk_profile.process_gyro_noise_density_rps_sqrt_hz[0] < + st_profile.process_gyro_noise_density_rps_sqrt_hz[0], + "TDK profile gives the ESKF lower gyro process noise"); + + quality_metrics_t tdk = run_profile(IMU_SENSOR_TDK_ICM42688, 0x11111111u); + quality_metrics_t st = run_profile(IMU_SENSOR_ST_LSM6DSOX, 0x11111111u); + check_metrics(&tdk, false); + check_metrics(&st, true); + CHECK(tdk.outage_peak_error_m <= st.outage_peak_error_m + 0.25, + "TDK-like profile is no worse than ST-like profile during GPS outage"); + + printf("Synthetic IMU quality-profile comparison (same production C ESKF)\n"); + printf(" %-16s RMSE=%5.3f m outage-peak=%5.3f m final=%5.3f m speed=%7.4f m/s GPS=%llu\n", + tdk.name, tdk.horizontal_rmse_m, tdk.outage_peak_error_m, + tdk.final_horizontal_error_m, tdk.final_speed_mps, + (unsigned long long)tdk.accepted_gps); + printf(" %-16s RMSE=%5.3f m outage-peak=%5.3f m final=%5.3f m speed=%7.4f m/s GPS=%llu\n", + st.name, st.horizontal_rmse_m, st.outage_peak_error_m, + st.final_horizontal_error_m, st.final_speed_mps, + (unsigned long long)st.accepted_gps); + printf(" detection TDK accel=%.3f s turn=%.3f s stop=%.3f s; ST accel=%.3f s turn=%.3f s stop=%.3f s\n", + tdk.first_accel_detection_s, tdk.rotation_detection_s, + tdk.final_stationary_reentry_s, st.first_accel_detection_s, + st.rotation_detection_s, st.final_stationary_reentry_s); + printf(" dynamic ZUPT violations TDK=%u ST=%u; zero-updates TDK=%llu ST=%llu\n", + tdk.dynamic_zero_update_count, st.dynamic_zero_update_count, + (unsigned long long)tdk.zero_updates, + (unsigned long long)st.zero_updates); + if(failures != 0) + { + fprintf(stderr, "%d IMU quality-profile check(s) failed\n", failures); + return 1; + } + printf("PASS: both TDK-like and noisier ST-like streams remain bounded.\n"); + return 0; +} diff --git a/soc/software/gps_system/Makefile b/soc/software/gps_system/Makefile index 5f3d306..8e97ae0 100644 --- a/soc/software/gps_system/Makefile +++ b/soc/software/gps_system/Makefile @@ -5,9 +5,41 @@ TARGET := gps_system # with the common 100 MHz fallback divider. BSP_UART_CLOCK_HZ ?= 50000000 +# 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). +IMU_SENSOR ?= auto +ifeq ($(IMU_SENSOR),auto) +IMU_DRIVER_SRCS := src/tdk_icm42688.c src/st_lsm6dsox.c +IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_AUTO=1 +else ifeq ($(IMU_SENSOR),tdk) +IMU_DRIVER_SRCS := src/tdk_icm42688.c +IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_TDK=1 +else ifeq ($(IMU_SENSOR),st) +IMU_DRIVER_SRCS := src/st_lsm6dsox.c +IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_ST=1 +else ifeq ($(IMU_SENSOR),stm) +IMU_DRIVER_SRCS := src/st_lsm6dsox.c +IMU_BACKEND_CFLAG := -DIMU_SENSOR_BUILD_ST=1 +else +$(error IMU_SENSOR must be auto, tdk, st or stm) +endif + +# Zero means use the selected backend's default. The legacy TDK variable is +# accepted so existing build commands remain valid. +ifdef TDK_IMU_ODR_HZ +IMU_ODR_HZ ?= $(TDK_IMU_ODR_HZ) +else +IMU_ODR_HZ ?= 0 +endif +IMU_ACCEL_FS_G ?= 0 +IMU_GYRO_FS_DPS ?= 0 + APP_SRCS := \ gps_test.c \ - src/tdk_icm42688.c \ + src/imu_calibration.c \ + src/imu_sensor.c \ + src/imu_sensor_profile.c \ + $(IMU_DRIVER_SRCS) \ src/eskf.c \ src/fusion_motion.c \ src/fusion_engine.c \ @@ -25,13 +57,15 @@ APP_SRCS := \ BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c -TDK_IMU_ODR_HZ ?= 500 GPS_NAV_SOLVE_TIMER_TICKS ?= 50 include ../common/app.mk CFLAGS += -Iheader -CFLAGS += -DTDK_IMU_ODR_HZ=$(TDK_IMU_ODR_HZ) \ +CFLAGS += $(IMU_BACKEND_CFLAG) \ + -DIMU_ODR_HZ=$(IMU_ODR_HZ) \ + -DIMU_ACCEL_FS_G=$(IMU_ACCEL_FS_G) \ + -DIMU_GYRO_FS_DPS=$(IMU_GYRO_FS_DPS) \ -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 2db308c..6628eb9 100644 --- a/soc/software/gps_system/README.md +++ b/soc/software/gps_system/README.md @@ -2,6 +2,124 @@ Bu notlar GPS bare-metal yaziliminin VCU118 Rocket SoC portu ve interrupt debug sureci icin yazildi. +## Generic IMU Mimarisi (TDK ve ST) + +GPS/ESKF artik dogrudan bir sensor surucusune bagli degildir. Veri yolu: + +```text +TDK ICM42688 -----\ + -> imu_sensor -> imu_calibration -> fusion_imu_measurement -> motion detector + ESKF +ST LSM6DSOX ------/ +``` + +- `src/tdk_icm42688.c`: TDK ICM42688'e ozel SPI, register, interrupt ve + ham-veri donusumu. +- `src/st_lsm6dsox.c`: STEVAL-MKI217V1 uzerindeki ST LSM6DSOX'e ozel SPI, + register, foreground polling ve ham-veri donusumu. +- `src/imu_sensor.c`: ortak init/start/stop/IRQ/service/measurement arayuzu. +- `src/imu_calibration.c`: markadan bagimsiz bias, 3x3 scale/cross-axis/mounting + donusumu ve opsiyonel sicaklik katsayisi. +- `fusion_measurement.h`, motion detector ve ESKF yalniz SI birimindeki ortak + mesaji gorur; aktif sensorun markasini bilmez. + +Firmware isletim sistemi kullanmaz. Yeni IMU katmani `malloc` cagirmadan sensor +kuyruklarini ve butun durumunu statik ayirir. Newlib ve formatli UART destegi +nedeniyle linker'da heap alani yine bulunur; yeni sensor kodu buna guvenmez. +Karttaki 1 MiB DTIM test icin genis tutulmustur; firmware tasarim butcesi +ilerideki 512 KiB DTIM hedefidir. `auto` build iki sensor surucusunu da icerir; +kesin sensor secimi kullanilmayan surucuyu linkten cikarir. + +2026-07-13 tarihli `-Os` bare-metal link boyutlari: + +| Secim | text | data | bss | Toplam DTIM image | +| --- | ---: | ---: | ---: | ---: | +| `tdk` | 67,256 | 2,496 | 205,160 | 274,912 bayt | +| `st` | 66,936 | 2,496 | 199,280 | 268,712 bayt | +| `auto` | 68,888 | 2,496 | 207,560 | 278,944 bayt | + +En buyuk `auto` image 512 KiB DTIM'in yaklasik yuzde 53.2'sini kullanir; stack +ve ilerideki buffer'lar icin 245,344 bayt (yaklasik 239.6 KiB) adres alani +kalir. Linker bunun 16 KiB'ini varsayilan stack icin ayirir. 1 MiB test +konfigurasyonunda gorulen ek bosluk yeni statik buffer eklemek icin gerekce +sayilmamalidir. Host unit-test executable'i bu rakamlara dahil degildir. +`auto` konfigurasyonu ayrica `BSP_DTIM_SIZE=0x00080000` ile gercekten link +edilerek 512 KiB sinirinda dogrulanmistir. + +### Bare-metal sensor secimi + +Otomatik WHO_AM_I algilama varsayilandir: + +```sh +make -C soc/software/gps_system IMU_SENSOR=auto +``` + +Otomatik secimde eslesen cihaz bulunana kadar yalniz read-only WHO_AM_I probe +yapilir. TDK ve ST build'leri acikca su sekilde secilebilir: + +```sh +make -C soc/software/gps_system IMU_SENSOR=tdk \ + IMU_ODR_HZ=500 IMU_ACCEL_FS_G=4 IMU_GYRO_FS_DPS=250 + +make -C soc/software/gps_system IMU_SENSOR=st \ + IMU_ODR_HZ=104 IMU_ACCEL_FS_G=2 IMU_GYRO_FS_DPS=250 +``` + +`IMU_SENSOR=stm`, `st` seciminin alias'idir. `IMU_ODR_HZ=0`, +`IMU_ACCEL_FS_G=0` ve `IMU_GYRO_FS_DPS=0` aktif surucunun guvenli varsayilanini +secer. Bu ozellikle `auto` icin onemlidir; TDK ve ST'nin ortak bir ODR degeri +yoktur. Desteklenen ODR'ler TDK icin 25/50/100/200/500/1000 Hz, ST icin +52/104/208 Hz'dir. + +TDK veri-hazir interrupt'i ile calisir ve ana dongu `wfi` kullanir. Mevcut ST +baglantisinda interrupt pini bagli olmadigi icin ST surucusu foreground'da +ODR'nin iki kati hizda status poll eder; yeni ornekleri sensor ODR'sinde +uretir. Bu yontem fonksiyoneldir fakat CPU'nun `wfi` ile uyumasini engeller. +Uzun WLS islemi sirasinda foreground durdugu icin sensorun yeni ornekleri +overwrite olabilir. Bu nedenle interruptsiz ST yolu stationary bring-up ve log +testi icindir; hareketli fusion icin kayipsiz bir yol kabul edilmez. +Uretim baglantisinda ST INT pini eklendiginde yalniz ST backend acquisition +yontemi interrupt'a cevrilecek (alternatif olarak sensor FIFO ve sensor +timestamp'i kullanilacak); ortak arayuz ve ESKF degismeyecektir. + +Sensor ODR'si ile GPS nav-solve periyodu birbirinden bagimsizdir. Her IMU ve GPS +olcumu ayni local monotonic CPU tick domaininde kendi olcum zamaniyla tasinir; +ESKF olaylari timestamp sirasinda isler. TDK interrupt timestamp'i data-ready +kenarini yakindan temsil eder. Poll edilen ST timestamp'i ise data-ready'nin +foreground'da goruldugu andir ve bir polling gecikmesi tasir. Hareketli nihai +sistemde ST INT veya FIFO timestamp kullanma gereksiniminin nedeni budur. + +### Kalibrasyon ve sicaklik politikasi + +Uc farkli duzeltme birbirinden ayridir: + +1. Sensorun fabrika trim'i ve datasheet sensitivity degerleri sensor + surucusunde kullanilir. +2. Kalici, karta ozel bias/scale/eksen/sicaklik katsayilari ortak + `imu_calibration_t` icinde uygulanabilir. TDK ve ST ayarlari + `calibration_by_backend[]` icinde ayri profillerdir; `auto` secimi bir + sensorun kalibrasyonunu digerine uygulayamaz. Varsayilan deger identity + + sifir bias'tir; yani varsayilanda olcumu degistirmez. +3. Fusion engine acilista kart sessizken iki saniyelik ornek toplar. Anlik gyro + bias'ini, ilk roll/pitch hizalamasini ve gravity yonundeki accel kalintisini + tahmin eder. ESKF bundan sonra residual accel/gyro bias state'lerini izler. + +Sicaklik GPS/IMU fusionunun calismasi icin zorunlu degildir. Kisa sureli ve +dar sicaklik araligindaki ilk testte sicaklik yalniz olculur ve mesajda +tasinir; ESKF denklemlerini etkilemez. Uzun sure, dis ortam veya sensorun +isinmasiyla bias degisimi gorulurse sicaklik katsayilari offline loglardan +cikarilip `temperature_compensation_enabled=true` ile etkinlestirilir. Katsayi +olmadan otomatik sicaklik duzeltmesi yapmak yanlis bias uretme riski tasir. + +Portable kalibrasyon matematigi Linux'ta heap kullanmadan su testle dogrulanir: + +```sh +make -C soc/software/gps_system/tests clean run +``` + +Bu test identity davranisini, bias + 3x3 donusumu, opsiyonel sicaklik +duzeltmesini ve singular matris reddini kontrol eder. Test executable'i host +icindir ve bare-metal firmware'e linklenmez. + ## Donanim Adresleri ve IRQ Map Generated DTS/RTL'e gore GPS IP adresleri: diff --git a/soc/software/gps_system/gps_test.c b/soc/software/gps_system/gps_test.c index 0035d29..7c16588 100644 --- a/soc/software/gps_system/gps_test.c +++ b/soc/software/gps_system/gps_test.c @@ -5,29 +5,42 @@ #include #include "matrixLib.h" #include "gps_macros.h" +#include "imu_sensor.h" #include "navigation_fusion.h" -#include "tdk_icm42688.h" int main() { - tdk_icm42688_config_t imu_config; - tdk_icm42688_default_config(&imu_config); - bool imu_initialized = tdk_icm42688_init(&imu_config); + imu_sensor_config_t imu_config; + imu_sensor_default_config(&imu_config); + imu_config.odr_hz = IMU_ODR_HZ; + imu_config.accel_full_scale_g = IMU_ACCEL_FS_G; + imu_config.gyro_full_scale_dps = IMU_GYRO_FS_DPS; + bool imu_initialized = imu_sensor_init(&imu_config); bool fusion_initialized = navigation_fusion_init(BSP_CLINT_TIMEBASE_HZ); gps_startup_initialization(GPS_NAV_SOLVE_TIMER_TICKS); - bool imu_running = imu_initialized && fusion_initialized && tdk_icm42688_start(); + bool imu_running = imu_initialized && fusion_initialized && imu_sensor_start(); + (void)imu_running; + imu_sensor_diagnostics_t imu_diagnostics; + imu_sensor_get_diagnostics(&imu_diagnostics); GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION, - "init imu=%u running=%u odr_hz=%u timebase_hz=%lu\r\n", - imu_initialized ? 1u : 0u, imu_running ? 1u : 0u, - imu_config.odr_hz, (unsigned long)BSP_CLINT_TIMEBASE_HZ); + "init imu=%u sensor=%s who=0x%02x running=%u odr_hz=%lu " + "poll=%u timebase_hz=%lu\r\n", + imu_initialized ? 1u : 0u, + imu_sensor_backend_name(imu_diagnostics.backend), + (unsigned int)imu_diagnostics.who_am_i, + imu_running ? 1u : 0u, + (unsigned long)imu_diagnostics.configured_odr_hz, + imu_diagnostics.foreground_polling ? 1u : 0u, + (unsigned long)BSP_CLINT_TIMEBASE_HZ); acq_start_loop(); uint64_t last_fusion_status_tick = 0u; while(1) { - asm volatile("wfi"); + /* GPS and either IMU backend wake the shared machine interrupt path. */ + asm volatile("wfi"); gps_background_task(); calculate_position(); if(fusion_initialized) diff --git a/soc/software/gps_system/header/fusion_engine.h b/soc/software/gps_system/header/fusion_engine.h index 9f35fdf..bc476fd 100644 --- a/soc/software/gps_system/header/fusion_engine.h +++ b/soc/software/gps_system/header/fusion_engine.h @@ -5,6 +5,7 @@ #include #include "eskf.h" +#include "fusion_imu_quality.h" #include "fusion_measurement.h" #include "fusion_motion.h" @@ -104,6 +105,9 @@ typedef struct { void fusion_engine_default_config(fusion_engine_config_t* config, uint64_t timebase_hz); +bool fusion_engine_apply_imu_quality( + fusion_engine_config_t* config, + const fusion_imu_quality_profile_t* profile); bool fusion_engine_init(fusion_engine_t* engine, const fusion_engine_config_t* config); diff --git a/soc/software/gps_system/header/fusion_imu_quality.h b/soc/software/gps_system/header/fusion_imu_quality.h new file mode 100644 index 0000000..a529fc8 --- /dev/null +++ b/soc/software/gps_system/header/fusion_imu_quality.h @@ -0,0 +1,31 @@ +#ifndef FUSION_IMU_QUALITY_H +#define FUSION_IMU_QUALITY_H + +#include + +#define FUSION_IMU_QUALITY_AXIS_COUNT 3u + +/* + * Hardware-neutral IMU characteristics. The nominal noise fields describe + * the sensor/board measurement stream. The process-noise fields are the + * deliberately conservative values used by the ESKF and may include timing, + * mounting, calibration and temperature uncertainty in addition to white + * sensor noise. + */ +typedef struct { + const char* name; + double nominal_accel_noise_density_mps2_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT]; + double nominal_gyro_noise_density_rps_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT]; + double process_accel_noise_density_mps2_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT]; + double process_gyro_noise_density_rps_sqrt_hz[FUSION_IMU_QUALITY_AXIS_COUNT]; + double accel_bias_random_walk_mps2_sqrt_s[FUSION_IMU_QUALITY_AXIS_COUNT]; + double gyro_bias_random_walk_rps_sqrt_s[FUSION_IMU_QUALITY_AXIS_COUNT]; + double calibration_accel_std_limit_mps2; + double calibration_gyro_std_limit_rps; + double zero_gyro_measurement_std_rps[FUSION_IMU_QUALITY_AXIS_COUNT]; +} fusion_imu_quality_profile_t; + +void fusion_imu_quality_default(fusion_imu_quality_profile_t* profile); +bool fusion_imu_quality_is_valid(const fusion_imu_quality_profile_t* profile); + +#endif diff --git a/soc/software/gps_system/header/imu_calibration.h b/soc/software/gps_system/header/imu_calibration.h new file mode 100644 index 0000000..4623cda --- /dev/null +++ b/soc/software/gps_system/header/imu_calibration.h @@ -0,0 +1,33 @@ +#ifndef IMU_CALIBRATION_H +#define IMU_CALIBRATION_H + +#include + +#include "fusion_measurement.h" + +/* + * Sensor-independent calibration in the IMU body frame. + * + * corrected = transform * (measured - bias - temperature_coefficient * dT) + * + * The transform can contain scale, cross-axis and mounting-axis corrections. + * Temperature correction is deliberately opt-in; collecting temperature does + * not change measurements unless temperature_compensation_enabled is true. + */ +typedef struct { + double accel_bias_mps2[3]; + double gyro_bias_rps[3]; + double accel_transform[3][3]; + double gyro_transform[3][3]; + double reference_temperature_c; + double accel_temperature_coefficient_mps2_per_c[3]; + double gyro_temperature_coefficient_rps_per_c[3]; + bool temperature_compensation_enabled; +} imu_calibration_t; + +void imu_calibration_default(imu_calibration_t* calibration); +bool imu_calibration_is_valid(const imu_calibration_t* calibration); +bool imu_calibration_apply(const imu_calibration_t* calibration, + fusion_imu_measurement_t* measurement); + +#endif diff --git a/soc/software/gps_system/header/imu_sensor.h b/soc/software/gps_system/header/imu_sensor.h new file mode 100644 index 0000000..5ab1380 --- /dev/null +++ b/soc/software/gps_system/header/imu_sensor.h @@ -0,0 +1,65 @@ +#ifndef IMU_SENSOR_H +#define IMU_SENSOR_H + +#include +#include + +#include "fusion_measurement.h" +#include "fusion_imu_quality.h" +#include "imu_calibration.h" + +typedef enum { + IMU_SENSOR_NONE = 0, + IMU_SENSOR_TDK_ICM42688, + IMU_SENSOR_ST_LSM6DSOX, + IMU_SENSOR_BACKEND_COUNT +} imu_sensor_backend_t; + +typedef struct { + /* Zero selects the active backend's safe default. */ + uint32_t odr_hz; + unsigned int accel_full_scale_g; + unsigned int gyro_full_scale_dps; + /* Auto-detected devices must never share board-specific calibration. */ + imu_calibration_t calibration_by_backend[IMU_SENSOR_BACKEND_COUNT]; +} imu_sensor_config_t; + +typedef struct { + uint64_t irq_count; + uint64_t sample_count; + uint64_t invalid_sample_count; + uint64_t spurious_irq_count; + uint64_t queue_overflow_count; + uint64_t estimated_missed_sample_count; + uint64_t last_timestamp_ticks; + uint32_t configured_odr_hz; + uint8_t who_am_i; + imu_sensor_backend_t backend; + bool initialized; + bool running; + bool foreground_polling; +} imu_sensor_diagnostics_t; + +void imu_sensor_default_config(imu_sensor_config_t* config); +bool imu_sensor_init(const imu_sensor_config_t* config); +bool imu_sensor_start(void); +void imu_sensor_stop(void); + +/* Interrupt backend hook. Safe no-op for a foreground-polled backend. */ +void imu_sensor_irq_handler(void); + +/* Foreground acquisition hook. Safe no-op for an interrupt backend. */ +void imu_sensor_service(void); +bool imu_sensor_requires_foreground_service(void); + +bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out); +void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out); +imu_sensor_backend_t imu_sensor_active_backend(void); +const char* imu_sensor_backend_name(imu_sensor_backend_t backend); +bool imu_sensor_quality_profile_for_backend( + imu_sensor_backend_t backend, + fusion_imu_quality_profile_t* profile_out); +bool imu_sensor_active_quality_profile( + fusion_imu_quality_profile_t* profile_out); + +#endif diff --git a/soc/software/gps_system/header/st_lsm6dsox.h b/soc/software/gps_system/header/st_lsm6dsox.h new file mode 100644 index 0000000..e498746 --- /dev/null +++ b/soc/software/gps_system/header/st_lsm6dsox.h @@ -0,0 +1,53 @@ +#ifndef ST_LSM6DSOX_H +#define ST_LSM6DSOX_H + +#include +#include + +#include "fusion_measurement.h" + +#ifndef ST_IMU_RAW_QUEUE_CAPACITY +#define ST_IMU_RAW_QUEUE_CAPACITY 256u +#endif + +typedef struct { + uint32_t odr_hz; + unsigned int accel_full_scale_g; + unsigned int gyro_full_scale_dps; +} st_lsm6dsox_config_t; + +typedef struct { + uint64_t timestamp_ticks; + int16_t temperature_raw; + int16_t accel_raw[3]; + int16_t gyro_raw[3]; + uint32_t sequence; +} st_lsm6dsox_raw_sample_t; + +typedef struct { + uint64_t irq_count; + uint64_t sample_count; + uint64_t invalid_sample_count; + uint64_t spurious_irq_count; + uint64_t queue_overflow_count; + uint64_t estimated_missed_sample_count; + uint64_t last_timestamp_ticks; + uint8_t who_am_i; + bool initialized; + bool running; +} st_lsm6dsox_diagnostics_t; + +void st_lsm6dsox_default_config(st_lsm6dsox_config_t* config); +bool st_lsm6dsox_init(const st_lsm6dsox_config_t* config); +bool st_lsm6dsox_start(void); +void st_lsm6dsox_stop(void); +/* Called only by the common PLIC ISR for BSP_PMOD_IRQ. */ +void st_lsm6dsox_irq_handler(void); +/* Retained as a no-op so every backend has the same service interface. */ +void st_lsm6dsox_service(void); +bool st_lsm6dsox_take_measurement( + fusion_imu_measurement_t* measurement_out); +void st_lsm6dsox_get_diagnostics( + st_lsm6dsox_diagnostics_t* diagnostics_out); + +#endif diff --git a/soc/software/gps_system/src/fusion_engine.c b/soc/software/gps_system/src/fusion_engine.c index 534f476..a2247e6 100644 --- a/soc/software/gps_system/src/fusion_engine.c +++ b/soc/software/gps_system/src/fusion_engine.c @@ -48,6 +48,64 @@ static uint64_t seconds_to_ticks(const fusion_engine_t* engine, double seconds) return (uint64_t)(ticks + 0.5); } +void fusion_imu_quality_default(fusion_imu_quality_profile_t* profile) +{ + if(profile == NULL) + { + return; + } + memset(profile, 0, sizeof(*profile)); + profile->name = "generic-conservative"; + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + /* Nominal fields are informational for the generic fallback. */ + profile->nominal_accel_noise_density_mps2_sqrt_hz[axis] = 0.001; + profile->nominal_gyro_noise_density_rps_sqrt_hz[axis] = 0.0001; + profile->process_accel_noise_density_mps2_sqrt_hz[axis] = 0.03; + profile->process_gyro_noise_density_rps_sqrt_hz[axis] = 0.003; + profile->accel_bias_random_walk_mps2_sqrt_s[axis] = 0.002; + profile->gyro_bias_random_walk_rps_sqrt_s[axis] = + 0.002 * FUSION_PI / 180.0; + profile->zero_gyro_measurement_std_rps[axis] = + 0.01 * FUSION_PI / 180.0; + } + profile->calibration_accel_std_limit_mps2 = 0.12; + profile->calibration_gyro_std_limit_rps = 0.15 * FUSION_PI / 180.0; +} + +bool fusion_imu_quality_is_valid(const fusion_imu_quality_profile_t* profile) +{ + if((profile == NULL) || (profile->name == NULL) || + !isfinite(profile->calibration_accel_std_limit_mps2) || + !(profile->calibration_accel_std_limit_mps2 > 0.0) || + !isfinite(profile->calibration_gyro_std_limit_rps) || + !(profile->calibration_gyro_std_limit_rps > 0.0)) + { + return false; + } + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + const double values[] = { + profile->nominal_accel_noise_density_mps2_sqrt_hz[axis], + profile->nominal_gyro_noise_density_rps_sqrt_hz[axis], + profile->process_accel_noise_density_mps2_sqrt_hz[axis], + profile->process_gyro_noise_density_rps_sqrt_hz[axis], + profile->accel_bias_random_walk_mps2_sqrt_s[axis], + profile->gyro_bias_random_walk_rps_sqrt_s[axis], + profile->zero_gyro_measurement_std_rps[axis] + }; + for(unsigned int value = 0u ; value < sizeof(values) / sizeof(values[0]); + value++) + { + if(!isfinite(values[value]) || !(values[value] > 0.0)) + { + return false; + } + } + } + return true; +} + void fusion_engine_default_config(fusion_engine_config_t* config, uint64_t timebase_hz) { @@ -77,6 +135,34 @@ void fusion_engine_default_config(fusion_engine_config_t* config, config->gnss_still_confirmations = 2u; } +bool fusion_engine_apply_imu_quality( + fusion_engine_config_t* config, + const fusion_imu_quality_profile_t* profile) +{ + if((config == NULL) || !fusion_imu_quality_is_valid(profile)) + { + return false; + } + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + config->eskf.accel_noise_density[axis] = + profile->process_accel_noise_density_mps2_sqrt_hz[axis]; + config->eskf.gyro_noise_density[axis] = + profile->process_gyro_noise_density_rps_sqrt_hz[axis]; + config->eskf.accel_bias_random_walk[axis] = + profile->accel_bias_random_walk_mps2_sqrt_s[axis]; + config->eskf.gyro_bias_random_walk[axis] = + profile->gyro_bias_random_walk_rps_sqrt_s[axis]; + config->zero_gyro_std_rps[axis] = + profile->zero_gyro_measurement_std_rps[axis]; + } + config->calibration_accel_std_limit_mps2 = + profile->calibration_accel_std_limit_mps2; + config->calibration_gyro_std_limit_rps = + profile->calibration_gyro_std_limit_rps; + return true; +} + static bool config_valid(const fusion_engine_config_t* config) { return (config != NULL) && (config->timebase_hz != 0u) && diff --git a/soc/software/gps_system/src/gps_interrupt_handlers.c b/soc/software/gps_system/src/gps_interrupt_handlers.c index 432b899..e8e787f 100644 --- a/soc/software/gps_system/src/gps_interrupt_handlers.c +++ b/soc/software/gps_system/src/gps_interrupt_handlers.c @@ -3,7 +3,7 @@ #include "Navigation.h" #include "gps_pps.h" #include "gps_time_anchor.h" -#include "tdk_icm42688.h" +#include "imu_sensor.h" #include #include #include @@ -397,7 +397,7 @@ void gps_dispatch_irq(unsigned int irq_id) { if(irq_id == BSP_PMOD_IRQ) { - tdk_icm42688_irq_handler(); + imu_sensor_irq_handler(); return; } diff --git a/soc/software/gps_system/src/imu_calibration.c b/soc/software/gps_system/src/imu_calibration.c new file mode 100644 index 0000000..1844acd --- /dev/null +++ b/soc/software/gps_system/src/imu_calibration.c @@ -0,0 +1,113 @@ +#include "imu_calibration.h" + +#include +#include +#include + +static bool vector_is_finite(const double vector[3]) +{ + return isfinite(vector[0]) && isfinite(vector[1]) && isfinite(vector[2]); +} + +static bool matrix_is_finite_and_invertible(const double matrix[3][3]) +{ + for(unsigned int row = 0 ; row < 3u ; row++) + { + if(!vector_is_finite(matrix[row])) + { + return false; + } + } + double determinant = + matrix[0][0] * (matrix[1][1] * matrix[2][2] - + matrix[1][2] * matrix[2][1]) - + matrix[0][1] * (matrix[1][0] * matrix[2][2] - + matrix[1][2] * matrix[2][0]) + + matrix[0][2] * (matrix[1][0] * matrix[2][1] - + matrix[1][1] * matrix[2][0]); + return isfinite(determinant) && (fabs(determinant) > 1.0e-12); +} + +void imu_calibration_default(imu_calibration_t* calibration) +{ + if(calibration == NULL) + { + return; + } + memset(calibration, 0, sizeof(*calibration)); + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + calibration->accel_transform[axis][axis] = 1.0; + calibration->gyro_transform[axis][axis] = 1.0; + } + calibration->reference_temperature_c = 25.0; +} + +bool imu_calibration_is_valid(const imu_calibration_t* calibration) +{ + if((calibration == NULL) || + !vector_is_finite(calibration->accel_bias_mps2) || + !vector_is_finite(calibration->gyro_bias_rps) || + !matrix_is_finite_and_invertible(calibration->accel_transform) || + !matrix_is_finite_and_invertible(calibration->gyro_transform) || + !isfinite(calibration->reference_temperature_c) || + !vector_is_finite(calibration->accel_temperature_coefficient_mps2_per_c) || + !vector_is_finite(calibration->gyro_temperature_coefficient_rps_per_c)) + { + return false; + } + return true; +} + +bool imu_calibration_apply(const imu_calibration_t* calibration, + fusion_imu_measurement_t* measurement) +{ + if(!imu_calibration_is_valid(calibration) || (measurement == NULL) || + !vector_is_finite(measurement->accel_mps2) || + !vector_is_finite(measurement->gyro_rps)) + { + return false; + } + double temperature_delta_c = 0.0; + if(calibration->temperature_compensation_enabled) + { + if(!isfinite(measurement->temperature_c)) + { + return false; + } + temperature_delta_c = + measurement->temperature_c - calibration->reference_temperature_c; + } + + double accel_unbiased[3]; + double gyro_unbiased[3]; + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + accel_unbiased[axis] = + measurement->accel_mps2[axis] - calibration->accel_bias_mps2[axis] - + calibration->accel_temperature_coefficient_mps2_per_c[axis] * + temperature_delta_c; + gyro_unbiased[axis] = + measurement->gyro_rps[axis] - calibration->gyro_bias_rps[axis] - + calibration->gyro_temperature_coefficient_rps_per_c[axis] * + temperature_delta_c; + } + + double accel_corrected[3] = {0.0}; + double gyro_corrected[3] = {0.0}; + for(unsigned int row = 0 ; row < 3u ; row++) + { + for(unsigned int col = 0 ; col < 3u ; col++) + { + accel_corrected[row] += + calibration->accel_transform[row][col] * accel_unbiased[col]; + gyro_corrected[row] += + calibration->gyro_transform[row][col] * gyro_unbiased[col]; + } + } + memcpy(measurement->accel_mps2, accel_corrected, + sizeof(measurement->accel_mps2)); + memcpy(measurement->gyro_rps, gyro_corrected, + sizeof(measurement->gyro_rps)); + return true; +} diff --git a/soc/software/gps_system/src/imu_sensor.c b/soc/software/gps_system/src/imu_sensor.c new file mode 100644 index 0000000..f663d9d --- /dev/null +++ b/soc/software/gps_system/src/imu_sensor.c @@ -0,0 +1,350 @@ +#include "imu_sensor.h" + +#include +#include + +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK) +#include "tdk_icm42688.h" +#endif + +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST) +#include "st_lsm6dsox.h" +#endif + +#if !defined(IMU_SENSOR_BUILD_AUTO) && !defined(IMU_SENSOR_BUILD_TDK) && \ + !defined(IMU_SENSOR_BUILD_ST) +#error "Select IMU_SENSOR_BUILD_AUTO, IMU_SENSOR_BUILD_TDK or IMU_SENSOR_BUILD_ST" +#endif + +static imu_sensor_backend_t active_backend; +static imu_calibration_t active_calibration; +static uint32_t active_odr_hz; +static uint64_t calibration_rejected_count; + +void imu_sensor_default_config(imu_sensor_config_t* config) +{ + if(config == NULL) + { + return; + } + memset(config, 0, sizeof(*config)); + for(unsigned int backend = 0u ; backend < IMU_SENSOR_BACKEND_COUNT ; backend++) + { + imu_calibration_default(&config->calibration_by_backend[backend]); + } +} + +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK) +static bool tdk_accel_range(unsigned int full_scale_g, + tdk_accel_full_scale_t* range) +{ + if(range == NULL) + { + return false; + } + switch(full_scale_g) + { + case 2u: *range = TDK_ACCEL_FS_2G; return true; + case 4u: *range = TDK_ACCEL_FS_4G; return true; + case 8u: *range = TDK_ACCEL_FS_8G; return true; + case 16u: *range = TDK_ACCEL_FS_16G; return true; + default: return false; + } +} + +static bool tdk_gyro_range(unsigned int full_scale_dps, + tdk_gyro_full_scale_mdps_t* range) +{ + if(range == NULL) + { + return false; + } + switch(full_scale_dps) + { + case 125u: *range = TDK_GYRO_FS_125; return true; + case 250u: *range = TDK_GYRO_FS_250; return true; + case 500u: *range = TDK_GYRO_FS_500; return true; + case 1000u: *range = TDK_GYRO_FS_1000; return true; + case 2000u: *range = TDK_GYRO_FS_2000; return true; + default: return false; + } +} + +static bool try_tdk(const imu_sensor_config_t* config) +{ + if(!imu_calibration_is_valid( + &config->calibration_by_backend[IMU_SENSOR_TDK_ICM42688])) + { + return false; + } + tdk_icm42688_config_t backend_config; + tdk_icm42688_default_config(&backend_config); + if(config->odr_hz != 0u) + { + backend_config.odr_hz = config->odr_hz; + } + if((config->accel_full_scale_g != 0u) && + !tdk_accel_range(config->accel_full_scale_g, + &backend_config.accel_full_scale_g)) + { + return false; + } + if((config->gyro_full_scale_dps != 0u) && + !tdk_gyro_range(config->gyro_full_scale_dps, + &backend_config.gyro_full_scale_mdps)) + { + return false; + } + if(!tdk_icm42688_init(&backend_config)) + { + return false; + } + active_backend = IMU_SENSOR_TDK_ICM42688; + active_calibration = + config->calibration_by_backend[IMU_SENSOR_TDK_ICM42688]; + active_odr_hz = backend_config.odr_hz; + return true; +} +#endif + +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST) +static bool try_st(const imu_sensor_config_t* config) +{ + if(!imu_calibration_is_valid( + &config->calibration_by_backend[IMU_SENSOR_ST_LSM6DSOX])) + { + return false; + } + st_lsm6dsox_config_t backend_config; + st_lsm6dsox_default_config(&backend_config); + if(config->odr_hz != 0u) + { + backend_config.odr_hz = config->odr_hz; + } + if(config->accel_full_scale_g != 0u) + { + backend_config.accel_full_scale_g = config->accel_full_scale_g; + } + if(config->gyro_full_scale_dps != 0u) + { + backend_config.gyro_full_scale_dps = config->gyro_full_scale_dps; + } + if(!st_lsm6dsox_init(&backend_config)) + { + return false; + } + active_backend = IMU_SENSOR_ST_LSM6DSOX; + active_calibration = + config->calibration_by_backend[IMU_SENSOR_ST_LSM6DSOX]; + active_odr_hz = backend_config.odr_hz; + return true; +} +#endif + +bool imu_sensor_init(const imu_sensor_config_t* config) +{ + active_backend = IMU_SENSOR_NONE; + active_odr_hz = 0u; + calibration_rejected_count = 0u; + if(config == NULL) + { + return false; + } + +#if defined(IMU_SENSOR_BUILD_TDK) + return try_tdk(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)) + { + return true; + } + return try_st(config); +#endif +} + +bool imu_sensor_start(void) +{ + switch(active_backend) + { +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK) + case IMU_SENSOR_TDK_ICM42688: + return tdk_icm42688_start(); +#endif +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST) + case IMU_SENSOR_ST_LSM6DSOX: + return st_lsm6dsox_start(); +#endif + default: + return false; + } +} + +void imu_sensor_stop(void) +{ + switch(active_backend) + { +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK) + case IMU_SENSOR_TDK_ICM42688: + tdk_icm42688_stop(); + break; +#endif +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST) + case IMU_SENSOR_ST_LSM6DSOX: + st_lsm6dsox_stop(); + break; +#endif + default: + break; + } +} + +void imu_sensor_irq_handler(void) +{ +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK) + if(active_backend == IMU_SENSOR_TDK_ICM42688) + { + tdk_icm42688_irq_handler(); + } +#endif +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST) + if(active_backend == IMU_SENSOR_ST_LSM6DSOX) + { + st_lsm6dsox_irq_handler(); + } +#endif +} + +void imu_sensor_service(void) +{ +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST) + if(active_backend == IMU_SENSOR_ST_LSM6DSOX) + { + st_lsm6dsox_service(); + } +#endif +} + +bool imu_sensor_requires_foreground_service(void) +{ + return false; +} + +bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out) +{ + if(measurement_out == NULL) + { + return false; + } + bool have_measurement = false; + switch(active_backend) + { +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK) + case IMU_SENSOR_TDK_ICM42688: + have_measurement = + tdk_icm42688_take_measurement(measurement_out); + break; +#endif +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST) + case IMU_SENSOR_ST_LSM6DSOX: + have_measurement = + st_lsm6dsox_take_measurement(measurement_out); + break; +#endif + default: + break; + } + if(!have_measurement) + { + return false; + } + if(!imu_calibration_apply(&active_calibration, measurement_out)) + { + calibration_rejected_count++; + return false; + } + return true; +} + +void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out) +{ + if(diagnostics_out == NULL) + { + return; + } + memset(diagnostics_out, 0, sizeof(*diagnostics_out)); + diagnostics_out->backend = active_backend; + diagnostics_out->configured_odr_hz = active_odr_hz; + diagnostics_out->foreground_polling = + imu_sensor_requires_foreground_service(); + + switch(active_backend) + { +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK) + case IMU_SENSOR_TDK_ICM42688: + { + tdk_icm42688_diagnostics_t source; + tdk_icm42688_get_diagnostics(&source); + diagnostics_out->irq_count = source.irq_count; + diagnostics_out->sample_count = source.sample_count; + diagnostics_out->invalid_sample_count = + source.invalid_sample_count + calibration_rejected_count; + diagnostics_out->spurious_irq_count = source.spurious_irq_count; + diagnostics_out->queue_overflow_count = source.queue_overflow_count; + diagnostics_out->estimated_missed_sample_count = + source.estimated_missed_sample_count; + diagnostics_out->last_timestamp_ticks = source.last_timestamp_ticks; + diagnostics_out->who_am_i = source.who_am_i; + diagnostics_out->initialized = source.initialized; + diagnostics_out->running = source.running; + break; + } +#endif +#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST) + case IMU_SENSOR_ST_LSM6DSOX: + { + st_lsm6dsox_diagnostics_t source; + st_lsm6dsox_get_diagnostics(&source); + diagnostics_out->irq_count = source.irq_count; + diagnostics_out->sample_count = source.sample_count; + diagnostics_out->invalid_sample_count = + source.invalid_sample_count + calibration_rejected_count; + diagnostics_out->spurious_irq_count = source.spurious_irq_count; + diagnostics_out->queue_overflow_count = source.queue_overflow_count; + diagnostics_out->estimated_missed_sample_count = + source.estimated_missed_sample_count; + diagnostics_out->last_timestamp_ticks = source.last_timestamp_ticks; + diagnostics_out->who_am_i = source.who_am_i; + diagnostics_out->initialized = source.initialized; + diagnostics_out->running = source.running; + break; + } +#endif + default: + diagnostics_out->invalid_sample_count = calibration_rejected_count; + break; + } +} + +imu_sensor_backend_t imu_sensor_active_backend(void) +{ + return active_backend; +} + +bool imu_sensor_active_quality_profile( + fusion_imu_quality_profile_t* profile_out) +{ + return imu_sensor_quality_profile_for_backend(active_backend, profile_out); +} + +const char* imu_sensor_backend_name(imu_sensor_backend_t backend) +{ + switch(backend) + { + case IMU_SENSOR_TDK_ICM42688: return "tdk-icm42688"; + case IMU_SENSOR_ST_LSM6DSOX: return "st-lsm6dsox"; + default: return "none"; + } +} diff --git a/soc/software/gps_system/src/imu_sensor_profile.c b/soc/software/gps_system/src/imu_sensor_profile.c new file mode 100644 index 0000000..afee6b7 --- /dev/null +++ b/soc/software/gps_system/src/imu_sensor_profile.c @@ -0,0 +1,74 @@ +#include "imu_sensor.h" + +#include + +#define PROFILE_GRAVITY_MPS2 9.80665 +#define PROFILE_DEGREES_TO_RADIANS 0.017453292519943295769236907684886 + +static void set_all(double values[3], double value) +{ + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + values[axis] = value; + } +} + +bool imu_sensor_quality_profile_for_backend( + imu_sensor_backend_t backend, + fusion_imu_quality_profile_t* profile_out) +{ + if(profile_out == NULL) + { + return false; + } + fusion_imu_quality_default(profile_out); + switch(backend) + { + case IMU_SENSOR_TDK_ICM42688: + profile_out->name = "tdk-icm42688"; + /* ICM-42688-P: 65/65/70 ug/sqrt(Hz), 2.8 mdps/sqrt(Hz). */ + profile_out->nominal_accel_noise_density_mps2_sqrt_hz[0] = + 65.0e-6 * PROFILE_GRAVITY_MPS2; + profile_out->nominal_accel_noise_density_mps2_sqrt_hz[1] = + 65.0e-6 * PROFILE_GRAVITY_MPS2; + profile_out->nominal_accel_noise_density_mps2_sqrt_hz[2] = + 70.0e-6 * PROFILE_GRAVITY_MPS2; + set_all(profile_out->nominal_gyro_noise_density_rps_sqrt_hz, + 2.8e-3 * PROFILE_DEGREES_TO_RADIANS); + /* Conservative fusion values; tune bias RW with long Allan logs. */ + set_all(profile_out->process_accel_noise_density_mps2_sqrt_hz, 0.025); + set_all(profile_out->process_gyro_noise_density_rps_sqrt_hz, 0.0022); + set_all(profile_out->accel_bias_random_walk_mps2_sqrt_s, 0.0015); + set_all(profile_out->gyro_bias_random_walk_rps_sqrt_s, + 0.002 * PROFILE_DEGREES_TO_RADIANS); + profile_out->calibration_accel_std_limit_mps2 = 0.09; + profile_out->calibration_gyro_std_limit_rps = + 0.10 * PROFILE_DEGREES_TO_RADIANS; + set_all(profile_out->zero_gyro_measurement_std_rps, + 0.02 * PROFILE_DEGREES_TO_RADIANS); + break; + + case IMU_SENSOR_ST_LSM6DSOX: + profile_out->name = "st-lsm6dsox"; + /* LSM6DSOX at +-2 g: 70 ug/sqrt(Hz), 3.8 mdps/sqrt(Hz). */ + set_all(profile_out->nominal_accel_noise_density_mps2_sqrt_hz, + 70.0e-6 * PROFILE_GRAVITY_MPS2); + set_all(profile_out->nominal_gyro_noise_density_rps_sqrt_hz, + 3.8e-3 * PROFILE_DEGREES_TO_RADIANS); + set_all(profile_out->process_accel_noise_density_mps2_sqrt_hz, 0.03); + set_all(profile_out->process_gyro_noise_density_rps_sqrt_hz, 0.0030); + set_all(profile_out->accel_bias_random_walk_mps2_sqrt_s, 0.0025); + set_all(profile_out->gyro_bias_random_walk_rps_sqrt_s, + 0.004 * PROFILE_DEGREES_TO_RADIANS); + profile_out->calibration_accel_std_limit_mps2 = 0.12; + profile_out->calibration_gyro_std_limit_rps = + 0.20 * PROFILE_DEGREES_TO_RADIANS; + set_all(profile_out->zero_gyro_measurement_std_rps, + 0.04 * PROFILE_DEGREES_TO_RADIANS); + break; + + default: + return false; + } + return fusion_imu_quality_is_valid(profile_out); +} diff --git a/soc/software/gps_system/src/navigation_fusion.c b/soc/software/gps_system/src/navigation_fusion.c index cd6ee99..6387cbc 100644 --- a/soc/software/gps_system/src/navigation_fusion.c +++ b/soc/software/gps_system/src/navigation_fusion.c @@ -2,7 +2,7 @@ #include "fusion_bus.h" #include "fusion_gps_adapter.h" -#include "tdk_icm42688.h" +#include "imu_sensor.h" #include #include @@ -23,6 +23,12 @@ bool navigation_fusion_init(uint64_t local_timebase_hz) } fusion_bus_init(&measurement_bus); fusion_engine_default_config(&engine_config, local_timebase_hz); + fusion_imu_quality_profile_t imu_profile; + if(imu_sensor_active_quality_profile(&imu_profile) && + !fusion_engine_apply_imu_quality(&engine_config, &imu_profile)) + { + return false; + } fusion_gps_adapter_default_config(&adapter_config); if(!fusion_engine_init(&fusion_engine, &engine_config) || !fusion_gps_adapter_init(&gps_adapter, &adapter_config)) @@ -46,8 +52,9 @@ unsigned int navigation_fusion_service(void) return 0u; } unsigned int processed = 0u; + imu_sensor_service(); fusion_imu_measurement_t imu; - while(tdk_icm42688_take_measurement(&imu)) + while(imu_sensor_take_measurement(&imu)) { (void)fusion_engine_push_imu(&fusion_engine, &imu); processed++; diff --git a/soc/software/gps_system/src/st_lsm6dsox.c b/soc/software/gps_system/src/st_lsm6dsox.c new file mode 100644 index 0000000..e141271 --- /dev/null +++ b/soc/software/gps_system/src/st_lsm6dsox.c @@ -0,0 +1,503 @@ +#include "st_lsm6dsox.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#define LSM6DSOX_WHO_AM_I 0x0fu +#define LSM6DSOX_COUNTER_BDR_REG1 0x0bu +#define LSM6DSOX_INT1_CTRL 0x0du +#define LSM6DSOX_INT2_CTRL 0x0eu +#define LSM6DSOX_CTRL1_XL 0x10u +#define LSM6DSOX_CTRL2_G 0x11u +#define LSM6DSOX_CTRL3_C 0x12u +#define LSM6DSOX_STATUS_REG 0x1eu +#define LSM6DSOX_TAP_CFG2 0x58u +#define LSM6DSOX_OUT_TEMP_L 0x20u +#define LSM6DSOX_OUTX_L_G 0x22u +#define LSM6DSOX_OUTX_L_A 0x28u + +#define LSM6DSOX_SPI_READ 0x80u +#define LSM6DSOX_SPI_DUMMY 0xffu +#define LSM6DSOX_WHO_AM_I_VALUE 0x6cu +#define LSM6DSOX_CTRL3_SW_RESET 0x01u +#define LSM6DSOX_CTRL3_IF_INC 0x04u +#define LSM6DSOX_CTRL3_BDU 0x40u +#define LSM6DSOX_STATUS_XLDA 0x01u +#define LSM6DSOX_STATUS_GDA 0x02u +#define LSM6DSOX_INT1_DRDY_XL 0x01u +#define LSM6DSOX_DATAREADY_PULSED 0x80u +#define LSM6DSOX_INTERRUPTS_ENABLE 0x80u +#define LSM6DSOX_GYRO_READY_TIMEOUT_US 100u + +#define ST_GRAVITY_MPS2 9.80665 +#define ST_DEGREES_TO_RADIANS 0.017453292519943295769236907684886 + +#if (ST_IMU_RAW_QUEUE_CAPACITY < 2u) +#error "ST_IMU_RAW_QUEUE_CAPACITY must be at least 2" +#endif + +static st_lsm6dsox_config_t active_config; +static st_lsm6dsox_raw_sample_t sample_queue[ST_IMU_RAW_QUEUE_CAPACITY]; +static volatile unsigned int sample_head; +static volatile unsigned int sample_tail; +static volatile st_lsm6dsox_diagnostics_t diagnostics; + +static uint64_t time_now(void) +{ + return bsp_read64(BSP_CLINT_MTIME); +} + +static void delay_ms(uint32_t milliseconds) +{ + uint64_t ticks = ((uint64_t)BSP_CLINT_TIMEBASE_HZ * milliseconds + 999u) / + 1000u; + uint64_t start = time_now(); + while((time_now() - start) < ticks) + { + } +} + +static void write_register(uint8_t reg, uint8_t value) +{ + bsp_pmod_spi_write_reg8(reg & (uint8_t)~LSM6DSOX_SPI_READ, value); +} + +static uint8_t read_register(uint8_t reg) +{ + return bsp_pmod_spi_read_reg8(reg | LSM6DSOX_SPI_READ, + LSM6DSOX_SPI_DUMMY); +} + +static int16_t little_endian_i16(const uint8_t* bytes) +{ + return (int16_t)((uint16_t)bytes[0] | ((uint16_t)bytes[1] << 8)); +} + +static bool odr_register_value(uint32_t odr_hz, uint8_t* value) +{ + if(value == NULL) + { + return false; + } + switch(odr_hz) + { + case 52u: *value = 0x30u; return true; + case 104u: *value = 0x40u; return true; + case 208u: *value = 0x50u; return true; + default: return false; + } +} + +static bool accel_full_scale_bits(unsigned int full_scale_g, uint8_t* bits, + double* mg_per_lsb) +{ + if((bits == NULL) || (mg_per_lsb == NULL)) + { + return false; + } + switch(full_scale_g) + { + case 2u: *bits = 0x00u; *mg_per_lsb = 0.061; return true; + case 4u: *bits = 0x08u; *mg_per_lsb = 0.122; return true; + case 8u: *bits = 0x0cu; *mg_per_lsb = 0.244; return true; + case 16u: *bits = 0x04u; *mg_per_lsb = 0.488; return true; + default: return false; + } +} + +static bool gyro_full_scale_bits(unsigned int full_scale_dps, uint8_t* bits, + double* mdps_per_lsb) +{ + if((bits == NULL) || (mdps_per_lsb == NULL)) + { + return false; + } + switch(full_scale_dps) + { + case 125u: *bits = 0x02u; *mdps_per_lsb = 4.375; return true; + case 250u: *bits = 0x00u; *mdps_per_lsb = 8.750; return true; + case 500u: *bits = 0x04u; *mdps_per_lsb = 17.50; return true; + case 1000u: *bits = 0x08u; *mdps_per_lsb = 35.00; return true; + case 2000u: *bits = 0x0cu; *mdps_per_lsb = 70.00; return true; + default: return false; + } +} + +static unsigned int next_queue_index(unsigned int index) +{ + index++; + return (index == ST_IMU_RAW_QUEUE_CAPACITY) ? 0u : index; +} + +static bool raw_sample_valid(const st_lsm6dsox_raw_sample_t* sample) +{ + if((sample == NULL) || (sample->temperature_raw == INT16_MIN)) + { + return false; + } + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + if((sample->accel_raw[axis] == INT16_MIN) || + (sample->gyro_raw[axis] == INT16_MIN)) + { + return false; + } + } + return true; +} + +static bool queue_push_isr(const st_lsm6dsox_raw_sample_t* sample) +{ + unsigned int head = __atomic_load_n(&sample_head, __ATOMIC_RELAXED); + unsigned int next = next_queue_index(head); + if(next == __atomic_load_n(&sample_tail, __ATOMIC_ACQUIRE)) + { + __atomic_fetch_add(&diagnostics.queue_overflow_count, 1u, + __ATOMIC_RELAXED); + return false; + } + sample_queue[head] = *sample; + __atomic_store_n(&sample_head, next, __ATOMIC_RELEASE); + return true; +} + +static bool queue_pop(st_lsm6dsox_raw_sample_t* sample) +{ + unsigned int tail = __atomic_load_n(&sample_tail, __ATOMIC_RELAXED); + if(tail == __atomic_load_n(&sample_head, __ATOMIC_ACQUIRE)) + { + return false; + } + *sample = sample_queue[tail]; + __atomic_store_n(&sample_tail, next_queue_index(tail), __ATOMIC_RELEASE); + return true; +} + +static void read_raw_outputs(st_lsm6dsox_raw_sample_t* sample, + uint64_t timestamp_ticks) +{ + uint8_t bytes[14]; + bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ, + LSM6DSOX_SPI_DUMMY, bytes, 2u); + /* Temperature and gyro/accel are separate internal output blocks. */ + bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_G | LSM6DSOX_SPI_READ, + LSM6DSOX_SPI_DUMMY, &bytes[2], 12u); + sample->timestamp_ticks = timestamp_ticks; + sample->temperature_raw = little_endian_i16(&bytes[0]); + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + sample->gyro_raw[axis] = little_endian_i16(&bytes[2u + 2u * axis]); + sample->accel_raw[axis] = little_endian_i16(&bytes[8u + 2u * axis]); + } +} + +static void read_raw_accel(st_lsm6dsox_raw_sample_t* sample) +{ + uint8_t bytes[6]; + bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_A | LSM6DSOX_SPI_READ, + LSM6DSOX_SPI_DUMMY, bytes, sizeof(bytes)); + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + sample->accel_raw[axis] = little_endian_i16(&bytes[2u * axis]); + } +} + +static void read_raw_gyro_temperature(st_lsm6dsox_raw_sample_t* sample) +{ + uint8_t bytes[8]; + bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ, + LSM6DSOX_SPI_DUMMY, bytes, 2u); + bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_G | LSM6DSOX_SPI_READ, + LSM6DSOX_SPI_DUMMY, &bytes[2], 6u); + sample->temperature_raw = little_endian_i16(&bytes[0]); + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + sample->gyro_raw[axis] = little_endian_i16(&bytes[2u + 2u * axis]); + } +} + +void st_lsm6dsox_default_config(st_lsm6dsox_config_t* config) +{ + if(config == NULL) + { + return; + } + config->odr_hz = 104u; + config->accel_full_scale_g = 2u; + config->gyro_full_scale_dps = 250u; +} + +bool st_lsm6dsox_init(const st_lsm6dsox_config_t* config) +{ + uint8_t odr_bits; + uint8_t accel_bits; + uint8_t gyro_bits; + double unused_accel_scale; + double unused_gyro_scale; + if((config == NULL) || !odr_register_value(config->odr_hz, &odr_bits) || + !accel_full_scale_bits(config->accel_full_scale_g, &accel_bits, + &unused_accel_scale) || + !gyro_full_scale_bits(config->gyro_full_scale_dps, &gyro_bits, + &unused_gyro_scale)) + { + return false; + } + memset((void*)&diagnostics, 0, sizeof(diagnostics)); + sample_head = 0u; + sample_tail = 0u; + active_config = *config; + + bsp_pmod_spi_init(); + bsp_pmod_spi_set_mode(BSP_PMOD_SPI_MODE_SINGLE); + bsp_pmod_spi_set_clock_mode(BSP_PMOD_SPI_CLOCK_MODE0); + bsp_pmod_spi_set_timing(2u, 2u, 2u, 5u); + delay_ms(20u); + diagnostics.who_am_i = read_register(LSM6DSOX_WHO_AM_I); + if(diagnostics.who_am_i != LSM6DSOX_WHO_AM_I_VALUE) + { + return false; + } + + write_register(LSM6DSOX_CTRL3_C, LSM6DSOX_CTRL3_SW_RESET); + uint64_t reset_start = time_now(); + uint64_t reset_timeout = + ((uint64_t)BSP_CLINT_TIMEBASE_HZ * 100u + 999u) / 1000u; + while((read_register(LSM6DSOX_CTRL3_C) & LSM6DSOX_CTRL3_SW_RESET) != 0u) + { + if((time_now() - reset_start) >= reset_timeout) + { + return false; + } + } + delay_ms(10u); + + write_register(LSM6DSOX_CTRL3_C, + LSM6DSOX_CTRL3_BDU | LSM6DSOX_CTRL3_IF_INC); + /* Active-high push-pull is the CTRL3_C reset polarity/driver setting. */ + write_register(LSM6DSOX_INT1_CTRL, 0u); + write_register(LSM6DSOX_INT2_CTRL, 0u); + write_register(LSM6DSOX_TAP_CFG2, + read_register(LSM6DSOX_TAP_CFG2) & + (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE); + write_register(LSM6DSOX_COUNTER_BDR_REG1, 0u); + write_register(LSM6DSOX_CTRL1_XL, (uint8_t)(odr_bits | accel_bits)); + write_register(LSM6DSOX_CTRL2_G, (uint8_t)(odr_bits | gyro_bits)); + delay_ms(100u); + if((read_register(LSM6DSOX_CTRL3_C) != + (LSM6DSOX_CTRL3_BDU | LSM6DSOX_CTRL3_IF_INC)) || + (read_register(LSM6DSOX_CTRL1_XL) != (uint8_t)(odr_bits | accel_bits)) || + (read_register(LSM6DSOX_CTRL2_G) != (uint8_t)(odr_bits | gyro_bits)) || + (read_register(LSM6DSOX_INT1_CTRL) != 0u) || + (read_register(LSM6DSOX_INT2_CTRL) != 0u) || + ((read_register(LSM6DSOX_TAP_CFG2) & + LSM6DSOX_INTERRUPTS_ENABLE) != 0u) || + ((read_register(LSM6DSOX_COUNTER_BDR_REG1) & + LSM6DSOX_DATAREADY_PULSED) != 0u)) + { + st_lsm6dsox_stop(); + return false; + } + diagnostics.initialized = true; + return true; +} + +bool st_lsm6dsox_start(void) +{ + if(!diagnostics.initialized || diagnostics.running) + { + return diagnostics.running; + } + bsp_plic_disable(BSP_PMOD_IRQ); + write_register(LSM6DSOX_INT1_CTRL, 0u); + write_register(LSM6DSOX_INT2_CTRL, 0u); + write_register(LSM6DSOX_TAP_CFG2, + read_register(LSM6DSOX_TAP_CFG2) & + (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE); + write_register(LSM6DSOX_COUNTER_BDR_REG1, 0u); + /* Clear an old latched data-ready before exposing the PLIC source. */ + if((read_register(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_XLDA) != 0u) + { + st_lsm6dsox_raw_sample_t stale; + read_raw_outputs(&stale, time_now()); + } + /* Route accel DRDY to the wired sensor INT1 pad, then globally enable the + * physical interrupt pads as required by ST's reference driver. */ + write_register(LSM6DSOX_INT1_CTRL, LSM6DSOX_INT1_DRDY_XL); + write_register(LSM6DSOX_TAP_CFG2, + read_register(LSM6DSOX_TAP_CFG2) | + LSM6DSOX_INTERRUPTS_ENABLE); + if((read_register(LSM6DSOX_INT1_CTRL) != LSM6DSOX_INT1_DRDY_XL) || + (read_register(LSM6DSOX_INT2_CTRL) != 0u) || + ((read_register(LSM6DSOX_TAP_CFG2) & + LSM6DSOX_INTERRUPTS_ENABLE) == 0u) || + ((read_register(LSM6DSOX_COUNTER_BDR_REG1) & + LSM6DSOX_DATAREADY_PULSED) != 0u)) + { + write_register(LSM6DSOX_INT1_CTRL, 0u); + write_register(LSM6DSOX_INT2_CTRL, 0u); + write_register(LSM6DSOX_TAP_CFG2, + read_register(LSM6DSOX_TAP_CFG2) & + (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE); + return false; + } + bsp_plic_set_priority(BSP_PMOD_IRQ, 1u); + diagnostics.running = true; + bsp_plic_enable(BSP_PMOD_IRQ); + return true; +} + +void st_lsm6dsox_stop(void) +{ + bsp_plic_disable(BSP_PMOD_IRQ); + if(diagnostics.who_am_i == LSM6DSOX_WHO_AM_I_VALUE) + { + write_register(LSM6DSOX_INT1_CTRL, 0u); + write_register(LSM6DSOX_INT2_CTRL, 0u); + write_register(LSM6DSOX_TAP_CFG2, + read_register(LSM6DSOX_TAP_CFG2) & + (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE); + write_register(LSM6DSOX_CTRL1_XL, 0u); + write_register(LSM6DSOX_CTRL2_G, 0u); + } + diagnostics.running = false; +} + +void st_lsm6dsox_irq_handler(void) +{ + const uint64_t timestamp = time_now(); + __atomic_fetch_add(&diagnostics.irq_count, 1u, __ATOMIC_RELAXED); + if(!__atomic_load_n(&diagnostics.running, __ATOMIC_RELAXED)) + { + __atomic_fetch_add(&diagnostics.spurious_irq_count, 1u, + __ATOMIC_RELAXED); + return; + } + uint8_t status = read_register(LSM6DSOX_STATUS_REG); + st_lsm6dsox_raw_sample_t sample; + sample.timestamp_ticks = timestamp; + /* Always clear the routed, latched accel source before returning. */ + read_raw_accel(&sample); + if((status & LSM6DSOX_STATUS_XLDA) == 0u) + { + __atomic_fetch_add(&diagnostics.spurious_irq_count, 1u, + __ATOMIC_RELAXED); + return; + } + uint64_t wait_start = time_now(); + uint64_t timeout_ticks = + ((uint64_t)BSP_CLINT_TIMEBASE_HZ * LSM6DSOX_GYRO_READY_TIMEOUT_US + + 999999u) / 1000000u; + while((read_register(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_GDA) == 0u) + { + if((time_now() - wait_start) >= timeout_ticks) + { + __atomic_fetch_add(&diagnostics.invalid_sample_count, 1u, + __ATOMIC_RELAXED); + return; + } + } + read_raw_gyro_temperature(&sample); + if(!raw_sample_valid(&sample)) + { + __atomic_fetch_add(&diagnostics.invalid_sample_count, 1u, + __ATOMIC_RELAXED); + return; + } + sample.sequence = (uint32_t)( + __atomic_load_n(&diagnostics.sample_count, __ATOMIC_RELAXED) + 1u); + uint64_t previous_timestamp = __atomic_exchange_n( + &diagnostics.last_timestamp_ticks, timestamp, __ATOMIC_RELAXED); + if(previous_timestamp != 0u) + { + uint64_t expected_ticks = BSP_CLINT_TIMEBASE_HZ / active_config.odr_hz; + uint64_t elapsed = timestamp - previous_timestamp; + if((expected_ticks != 0u) && + (elapsed > expected_ticks + expected_ticks / 2u)) + { + uint64_t periods = (elapsed + expected_ticks / 2u) / expected_ticks; + if(periods > 1u) + { + __atomic_fetch_add(&diagnostics.estimated_missed_sample_count, + periods - 1u, __ATOMIC_RELAXED); + } + } + } + (void)queue_push_isr(&sample); + __atomic_fetch_add(&diagnostics.sample_count, 1u, __ATOMIC_RELAXED); +} + +void st_lsm6dsox_service(void) +{ +} + +bool st_lsm6dsox_take_measurement( + fusion_imu_measurement_t* measurement_out) +{ + if(measurement_out == NULL) + { + return false; + } + st_lsm6dsox_raw_sample_t raw; + if(!queue_pop(&raw)) + { + return false; + } + double accel_mg_per_lsb; + double gyro_mdps_per_lsb; + uint8_t unused_bits; + if(!accel_full_scale_bits(active_config.accel_full_scale_g, &unused_bits, + &accel_mg_per_lsb) || + !gyro_full_scale_bits(active_config.gyro_full_scale_dps, &unused_bits, + &gyro_mdps_per_lsb)) + { + __atomic_fetch_add(&diagnostics.invalid_sample_count, 1u, + __ATOMIC_RELAXED); + return false; + } + memset(measurement_out, 0, sizeof(*measurement_out)); + measurement_out->timestamp_ticks = raw.timestamp_ticks; + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + measurement_out->accel_mps2[axis] = + (double)raw.accel_raw[axis] * accel_mg_per_lsb * + ST_GRAVITY_MPS2 / 1000.0; + measurement_out->gyro_rps[axis] = + (double)raw.gyro_raw[axis] * gyro_mdps_per_lsb * + ST_DEGREES_TO_RADIANS / 1000.0; + } + measurement_out->temperature_c = + 25.0 + (double)raw.temperature_raw / 256.0; + measurement_out->sequence = raw.sequence; + return true; +} + +void st_lsm6dsox_get_diagnostics( + st_lsm6dsox_diagnostics_t* diagnostics_out) +{ + if(diagnostics_out != NULL) + { + diagnostics_out->irq_count = + __atomic_load_n(&diagnostics.irq_count, __ATOMIC_RELAXED); + diagnostics_out->sample_count = + __atomic_load_n(&diagnostics.sample_count, __ATOMIC_RELAXED); + diagnostics_out->invalid_sample_count = + __atomic_load_n(&diagnostics.invalid_sample_count, __ATOMIC_RELAXED); + diagnostics_out->spurious_irq_count = + __atomic_load_n(&diagnostics.spurious_irq_count, __ATOMIC_RELAXED); + diagnostics_out->queue_overflow_count = + __atomic_load_n(&diagnostics.queue_overflow_count, __ATOMIC_RELAXED); + diagnostics_out->estimated_missed_sample_count = __atomic_load_n( + &diagnostics.estimated_missed_sample_count, __ATOMIC_RELAXED); + diagnostics_out->last_timestamp_ticks = + __atomic_load_n(&diagnostics.last_timestamp_ticks, __ATOMIC_RELAXED); + diagnostics_out->who_am_i = diagnostics.who_am_i; + diagnostics_out->initialized = diagnostics.initialized; + diagnostics_out->running = diagnostics.running; + } +} diff --git a/soc/software/gps_system/tests/Makefile b/soc/software/gps_system/tests/Makefile new file mode 100644 index 0000000..67425cc --- /dev/null +++ b/soc/software/gps_system/tests/Makefile @@ -0,0 +1,18 @@ +CC ?= gcc +CFLAGS ?= -O2 -std=c11 -Wall -Wextra -Werror + +ROOT := .. +TARGET := test_imu_calibration + +.PHONY: all run clean + +all: $(TARGET) + +$(TARGET): test_imu_calibration.c $(ROOT)/src/imu_calibration.c + $(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@ + +run: $(TARGET) + ./$(TARGET) + +clean: + rm -f $(TARGET) diff --git a/soc/software/gps_system/tests/test_imu_calibration.c b/soc/software/gps_system/tests/test_imu_calibration.c new file mode 100644 index 0000000..a7ac7e5 --- /dev/null +++ b/soc/software/gps_system/tests/test_imu_calibration.c @@ -0,0 +1,78 @@ +#include "imu_calibration.h" + +#include +#include +#include +#include + +static bool near(double actual, double expected) +{ + return fabs(actual - expected) < 1.0e-12; +} + +static void test_identity_is_noop(void) +{ + imu_calibration_t calibration; + imu_calibration_default(&calibration); + fusion_imu_measurement_t measurement = { + .accel_mps2 = {1.0, -2.0, 9.7}, + .gyro_rps = {0.01, -0.02, 0.03}, + .temperature_c = 41.0 + }; + fusion_imu_measurement_t expected = measurement; + assert(imu_calibration_apply(&calibration, &measurement)); + assert(memcmp(measurement.accel_mps2, expected.accel_mps2, + sizeof(measurement.accel_mps2)) == 0); + assert(memcmp(measurement.gyro_rps, expected.gyro_rps, + sizeof(measurement.gyro_rps)) == 0); +} + +static void test_bias_transform_and_temperature(void) +{ + imu_calibration_t calibration; + imu_calibration_default(&calibration); + for(unsigned int axis = 0 ; axis < 3u ; axis++) + { + calibration.accel_bias_mps2[axis] = 1.0; + calibration.gyro_bias_rps[axis] = 0.1; + calibration.accel_temperature_coefficient_mps2_per_c[axis] = 0.1; + calibration.gyro_temperature_coefficient_rps_per_c[axis] = 0.01; + } + memset(calibration.accel_transform, 0, + sizeof(calibration.accel_transform)); + calibration.accel_transform[0][1] = 1.0; + calibration.accel_transform[1][0] = -1.0; + calibration.accel_transform[2][2] = 2.0; + calibration.reference_temperature_c = 25.0; + calibration.temperature_compensation_enabled = true; + + fusion_imu_measurement_t measurement = { + .accel_mps2 = {3.0, 5.0, 7.0}, + .gyro_rps = {0.4, 0.5, 0.6}, + .temperature_c = 35.0 + }; + assert(imu_calibration_apply(&calibration, &measurement)); + assert(near(measurement.accel_mps2[0], 3.0)); + assert(near(measurement.accel_mps2[1], -1.0)); + assert(near(measurement.accel_mps2[2], 10.0)); + assert(near(measurement.gyro_rps[0], 0.2)); + assert(near(measurement.gyro_rps[1], 0.3)); + assert(near(measurement.gyro_rps[2], 0.4)); +} + +static void test_singular_transform_is_rejected(void) +{ + imu_calibration_t calibration; + imu_calibration_default(&calibration); + memset(calibration.gyro_transform, 0, sizeof(calibration.gyro_transform)); + assert(!imu_calibration_is_valid(&calibration)); +} + +int main(void) +{ + test_identity_is_noop(); + test_bias_transform_and_temperature(); + test_singular_transform_is_rejected(); + puts("imu_calibration tests passed"); + return 0; +} diff --git a/soc/software/pmod_irq_test/Makefile b/soc/software/pmod_irq_test/Makefile new file mode 100644 index 0000000..22e7a79 --- /dev/null +++ b/soc/software/pmod_irq_test/Makefile @@ -0,0 +1,14 @@ +TARGET := pmod_irq_test + +APP_SRCS := main.c +BSP_SRCS := init.c uart.c plic.c irq.c pmod_spi.c + +# Genesys2 Rocket fabric/UART and CLINT clocks. +BSP_UART_CLOCK_HZ ?= 50000000 +BSP_CLINT_TIMEBASE_HZ ?= 500000 + +include ../common/app.mk + +ifeq ($(PMOD_IRQ_TEST_QUIET),1) +CFLAGS += -DPMOD_IRQ_TEST_QUIET=1 +endif diff --git a/soc/software/pmod_irq_test/README.md b/soc/software/pmod_irq_test/README.md new file mode 100644 index 0000000..ca7f3d6 --- /dev/null +++ b/soc/software/pmod_irq_test/README.md @@ -0,0 +1,30 @@ +# Genesys2 JB1 direct PLIC test + +This standalone image tests only the asynchronous PMOD interrupt path: + +`JB1 (V29) -> wrapper 3FF synchronizer -> Rocket interrupt bus -> PLIC 11` + +The input is LVCMOS33 with a weak FPGA pull-down. The Rocket PLIC gateway is +level-sensitive, so the ISR masks source 11 on its first assertion before it +completes the claim. This makes a hand-driven HIGH test one-shot and prevents +an interrupt storm. + +Build: + +```sh +make -C soc/software/pmod_irq_test clean all +``` + +After loading `build/pmod_irq_test.elf`, wait for `PMOD_IRQ_READY`. Start with +JB1 disconnected, then connect JB1 to a Genesys2 PMOD 3.3 V pin once. A working +path prints `PMOD_IRQ_PASS`. Remove the 3.3 V connection before resetting or +reloading the image. + +Never apply 5 V to JB1 and never short a PMOD supply pin to ground. + +For a voltage-source-independent loopback test, connect the FPGA-driven JC4 +SPI clock output directly to the JB1 interrupt input. After +`LOOPBACK_READY`, send `t` over UART. The firmware emits 16 SPI transfers; +the first SCK HIGH must produce `PMOD_IRQ_PASS`. This loopback is confined to +the standalone diagnostic and does not add GPIO or alter the production PLIC +path. diff --git a/soc/software/pmod_irq_test/main.c b/soc/software/pmod_irq_test/main.c new file mode 100644 index 0000000..ffe6752 --- /dev/null +++ b/soc/software/pmod_irq_test/main.c @@ -0,0 +1,136 @@ +#include +#include + +#include "bsp/irq.h" +#include "bsp/mmio.h" +#include "bsp/plic.h" +#include "bsp/pmod_spi.h" +#include "bsp/soc.h" +#include "bsp/uart.h" + +#ifdef PMOD_IRQ_TEST_QUIET +#define TEST_PRINTF(...) ((void)0) +#define TEST_UART_FLUSH() ((void)0) +#else +#define TEST_PRINTF(...) printf(__VA_ARGS__) +#define TEST_UART_FLUSH() bsp_uart_wait_tx_idle() +#endif + +static volatile uint64_t irq_count; +static volatile uint64_t first_irq_timestamp; +static volatile uint64_t unexpected_mcause; +static volatile uint32_t unexpected_irq_id; + +void pmod_irq_handler(void) + __attribute__((interrupt("machine"), aligned(4))); + +void pmod_irq_handler(void) +{ + asm volatile( + ".option push\n" + ".option norelax\n" + "la gp, __global_pointer$\n" + ".option pop\n" + : + : + : "memory"); + + const uint64_t timestamp = bsp_read64(BSP_CLINT_MTIME); + const uint64_t mcause = bsp_read_mcause(); + if(((mcause >> 63) == 0u) || + ((mcause & 0xfffu) != BSP_MCAUSE_MACHINE_EXTERNAL)) + { + unexpected_mcause = mcause; + return; + } + + const uint32_t irq_id = bsp_plic_claim(); + if(irq_id == BSP_PMOD_IRQ) + { + /* PLIC gateway is level-sensitive. Mask on the first assertion so a + * hand-held HIGH wire cannot create an interrupt storm. */ + bsp_plic_disable(BSP_PMOD_IRQ); + if(irq_count == 0u) + { + first_irq_timestamp = timestamp; + } + irq_count++; + } + else if(irq_id != 0u) + { + unexpected_irq_id = irq_id; + } + + if(irq_id != 0u) + { + bsp_plic_complete(irq_id); + } +} + +int main(void) +{ + TEST_PRINTF("\nGenesys2 JB1 direct PLIC interrupt test\n"); + TEST_PRINTF("PATH,JB1/V29,Rocket_interrupt_bus,PLIC%lu\n", + (unsigned long)BSP_PMOD_IRQ); + TEST_PRINTF("EXPECTED_RTL,wrapper_3FF,internal_pulldown; verify after synthesis\n"); + TEST_PRINTF("INPUT,LVCMOS33,active_high,level_sensitive\n"); + + irq_count = 0u; + first_irq_timestamp = 0u; + unexpected_mcause = 0u; + unexpected_irq_id = 0u; + bsp_plic_disable_all(); + bsp_plic_set_threshold(0u); + bsp_plic_set_priority(BSP_PMOD_IRQ, 1u); + bsp_pmod_spi_init(); + bsp_pmod_spi_set_mode(BSP_PMOD_SPI_MODE_SINGLE); + bsp_pmod_spi_set_clock_mode(BSP_PMOD_SPI_CLOCK_MODE0); + bsp_set_mtvec(pmod_irq_handler); + bsp_plic_enable(BSP_PMOD_IRQ); + bsp_enable_machine_external_interrupts(); + + TEST_PRINTF("PMOD_IRQ_READY,disconnect_JB1_then_touch_JB1_to_3V3_once\n"); + TEST_PRINTF("LOOPBACK_READY,wire_JC4_SCK_to_JB1_then_send_t\n"); + TEST_UART_FLUSH(); + + while((irq_count == 0u) && (unexpected_mcause == 0u) && + (unexpected_irq_id == 0u)) + { + const int command = bsp_uart_getc_nonblocking(); + if((command == 't') || (command == 'T')) + { + TEST_PRINTF("LOOPBACK_TRIGGER,source=JC4,destination=JB1\n"); + TEST_UART_FLUSH(); + for(unsigned int transfer = 0u; transfer < 16u; transfer++) + { + (void)bsp_pmod_spi_xfer8((uint8_t)(0xa5u ^ transfer)); + } + if(irq_count == 0u) + { + TEST_PRINTF("LOOPBACK_NO_IRQ,check_JC4_to_JB1_jumper\n"); + TEST_UART_FLUSH(); + } + } + asm volatile("nop"); + } + + if(irq_count != 0u) + { + TEST_PRINTF("PMOD_IRQ_PASS,id=%lu,count=%lu,tick=%lu,tick_hz=%lu\n", + (unsigned long)BSP_PMOD_IRQ, + (unsigned long)irq_count, + (unsigned long)first_irq_timestamp, + (unsigned long)BSP_CLINT_TIMEBASE_HZ); + } + else + { + TEST_PRINTF("PMOD_IRQ_FAIL,mcause=%lu,unexpected_id=%lu\n", + (unsigned long)unexpected_mcause, + (unsigned long)unexpected_irq_id); + } + TEST_PRINTF("PMOD_IRQ_MASKED,remove_3V3_before_reset_or_reload\n"); + TEST_UART_FLUSH(); + /* The ASIC testbench observes this through mscratch. Returning success is + * therefore reserved for the expected PLIC device-11 ISR path. */ + return (irq_count != 0u) ? 0 : 1; +} diff --git a/soc/software/steval_mki217v1_test/Makefile b/soc/software/steval_mki217v1_test/Makefile index 43b059d..522c32e 100644 --- a/soc/software/steval_mki217v1_test/Makefile +++ b/soc/software/steval_mki217v1_test/Makefile @@ -4,7 +4,7 @@ APP_SRCS := \ main.c \ src/lsm6dsox.c -BSP_SRCS := init.c uart.c pmod_spi.c +BSP_SRCS := init.c uart.c plic.c irq.c pmod_spi.c # Genesys2 fabric/UART and CLINT defaults. Top-level board builds may override. BSP_UART_CLOCK_HZ ?= 50000000 diff --git a/soc/software/steval_mki217v1_test/README.md b/soc/software/steval_mki217v1_test/README.md index a76be5e..b53fb43 100644 --- a/soc/software/steval_mki217v1_test/README.md +++ b/soc/software/steval_mki217v1_test/README.md @@ -19,7 +19,7 @@ verified. | VDDIO | 3.3 V | digital I/O supply | | GND | GND | common ground | -`CS` is mandatory for four-wire SPI. Never feed 5 V to VDDIO. JC9 is the +`CS` is mandatory for four-wire SPI. Never feed 5 V to VDDIO. JB1 is the optional sensor interrupt pin but is deliberately unused here. ## Hardware bring-up results (2026-07-13) diff --git a/soc/software/steval_mki217v1_test/header/lsm6dsox.h b/soc/software/steval_mki217v1_test/header/lsm6dsox.h index cf5a372..08a69bc 100644 --- a/soc/software/steval_mki217v1_test/header/lsm6dsox.h +++ b/soc/software/steval_mki217v1_test/header/lsm6dsox.h @@ -30,6 +30,15 @@ uint8_t lsm6dsox_read_register(uint8_t reg); /* Return 1 for a new coherent gyro+accelerometer sample, 0 if not ready. */ int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample); +/* Route accel data-ready to sensor INT1 in latched, active-high mode. The ISR + * clears that source first, then waits briefly for the same-ODR gyro block. */ +int lsm6dsox_enable_data_ready_interrupt(void); +void lsm6dsox_disable_data_ready_interrupt(void); + +/* Read one coherent sample for a timestamp captured at ISR entry. */ +int lsm6dsox_read_interrupt_sample(lsm6dsox_sample_t *sample, + uint64_t interrupt_timestamp_ticks); + void lsm6dsox_power_down(void); const lsm6dsox_scale_t *lsm6dsox_scale(void); diff --git a/soc/software/steval_mki217v1_test/main.c b/soc/software/steval_mki217v1_test/main.c index b9da82c..26182ee 100644 --- a/soc/software/steval_mki217v1_test/main.c +++ b/soc/software/steval_mki217v1_test/main.c @@ -1,7 +1,9 @@ #include #include +#include "bsp/irq.h" #include "bsp/mmio.h" +#include "bsp/plic.h" #include "bsp/soc.h" #include "bsp/uart.h" #include "lsm6dsox.h" @@ -14,11 +16,106 @@ #define CAPTURE_DURATION_S 60u #endif +#ifndef LSM6DSOX_IRQ_QUEUE_CAPACITY +#define LSM6DSOX_IRQ_QUEUE_CAPACITY 256u +#endif + +#if (LSM6DSOX_IRQ_QUEUE_CAPACITY < 2u) +#error "LSM6DSOX_IRQ_QUEUE_CAPACITY must be at least 2" +#endif + +typedef struct { + lsm6dsox_sample_t sample; + uint32_t sequence; +} queued_sample_t; + +static queued_sample_t sample_queue[LSM6DSOX_IRQ_QUEUE_CAPACITY]; +static volatile unsigned sample_head; +static volatile unsigned sample_tail; +static volatile uint64_t interrupt_count; +static volatile uint64_t interrupt_sample_count; +static volatile uint64_t interrupt_read_fail_count; +static volatile uint64_t queue_overflow_count; +static volatile uint64_t unexpected_irq_count; +static volatile uint64_t unexpected_trap_cause; + +void sensor_irq_handler(void) + __attribute__((interrupt("machine"), aligned(4))); + static uint64_t time_now(void) { return bsp_read64(BSP_CLINT_MTIME); } +static unsigned next_queue_index(unsigned index) +{ + index++; + return (index == LSM6DSOX_IRQ_QUEUE_CAPACITY) ? 0u : index; +} + +static void queue_push_isr(const lsm6dsox_sample_t *sample, + uint32_t sequence) +{ + unsigned head = __atomic_load_n(&sample_head, __ATOMIC_RELAXED); + const unsigned next = next_queue_index(head); + if (next == __atomic_load_n(&sample_tail, __ATOMIC_ACQUIRE)) { + __atomic_fetch_add(&queue_overflow_count, 1u, __ATOMIC_RELAXED); + return; + } + sample_queue[head].sample = *sample; + sample_queue[head].sequence = sequence; + __atomic_store_n(&sample_head, next, __ATOMIC_RELEASE); +} + +static int queue_pop(queued_sample_t *queued) +{ + unsigned tail = __atomic_load_n(&sample_tail, __ATOMIC_RELAXED); + if (tail == __atomic_load_n(&sample_head, __ATOMIC_ACQUIRE)) { + return 0; + } + *queued = sample_queue[tail]; + __atomic_store_n(&sample_tail, next_queue_index(tail), __ATOMIC_RELEASE); + return 1; +} + +void sensor_irq_handler(void) +{ + asm volatile( + ".option push\n" + ".option norelax\n" + "la gp, __global_pointer$\n" + ".option pop\n" + : : : "memory"); + + const uint64_t interrupt_timestamp = time_now(); + const uint64_t mcause = bsp_read_mcause(); + if (((mcause >> 63) == 0u) || + ((mcause & 0xfffu) != BSP_MCAUSE_MACHINE_EXTERNAL)) { + unexpected_trap_cause = mcause; + return; + } + + const uint32_t irq_id = bsp_plic_claim(); + if (irq_id == 0u) { + __atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED); + return; + } + if (irq_id == BSP_PMOD_IRQ) { + __atomic_fetch_add(&interrupt_count, 1u, __ATOMIC_RELAXED); + lsm6dsox_sample_t sample; + if (lsm6dsox_read_interrupt_sample(&sample, interrupt_timestamp)) { + const uint64_t sample_number = + __atomic_fetch_add(&interrupt_sample_count, 1u, __ATOMIC_RELAXED); + queue_push_isr(&sample, (uint32_t)sample_number); + } else { + __atomic_fetch_add(&interrupt_read_fail_count, 1u, __ATOMIC_RELAXED); + } + } else { + __atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED); + } + bsp_plic_complete(irq_id); +} + static void print_identity_probe(void) { printf("WHO_AM_I probes:"); @@ -30,9 +127,10 @@ static void print_identity_probe(void) int main(void) { - printf("\nSTEVAL-MKI217V1 / LSM6DSOX SPI polling test\n"); + printf("\nSTEVAL-MKI217V1 / LSM6DSOX INT1 data-ready test\n"); printf("Genesys2 JC: CS=JC1 SDA/MOSI=JC2 SDO/MISO=JC3 SCL/SCK=JC4\n"); - printf("Power: VDD=3.3V VDDIO=3.3V common GND; INT is not used\n"); + printf("Power: VDD=3.3V VDDIO=3.3V common GND; sensor INT1=JB1/PLIC%lu\n", + (unsigned long)BSP_PMOD_IRQ); printf("SPI MMIO=0x%08lx CLINT=%lu Hz UART=%lu baud\n", (unsigned long)BSP_PMOD_SPI_BASE, (unsigned long)BSP_CLINT_TIMEBASE_HZ, @@ -53,6 +151,42 @@ int main(void) lsm6dsox_read_register(0x12u), lsm6dsox_read_register(0x1eu)); + sample_head = 0u; + sample_tail = 0u; + interrupt_count = 0u; + interrupt_sample_count = 0u; + interrupt_read_fail_count = 0u; + queue_overflow_count = 0u; + unexpected_irq_count = 0u; + unexpected_trap_cause = 0u; + bsp_plic_disable_all(); + bsp_plic_set_threshold(0u); + bsp_plic_set_priority(BSP_PMOD_IRQ, 1u); + /* Recover the PMOD LevelGateway if a previous GDB-loaded image stopped + * after claim but before complete. The sensor route is still disabled. */ + bsp_plic_complete(BSP_PMOD_IRQ); + bsp_set_mtvec(sensor_irq_handler); + bsp_plic_enable(BSP_PMOD_IRQ); + bsp_enable_machine_external_interrupts(); + if (!lsm6dsox_enable_data_ready_interrupt()) { + printf("INT INIT FAIL: INT1_CTRL=%02x INT2_CTRL=%02x TAP_CFG2=%02x " + "COUNTER_BDR_REG1=%02x\n", + lsm6dsox_read_register(0x0du), + lsm6dsox_read_register(0x0eu), + lsm6dsox_read_register(0x58u), + lsm6dsox_read_register(0x0bu)); + bsp_plic_disable(BSP_PMOD_IRQ); + lsm6dsox_power_down(); + bsp_uart_wait_tx_idle(); + return 2; + } + printf("INT_CONFIG,pin=INT1,INT1_CTRL=%02x,INT2_CTRL=%02x," + "TAP_CFG2=%02x,COUNTER_BDR_REG1=%02x,mode=latched\n", + lsm6dsox_read_register(0x0du), + lsm6dsox_read_register(0x0eu), + lsm6dsox_read_register(0x58u), + lsm6dsox_read_register(0x0bu)); + const lsm6dsox_scale_t *scale = lsm6dsox_scale(); printf("IMU_META,LSM6DSOX,tick_hz=%lu,odr_hz=%lu," "accel_ug_lsb=%lu,gyro_udps_lsb=%lu,temp_lsb_c=%lu," @@ -70,37 +204,57 @@ int main(void) const uint64_t duration_ticks = (uint64_t)BSP_CLINT_TIMEBASE_HZ * CAPTURE_DURATION_S; uint32_t samples = 0u; + uint32_t sequence_discontinuities = 0u; + uint32_t expected_sequence = 0u; uint64_t previous_tick = 0u; uint64_t maximum_gap = 0u; while ((time_now() - start) < duration_ticks) { - lsm6dsox_sample_t sample; - if (!lsm6dsox_poll_sample(&sample)) { - continue; - } - - if (previous_tick != 0u) { - const uint64_t gap = sample.timestamp_ticks - previous_tick; - if (gap > maximum_gap) { - maximum_gap = gap; + /* This diagnostic must still time out and report zero IRQs when INT1 is + * disconnected. The production application can sleep on its other IRQs. */ + asm volatile("nop"); + queued_sample_t queued; + while (queue_pop(&queued)) { + const lsm6dsox_sample_t *sample = &queued.sample; + if (queued.sequence != expected_sequence) { + sequence_discontinuities++; } - } - previous_tick = sample.timestamp_ticks; + expected_sequence = queued.sequence + 1u; - printf("IMU_RAW,LSM6DSOX,%lu,%lu,%d,%d,%d,%d,%d,%d,%d\n", - (unsigned long)samples, - (unsigned long)sample.timestamp_ticks, - (int)sample.accel[0], (int)sample.accel[1], - (int)sample.accel[2], (int)sample.gyro[0], - (int)sample.gyro[1], (int)sample.gyro[2], - (int)sample.temperature); - samples++; + if (previous_tick != 0u) { + const uint64_t gap = sample->timestamp_ticks - previous_tick; + if (gap > maximum_gap) { + maximum_gap = gap; + } + } + previous_tick = sample->timestamp_ticks; + + printf("IMU_RAW,LSM6DSOX,%lu,%lu,%d,%d,%d,%d,%d,%d,%d\n", + (unsigned long)queued.sequence, + (unsigned long)sample->timestamp_ticks, + (int)sample->accel[0], (int)sample->accel[1], + (int)sample->accel[2], (int)sample->gyro[0], + (int)sample->gyro[1], (int)sample->gyro[2], + (int)sample->temperature); + samples++; + } } + bsp_plic_disable(BSP_PMOD_IRQ); + lsm6dsox_disable_data_ready_interrupt(); lsm6dsox_power_down(); - printf("IMU_DONE,samples=%lu,duration_s=%u,max_gap_ticks=%lu\n", + printf("IMU_DONE,samples=%lu,duration_s=%u,max_gap_ticks=%lu," + "irq=%lu,irq_samples=%lu,read_fail=%lu,queue_overflow=%lu," + "seq_discontinuities=%lu,unexpected_irq=%lu,trap=%lu\n", (unsigned long)samples, (unsigned)CAPTURE_DURATION_S, - (unsigned long)maximum_gap); + (unsigned long)maximum_gap, + (unsigned long)interrupt_count, + (unsigned long)interrupt_sample_count, + (unsigned long)interrupt_read_fail_count, + (unsigned long)queue_overflow_count, + (unsigned long)sequence_discontinuities, + (unsigned long)unexpected_irq_count, + (unsigned long)unexpected_trap_cause); bsp_uart_wait_tx_idle(); return 0; } diff --git a/soc/software/steval_mki217v1_test/src/lsm6dsox.c b/soc/software/steval_mki217v1_test/src/lsm6dsox.c index 6fd6540..4052288 100644 --- a/soc/software/steval_mki217v1_test/src/lsm6dsox.c +++ b/soc/software/steval_mki217v1_test/src/lsm6dsox.c @@ -9,12 +9,17 @@ #define LSM6DSOX_SPI_READ 0x80u #define LSM6DSOX_SPI_DUMMY 0xffu +#define LSM6DSOX_COUNTER_BDR_REG1 0x0bu +#define LSM6DSOX_INT1_CTRL 0x0du +#define LSM6DSOX_INT2_CTRL 0x0eu #define LSM6DSOX_CTRL1_XL 0x10u #define LSM6DSOX_CTRL2_G 0x11u #define LSM6DSOX_CTRL3_C 0x12u #define LSM6DSOX_STATUS_REG 0x1eu +#define LSM6DSOX_TAP_CFG2 0x58u #define LSM6DSOX_OUT_TEMP_L 0x20u #define LSM6DSOX_OUTX_L_G 0x22u +#define LSM6DSOX_OUTX_L_A 0x28u #define LSM6DSOX_WHO_AM_I 0x0fu #define LSM6DSOX_CTRL3_SW_RESET 0x01u @@ -22,6 +27,9 @@ #define LSM6DSOX_CTRL3_BDU 0x40u #define LSM6DSOX_STATUS_XLDA 0x01u #define LSM6DSOX_STATUS_GDA 0x02u +#define LSM6DSOX_INT1_DRDY_XL 0x01u +#define LSM6DSOX_INTERRUPTS_ENABLE 0x80u +#define LSM6DSOX_GYRO_READY_TIMEOUT_US 100u /* CTRL1_XL/CTRL2_G ODR fields (bits 7:4). */ #define LSM6DSOX_ODR_52 0x30u @@ -147,21 +155,9 @@ int lsm6dsox_init(uint32_t odr_hz) return 1; } -int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample) +static void read_output_registers(lsm6dsox_sample_t *sample) { uint8_t raw[14]; - if (sample == NULL) { - return 0; - } - - const uint8_t status = read_reg(LSM6DSOX_STATUS_REG); - if ((status & (LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) != - (LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) { - return 0; - } - - /* Timestamp the measurement before the foreground SPI burst. */ - sample->timestamp_ticks = time_now(); bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ, LSM6DSOX_SPI_DUMMY, raw, @@ -180,11 +176,111 @@ int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample) sample->accel[0] = little_endian_i16(&raw[8]); sample->accel[1] = little_endian_i16(&raw[10]); sample->accel[2] = little_endian_i16(&raw[12]); +} + +static void read_accel_registers(lsm6dsox_sample_t *sample) +{ + uint8_t raw[6]; + bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_A | LSM6DSOX_SPI_READ, + LSM6DSOX_SPI_DUMMY, raw, sizeof(raw)); + sample->accel[0] = little_endian_i16(&raw[0]); + sample->accel[1] = little_endian_i16(&raw[2]); + sample->accel[2] = little_endian_i16(&raw[4]); +} + +static void read_gyro_and_temperature_registers(lsm6dsox_sample_t *sample) +{ + uint8_t raw[8]; + bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ, + LSM6DSOX_SPI_DUMMY, raw, 2u); + bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_G | LSM6DSOX_SPI_READ, + LSM6DSOX_SPI_DUMMY, &raw[2], 6u); + sample->temperature = little_endian_i16(&raw[0]); + sample->gyro[0] = little_endian_i16(&raw[2]); + sample->gyro[1] = little_endian_i16(&raw[4]); + sample->gyro[2] = little_endian_i16(&raw[6]); +} + +int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample) +{ + if (sample == NULL) { + return 0; + } + + const uint8_t status = read_reg(LSM6DSOX_STATUS_REG); + if ((status & (LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) != + (LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) { + return 0; + } + + /* Timestamp the measurement before the foreground SPI burst. */ + sample->timestamp_ticks = time_now(); + read_output_registers(sample); + return 1; +} + +int lsm6dsox_enable_data_ready_interrupt(void) +{ + /* Explicitly select latched data-ready. Reading the associated output + * register clears INT1; this matches the level-sensitive Rocket PLIC. */ + write_reg(LSM6DSOX_INT1_CTRL, 0u); + write_reg(LSM6DSOX_INT2_CTRL, 0u); + write_reg(LSM6DSOX_TAP_CFG2, + read_reg(LSM6DSOX_TAP_CFG2) & + (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE); + write_reg(LSM6DSOX_COUNTER_BDR_REG1, 0u); + if ((read_reg(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_XLDA) != 0u) { + lsm6dsox_sample_t stale; + read_output_registers(&stale); + } + write_reg(LSM6DSOX_INT1_CTRL, LSM6DSOX_INT1_DRDY_XL); + write_reg(LSM6DSOX_TAP_CFG2, + read_reg(LSM6DSOX_TAP_CFG2) | LSM6DSOX_INTERRUPTS_ENABLE); + return (read_reg(LSM6DSOX_INT1_CTRL) == LSM6DSOX_INT1_DRDY_XL) && + (read_reg(LSM6DSOX_INT2_CTRL) == 0u) && + ((read_reg(LSM6DSOX_TAP_CFG2) & LSM6DSOX_INTERRUPTS_ENABLE) != 0u) && + ((read_reg(LSM6DSOX_COUNTER_BDR_REG1) & 0x80u) == 0u); +} + +void lsm6dsox_disable_data_ready_interrupt(void) +{ + write_reg(LSM6DSOX_INT1_CTRL, 0u); + write_reg(LSM6DSOX_INT2_CTRL, 0u); + write_reg(LSM6DSOX_TAP_CFG2, + read_reg(LSM6DSOX_TAP_CFG2) & + (uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE); +} + +int lsm6dsox_read_interrupt_sample(lsm6dsox_sample_t *sample, + uint64_t interrupt_timestamp_ticks) +{ + if (sample == NULL) { + return 0; + } + const uint8_t status = read_reg(LSM6DSOX_STATUS_REG); + sample->timestamp_ticks = interrupt_timestamp_ticks; + /* Accel DRDY is the routed source, so clear its entire output block first. + * Then wait only long enough to form a coherent same-ODR gyro pair. */ + read_accel_registers(sample); + if ((status & LSM6DSOX_STATUS_XLDA) == 0u) { + return 0; + } + const uint64_t wait_start = time_now(); + const uint64_t timeout_ticks = + ((uint64_t)BSP_CLINT_TIMEBASE_HZ * LSM6DSOX_GYRO_READY_TIMEOUT_US + + 999999u) / 1000000u; + while ((read_reg(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_GDA) == 0u) { + if ((time_now() - wait_start) >= timeout_ticks) { + return 0; + } + } + read_gyro_and_temperature_registers(sample); return 1; } void lsm6dsox_power_down(void) { + lsm6dsox_disable_data_ready_interrupt(); write_reg(LSM6DSOX_CTRL1_XL, 0u); write_reg(LSM6DSOX_CTRL2_G, 0u); } diff --git a/targets/asic/rocket/Makefile b/targets/asic/rocket/Makefile index a62790b..d548199 100644 --- a/targets/asic/rocket/Makefile +++ b/targets/asic/rocket/Makefile @@ -47,6 +47,9 @@ APP_FLASH_HEX ?= $(FLASH_DIR)/$(SIM_APP).flash.hex BOOTLOADER_IMG ?= $(SOC_OUT_DIR)/software/bootloader/bootloader.img GPS_DEBUG ?= 1 GPS_DEBUG_LEVEL ?= 4 +BSP_UART_CLOCK_HZ ?= 100000000 +BSP_UART_BAUDRATE ?= 115200 +BSP_CLINT_TIMEBASE_HZ ?= 1000000 RTL_FILELIST := $(FILELIST_DIR)/asic_rtl.f TB_FILELIST := $(FILELIST_DIR)/asic_tb.f SIM_DPI := $(OUT_DIR)/dpi/libsimjtag.so @@ -58,6 +61,10 @@ ASIC_PLL_MODEL := $(ASIC_SIM_DIR)/tb/models/clock/asic_pll_model.sv CHISEL_SRCS := $(shell find $(SOC_SCALA_DIR) -type f -name '*.scala' 2>/dev/null) APP_SW_DIR := $(SOC_SW_DIR)/$(SIM_APP) APP_SW_SRCS := $(wildcard $(APP_SW_DIR)/*) +SIM_APP_MAKE_ARGS := +ifeq ($(SIM_APP),pmod_irq_test) +SIM_APP_MAKE_ARGS += PMOD_IRQ_TEST_QUIET=1 BSP_UART_CLOCK_HZ=$(BSP_UART_CLOCK_HZ) BSP_CLINT_TIMEBASE_HZ=$(BSP_CLINT_TIMEBASE_HZ) +endif BOOTLOADER_SW_SRCS := $(wildcard $(SOC_SW_DIR)/bootloader/*) MAKE_FLASH_IMAGE := $(SOC_SW_DIR)/tools/make_flash_image.py OBJCOPY := $(CROSS)objcopy @@ -66,8 +73,9 @@ READELF := $(CROSS)readelf VLOG_FLAGS ?= +define+PRINTF_COND=0 SIM_VLOG_FLAGS ?= +define+PRINTF_COND=0 +define+RANDOMIZE_REG_INIT +define+RANDOMIZE_MEM_INIT +define+RANDOM=0 +define+INIT_RANDOM_PROLOG_ GUI_VOPTARGS ?= +acc +SIM_PLUSARGS ?= -.PHONY: help all software chisel rtl sim-rtl filelist vlog-soc vlog-tb dpi sim sim-gui sanity clean +.PHONY: help all software chisel rtl sim-rtl filelist vlog-soc vlog-tb dpi sim sim-gui sim-pmod-irq sim-pmod-irq-rtl sanity clean FORCE help: @printf '%s\n' \ @@ -80,6 +88,8 @@ help: ' make vlog-soc Compile synthesis-style SoC RTL with Questa vlog' \ ' make vlog-tb Compile simulation RTL, models, SimJTAG, and TB' \ ' make sim Run UART/SPI flash sanity simulation' \ + ' make sim-pmod-irq Run PMOD IRQ square-wave/PLIC/ISR simulation' \ + ' make sim-pmod-irq-rtl Run fast PMOD IRQ square-wave/PLIC simulation' \ ' make sim GUI=1 Open Questa/ModelSim GUI, load wave.do, run 0 ns' \ ' make sim-gui Alias for make sim GUI=1' \ ' make clean Remove generated ASIC outputs' @@ -88,12 +98,16 @@ all: rtl vlog-tb software: $(BOOTLOADER_IMG) $(APP_FLASH_HEX) -$(BOOTLOADER_IMG): $(BOOTLOADER_SW_SRCS) - $(MAKE) -C "$(SOC_SW_DIR)" TOOLCHAIN="$(TOOLCHAIN)" APP="$(SIM_APP)" APP_FLASH_OFFSET="$(APP_FLASH_OFFSET)" bootloader +$(BOOTLOADER_IMG): FORCE $(BOOTLOADER_SW_SRCS) + $(MAKE) -C "$(SOC_SW_DIR)" TOOLCHAIN="$(TOOLCHAIN)" APP="$(SIM_APP)" \ + APP_FLASH_OFFSET="$(APP_FLASH_OFFSET)" \ + BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" \ + BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" \ + BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" bootloader $(APP_ELF): $(APP_SW_SRCS) test -d "$(APP_SW_DIR)" - $(MAKE) -C "$(APP_SW_DIR)" TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(APP_BUILD_DIR)" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" all + $(MAKE) -C "$(APP_SW_DIR)" TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(APP_BUILD_DIR)" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" $(SIM_APP_MAKE_ARGS) all $(APP_FLASH_BIN): $(APP_ELF) mkdir -p "$(FLASH_DIR)" @@ -122,7 +136,7 @@ $(FIRRTL_DIR)/VCU118RocketSystem.fir: $(BOOTLOADER_IMG) $(CHISEL_SRCS) rtl: $(ASIC_TOP) $(RTL_FILELIST) -$(ASIC_TOP): $(FIRRTL_DIR)/VCU118RocketSystem.fir +$(ASIC_TOP): $(FIRRTL_DIR)/VCU118RocketSystem.fir $(TOOLS_DIR)/gen_asic_wrapper.py cd "$(REPO_DIR)" rm -rf "$(VERILOG_DIR)" mkdir -p "$(VERILOG_DIR)" "$(OUT_DIR)/generated" @@ -141,7 +155,7 @@ $(ASIC_TOP): $(FIRRTL_DIR)/VCU118RocketSystem.fir sim-rtl: $(SIM_ASIC_TOP) $(TB_FILELIST) -$(SIM_ASIC_TOP): $(FIRRTL_DIR)/VCU118RocketSystem.fir +$(SIM_ASIC_TOP): $(FIRRTL_DIR)/VCU118RocketSystem.fir $(TOOLS_DIR)/gen_asic_wrapper.py cd "$(REPO_DIR)" rm -rf "$(SIM_VERILOG_DIR)" mkdir -p "$(SIM_VERILOG_DIR)" "$(OUT_DIR)/generated" @@ -223,6 +237,7 @@ sim sanity: vlog-tb $(SIM_DPI) else "$(VSIM)" -c -suppress 3009 -lib "$(ASIC_DIR)/work" -sv_lib "$(OUT_DIR)/dpi/libsimjtag" asic_rocket_tb \ +flash_hex="$(APP_FLASH_HEX)" \ + $(SIM_PLUSARGS) \ -do "run -all; quit -f" | tee "$(LOG_DIR)/vsim_sanity.log" grep -q "ASIC sanity PASS" "$(LOG_DIR)/vsim_sanity.log" fi @@ -230,6 +245,12 @@ sim sanity: vlog-tb $(SIM_DPI) sim-gui: $(MAKE) sim GUI=1 +sim-pmod-irq: + $(MAKE) sim SIM_APP=pmod_irq_test + +sim-pmod-irq-rtl: + $(MAKE) sim SIM_APP=pmod_irq_test SIM_PLUSARGS=+pmod_irq_rtl_only + clean: rm -rf "$(OUT_DIR)" "$(FIRRTL_DIR)" "$(VERILOG_DIR)" "$(SIM_VERILOG_DIR)" diff --git a/targets/asic/rocket/README.md b/targets/asic/rocket/README.md index 1b05eb3..329718b 100644 --- a/targets/asic/rocket/README.md +++ b/targets/asic/rocket/README.md @@ -15,11 +15,51 @@ make -C targets/asic/rocket sim-rtl make -C targets/asic/rocket vlog-soc make -C targets/asic/rocket vlog-tb make -C targets/asic/rocket sim +make -C targets/asic/rocket sim-pmod-irq +make -C targets/asic/rocket sim-pmod-irq-rtl make -C targets/asic/rocket sim GUI=1 ``` The default simulation app is `hello_uart`. +The ASIC Makefile always checks the bootloader build configuration before +hardware generation. This prevents a previously built Genesys2 image (50 MHz +UART and a different flash offset) from being silently embedded in the ASIC +BootROM; the ASIC defaults are 100 MHz, 115200 baud, and flash offset +`0x06000000`. + +`sim-pmod-irq` drives the ASIC top-level `pmod_irq` input with four +asynchronous square-wave periods, checks that TLPLIC source 10 becomes pending +(architectural device ID 11), and boots the standalone `pmod_irq_test` +firmware. PASS therefore requires both the RTL pending bit and a successful CPU +PLIC interrupt/ISR return; a pin toggle alone is not accepted. + +`sim-pmod-irq-rtl` applies the same four asynchronous periods but finishes as +soon as all four input rising edges and the PLIC device-11 pending state have +been observed. It skips the slow bootloader/SPI-flash/firmware phase and is the +fast regression for the ASIC input-to-PLIC hardware route. It does not claim to +test the C interrupt handler; use `sim-pmod-irq` for that end-to-end check. + +## PMOD IRQ ASIC simulation result + +Verified on 2026-07-14 with QuestaSim 2021.2_1: + +```text +ASIC PMOD IRQ: PLIC device 11 pending observed +UART: Flash JEDEC ID: 0x0020bb21 +UART: Load: flash 0x06000040 -> mem 0x80000000, size 0x00000908 +UART: CRC ok, jumping 0x80000000 +C main returned 0 +ASIC PMOD IRQ PASS: rises=4 pending_seen=1 +ASIC sanity PASS +Errors: 0 +``` + +The fast RTL-only regression also passed at simulation time 1455 ns with four +rising edges and PLIC device 11 pending. The full test completed at simulation +time 50,972,035 ns. In `pmod_irq_test`, return code zero is emitted only after +the expected device-11 ISR increments its interrupt counter. + `make -C targets/asic/rocket sim GUI=1` opens the Questa/ModelSim graphical simulator, loads `asic_rocket_tb` with `-voptargs="+acc"`, sources `hardware/sim/asic/rocket/wave.do`, sets `onfinish stop`, and runs only `0 ns` diff --git a/targets/asic/rocket/tools/gen_asic_wrapper.py b/targets/asic/rocket/tools/gen_asic_wrapper.py index 657fd14..2107fdc 100755 --- a/targets/asic/rocket/tools/gen_asic_wrapper.py +++ b/targets/asic/rocket/tools/gen_asic_wrapper.py @@ -83,6 +83,8 @@ def main(): value = "1'b0" elif name == "gps_sample_counter": value = "32'd0" + elif name == "pmod_irq": + value = "pmod_irq" elif name == "resetctrl_hartIsInReset_0": value = "core_reset" elif name == "debug_systemjtag_jtag_TCK": @@ -116,6 +118,7 @@ module asic_rocket_top ( input wire board_reset, input wire gps_rf_clk, input wire [3:0] gps_rf_data, + input wire pmod_irq, input wire uart_rxd, output wire uart_txd, output wire spi_sck, diff --git a/targets/fpga/genesys2/tools/gen_genesys2_wrapper.py b/targets/fpga/genesys2/tools/gen_genesys2_wrapper.py index 43aed83..c04430b 100644 --- a/targets/fpga/genesys2/tools/gen_genesys2_wrapper.py +++ b/targets/fpga/genesys2/tools/gen_genesys2_wrapper.py @@ -218,7 +218,10 @@ def main(): value = const_for(width) assigns.append(f" assign {name} = {value};") - port_map = ",\n".join(f" .{name}({name})" for _, _, name in ports) + port_map = ",\n".join( + f" .{name}({'pmod_irq_sync[2]' if name == 'pmod_irq' else name})" + for _, _, name in ports + ) led_debug = [] for candidate in ["debug_ndreset", "debug_dmactive", "debug_clockeddmi_dmi_req_ready"]: @@ -361,10 +364,25 @@ def main(): """ pmod_irq_top_ports = "" + pmod_irq_block = "" if "pmod_irq" in port_by_name: pmod_irq_top_ports = """\ input wire pmod_irq, """ + pmod_irq_block = """\ + // JB1 is asynchronous to the 50 MHz Rocket fabric. Keep this as three + // physical flip-flops: ASYNC_REG guides placement for metastability MTBF, + // and SHREG_EXTRACT prevents Vivado from collapsing stages into an SRL. + (* ASYNC_REG = "TRUE", SHREG_EXTRACT = "NO" *) + reg [2:0] pmod_irq_sync = 3'b000; + always @(posedge fabric_clk) begin + if (fabric_reset) begin + pmod_irq_sync <= 3'b000; + end else begin + pmod_irq_sync <= {pmod_irq_sync[1:0], pmod_irq}; + end + end +""" pps_top_ports = "" if port_by_name.get("gps_pps") == ("output", ""): @@ -788,6 +806,7 @@ module genesys2_rocket_top ( {bscan_block} {startup_block} {pmod_spi_block} +{pmod_irq_block} {chr(10).join(assigns)} assign led = {{