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

Add generic IMU fusion and PMOD IRQ verification

Browse Source
This commit is contained in:
onur
2026-07-14 11:03:13 +03:00
parent e77651dac3
commit dd6569c025
35 changed files with 2680 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
hardware/fpga/genesys2/constraints/genesys2_pmod_irq.xdc
+4 -3
View File
@@ -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. ## 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 IOSTANDARD LVCMOS33 [get_ports pmod_irq]
set_property PULLDOWN true [get_ports pmod_irq]
hardware/fpga/genesys2/constraints/genesys2_pmod_pps.xdc
+1 -1
View File
@@ -2,7 +2,7 @@
## ##
## Existing allocation: ## Existing allocation:
## JC1..JC4, JC7..JC8 user SPI ## JC1..JC4, JC7..JC8 user SPI
## JC9 sensor IRQ ## JB1 sensor IRQ
## JC10 GPS PPS (this file) ## JC10 GPS PPS (this file)
set_property PACKAGE_PIN AK28 [get_ports gps_pps] set_property PACKAGE_PIN AK28 [get_ports gps_pps]
hardware/sim/asic/rocket/tb/asic_rocket_tb.sv
+75 -2
View File
@@ -9,6 +9,7 @@ module asic_rocket_tb;
reg board_reset = 1'b1; reg board_reset = 1'b1;
reg gps_rf_clk = 1'b0; reg gps_rf_clk = 1'b0;
reg [3:0] gps_rf_data = 4'h0; reg [3:0] gps_rf_data = 4'h0;
reg pmod_irq = 1'b0;
wire core_clk = board_clock; wire core_clk = board_clock;
wire core_reset; wire core_reset;
@@ -34,11 +35,16 @@ module asic_rocket_tb;
wire uart_valid; wire uart_valid;
wire [7:0] uart_data; wire [7:0] uart_data;
reg simjtag_enable = 1'b0; reg simjtag_enable = 1'b0;
reg pmod_irq_rtl_only = 1'b0;
reg [7:0] last_uart_byte = 8'h00; reg [7:0] last_uart_byte = 8'h00;
integer app_match_count = 0; integer app_match_count = 0;
integer gps_match_count = 0; integer gps_match_count = 0;
integer uart_toggle_count = 0; integer uart_toggle_count = 0;
integer pmod_irq_rise_count = 0;
reg main_return_seen = 1'b0; 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; reg uart_txd_q = 1'b1;
wire core_wfi; wire core_wfi;
wire [63:0] main_return_status; wire [63:0] main_return_status;
@@ -46,6 +52,7 @@ module asic_rocket_tb;
initial begin initial begin
simjtag_enable = $test$plusargs("jtag_rbb_enable"); simjtag_enable = $test$plusargs("jtag_rbb_enable");
pmod_irq_rtl_only = $test$plusargs("pmod_irq_rtl_only");
end end
always #(CLOCK_HALF_PERIOD_NS) board_clock = ~board_clock; always #(CLOCK_HALF_PERIOD_NS) board_clock = ~board_clock;
@@ -69,6 +76,25 @@ module asic_rocket_tb;
board_reset = 1'b0; board_reset = 1'b0;
end 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 always @(posedge core_clk) begin
uart_txd_q <= uart_txd; uart_txd_q <= uart_txd;
if (!core_reset && uart_txd_q !== uart_txd) begin if (!core_reset && uart_txd_q !== uart_txd) begin
@@ -81,6 +107,7 @@ module asic_rocket_tb;
.board_reset(board_reset), .board_reset(board_reset),
.gps_rf_clk(gps_rf_clk), .gps_rf_clk(gps_rf_clk),
.gps_rf_data(gps_rf_data), .gps_rf_data(gps_rf_data),
.pmod_irq(pmod_irq),
.uart_rxd(1'b1), .uart_rxd(1'b1),
.uart_txd(uart_txd), .uart_txd(uart_txd),
.spi_sck(spi_sck), .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_status = dut.u_rocket.tile_prci_domain.element_reset_domain.rockettile.core.csr.reg_mscratch;
assign main_return_code = main_return_status[15:0]; 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 ( SimJTAG #(.TICK_DELAY(3)) sim_jtag (
.clock(core_clk), .clock(core_clk),
.reset(core_reset), .reset(core_reset),
@@ -179,21 +219,54 @@ module asic_rocket_tb;
end end
always @(posedge core_clk) begin 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"); $display("ASIC sanity PASS");
$finish; $finish;
end end
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 always @(posedge core_clk) begin
if (core_reset) begin if (core_reset) begin
main_return_seen <= 1'b0; 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 end else if (!main_return_seen && main_return_status[31:16] == 16'h5a5a) begin
main_return_seen <= 1'b1; main_return_seen <= 1'b1;
main_return_code_q <= main_return_code;
$display("C main returned %0d", 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"); $display("ASIC sanity PASS");
end else begin 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"); $display("ASIC sanity FAIL");
end end
$finish; $finish;
hardware/sim/asic/rocket/wave.do
+5
View File
@@ -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/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_ndreset
add wave -noupdate -expand -group Core /asic_rocket_tb/dut/debug_dmactive 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/sck
add wave -noupdate -expand -group Flash /asic_rocket_tb/u_flash/csn 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 add wave -noupdate -expand -group Flash /asic_rocket_tb/u_flash/dq_o
soc/software/fusion/tests/Makefile
+14 -3
View File
@@ -5,10 +5,11 @@ LDLIBS ?= -lm
GPS_TIME_SOURCE := ../../gps_system/src/gps_time_anchor.c 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 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 .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) standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE)
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) $(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) fusion_synthetic_test: fusion_synthetic_test.c $(ESKF_SOURCES)
$(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) $(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 ./standalone_gps_integrity_test
./eskf_motion_test ./eskf_motion_test
./fusion_synthetic_test ./fusion_synthetic_test
./imu_quality_synthetic_test
sanitize: sanitize:
$(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \ $(CC) -std=c11 -O1 -g -Wall -Wextra -Wpedantic -Werror \
@@ -37,6 +42,12 @@ sanitize:
-o fusion_synthetic_test_san $(LDLIBS) -o fusion_synthetic_test_san $(LDLIBS)
ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \ ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
UBSAN_OPTIONS=halt_on_error=1 ./fusion_synthetic_test_san 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: profile:
$(CC) -std=c11 -O2 -g -pg -Wall -Wextra -Wpedantic \ $(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 ./standalone_gps_integrity_test --bench-double 100000
clean: 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
soc/software/fusion/tests/imu_quality_synthetic_test.c
+418
View File
@@ -0,0 +1,418 @@
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#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;
}
soc/software/gps_system/Makefile
+37 -3
View File
@@ -5,9 +5,41 @@ TARGET := gps_system
# with the common 100 MHz fallback divider. # with the common 100 MHz fallback divider.
BSP_UART_CLOCK_HZ ?= 50000000 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 := \ APP_SRCS := \
gps_test.c \ 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/eskf.c \
src/fusion_motion.c \ src/fusion_motion.c \
src/fusion_engine.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 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 GPS_NAV_SOLVE_TIMER_TICKS ?= 50
include ../common/app.mk include ../common/app.mk
CFLAGS += -Iheader 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) -DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS)
LDLIBS += -lm LDLIBS += -lm
soc/software/gps_system/README.md
+118
View File
@@ -2,6 +2,124 @@
Bu notlar GPS bare-metal yaziliminin VCU118 Rocket SoC portu ve interrupt debug sureci icin yazildi. 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 ## Donanim Adresleri ve IRQ Map
Generated DTS/RTL'e gore GPS IP adresleri: Generated DTS/RTL'e gore GPS IP adresleri:
soc/software/gps_system/gps_test.c
+21 -8
View File
@@ -5,28 +5,41 @@
#include <float.h> #include <float.h>
#include "matrixLib.h" #include "matrixLib.h"
#include "gps_macros.h" #include "gps_macros.h"
#include "imu_sensor.h"
#include "navigation_fusion.h" #include "navigation_fusion.h"
#include "tdk_icm42688.h"
int main() int main()
{ {
tdk_icm42688_config_t imu_config; imu_sensor_config_t imu_config;
tdk_icm42688_default_config(&imu_config); imu_sensor_default_config(&imu_config);
bool imu_initialized = tdk_icm42688_init(&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); bool fusion_initialized = navigation_fusion_init(BSP_CLINT_TIMEBASE_HZ);
gps_startup_initialization(GPS_NAV_SOLVE_TIMER_TICKS); 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, GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
"init imu=%u running=%u odr_hz=%u timebase_hz=%lu\r\n", "init imu=%u sensor=%s who=0x%02x running=%u odr_hz=%lu "
imu_initialized ? 1u : 0u, imu_running ? 1u : 0u, "poll=%u timebase_hz=%lu\r\n",
imu_config.odr_hz, (unsigned long)BSP_CLINT_TIMEBASE_HZ); 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(); acq_start_loop();
uint64_t last_fusion_status_tick = 0u; uint64_t last_fusion_status_tick = 0u;
while(1) while(1)
{ {
/* GPS and either IMU backend wake the shared machine interrupt path. */
asm volatile("wfi"); asm volatile("wfi");
gps_background_task(); gps_background_task();
calculate_position(); calculate_position();
soc/software/gps_system/header/fusion_engine.h
+4
View File
@@ -5,6 +5,7 @@
#include <stdint.h> #include <stdint.h>
#include "eskf.h" #include "eskf.h"
#include "fusion_imu_quality.h"
#include "fusion_measurement.h" #include "fusion_measurement.h"
#include "fusion_motion.h" #include "fusion_motion.h"
@@ -104,6 +105,9 @@ typedef struct {
void fusion_engine_default_config(fusion_engine_config_t* config, void fusion_engine_default_config(fusion_engine_config_t* config,
uint64_t timebase_hz); 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, bool fusion_engine_init(fusion_engine_t* engine,
const fusion_engine_config_t* config); const fusion_engine_config_t* config);
soc/software/gps_system/header/fusion_imu_quality.h
+31
View File
@@ -0,0 +1,31 @@
#ifndef FUSION_IMU_QUALITY_H
#define FUSION_IMU_QUALITY_H
#include <stdbool.h>
#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
soc/software/gps_system/header/imu_calibration.h
+33
View File
@@ -0,0 +1,33 @@
#ifndef IMU_CALIBRATION_H
#define IMU_CALIBRATION_H
#include <stdbool.h>
#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
soc/software/gps_system/header/imu_sensor.h
+65
View File
@@ -0,0 +1,65 @@
#ifndef IMU_SENSOR_H
#define IMU_SENSOR_H
#include <stdbool.h>
#include <stdint.h>
#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
soc/software/gps_system/header/st_lsm6dsox.h
+53
View File
@@ -0,0 +1,53 @@
#ifndef ST_LSM6DSOX_H
#define ST_LSM6DSOX_H
#include <stdbool.h>
#include <stdint.h>
#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
soc/software/gps_system/src/fusion_engine.c
+86
View File
@@ -48,6 +48,64 @@ static uint64_t seconds_to_ticks(const fusion_engine_t* engine, double seconds)
return (uint64_t)(ticks + 0.5); 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, void fusion_engine_default_config(fusion_engine_config_t* config,
uint64_t timebase_hz) uint64_t timebase_hz)
{ {
@@ -77,6 +135,34 @@ void fusion_engine_default_config(fusion_engine_config_t* config,
config->gnss_still_confirmations = 2u; 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) static bool config_valid(const fusion_engine_config_t* config)
{ {
return (config != NULL) && (config->timebase_hz != 0u) && return (config != NULL) && (config->timebase_hz != 0u) &&
soc/software/gps_system/src/gps_interrupt_handlers.c
+2 -2
View File
@@ -3,7 +3,7 @@
#include "Navigation.h" #include "Navigation.h"
#include "gps_pps.h" #include "gps_pps.h"
#include "gps_time_anchor.h" #include "gps_time_anchor.h"
#include "tdk_icm42688.h" #include "imu_sensor.h"
#include <bsp/irq.h> #include <bsp/irq.h>
#include <bsp/mmio.h> #include <bsp/mmio.h>
#include <bsp/plic.h> #include <bsp/plic.h>
@@ -397,7 +397,7 @@ void gps_dispatch_irq(unsigned int irq_id)
{ {
if(irq_id == BSP_PMOD_IRQ) if(irq_id == BSP_PMOD_IRQ)
{ {
tdk_icm42688_irq_handler(); imu_sensor_irq_handler();
return; return;
} }
soc/software/gps_system/src/imu_calibration.c
+113
View File
@@ -0,0 +1,113 @@
#include "imu_calibration.h"
#include <math.h>
#include <stddef.h>
#include <string.h>
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;
}
soc/software/gps_system/src/imu_sensor.c
+350
View File
@@ -0,0 +1,350 @@
#include "imu_sensor.h"
#include <stddef.h>
#include <string.h>
#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";
}
}
soc/software/gps_system/src/imu_sensor_profile.c
+74
View File
@@ -0,0 +1,74 @@
#include "imu_sensor.h"
#include <stddef.h>
#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);
}
soc/software/gps_system/src/navigation_fusion.c
+9 -2
View File
@@ -2,7 +2,7 @@
#include "fusion_bus.h" #include "fusion_bus.h"
#include "fusion_gps_adapter.h" #include "fusion_gps_adapter.h"
#include "tdk_icm42688.h" #include "imu_sensor.h"
#include <math.h> #include <math.h>
#include <stddef.h> #include <stddef.h>
@@ -23,6 +23,12 @@ bool navigation_fusion_init(uint64_t local_timebase_hz)
} }
fusion_bus_init(&measurement_bus); fusion_bus_init(&measurement_bus);
fusion_engine_default_config(&engine_config, local_timebase_hz); 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); fusion_gps_adapter_default_config(&adapter_config);
if(!fusion_engine_init(&fusion_engine, &engine_config) || if(!fusion_engine_init(&fusion_engine, &engine_config) ||
!fusion_gps_adapter_init(&gps_adapter, &adapter_config)) !fusion_gps_adapter_init(&gps_adapter, &adapter_config))
@@ -46,8 +52,9 @@ unsigned int navigation_fusion_service(void)
return 0u; return 0u;
} }
unsigned int processed = 0u; unsigned int processed = 0u;
imu_sensor_service();
fusion_imu_measurement_t imu; 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); (void)fusion_engine_push_imu(&fusion_engine, &imu);
processed++; processed++;
soc/software/gps_system/src/st_lsm6dsox.c
+503
View File
@@ -0,0 +1,503 @@
#include "st_lsm6dsox.h"
#include <bsp/mmio.h>
#include <bsp/plic.h>
#include <bsp/pmod_spi.h>
#include <bsp/soc.h>
#include <limits.h>
#include <math.h>
#include <stddef.h>
#include <string.h>
#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;
}
}
soc/software/gps_system/tests/Makefile
+18
View File
@@ -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)
soc/software/gps_system/tests/test_imu_calibration.c
+78
View File
@@ -0,0 +1,78 @@
#include "imu_calibration.h"
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
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;
}
soc/software/pmod_irq_test/Makefile
+14
View File
@@ -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
soc/software/pmod_irq_test/README.md
+30
View File
@@ -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.
soc/software/pmod_irq_test/main.c
+136
View File
@@ -0,0 +1,136 @@
#include <stdint.h>
#include <stdio.h>
#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;
}
soc/software/steval_mki217v1_test/Makefile
+1 -1
View File
@@ -4,7 +4,7 @@ APP_SRCS := \
main.c \ main.c \
src/lsm6dsox.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. # Genesys2 fabric/UART and CLINT defaults. Top-level board builds may override.
BSP_UART_CLOCK_HZ ?= 50000000 BSP_UART_CLOCK_HZ ?= 50000000
soc/software/steval_mki217v1_test/README.md
+1 -1
View File
@@ -19,7 +19,7 @@ verified.
| VDDIO | 3.3 V | digital I/O supply | | VDDIO | 3.3 V | digital I/O supply |
| GND | GND | common ground | | 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. optional sensor interrupt pin but is deliberately unused here.
## Hardware bring-up results (2026-07-13) ## Hardware bring-up results (2026-07-13)
soc/software/steval_mki217v1_test/header/lsm6dsox.h
+9
View File
@@ -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. */ /* Return 1 for a new coherent gyro+accelerometer sample, 0 if not ready. */
int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample); 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); void lsm6dsox_power_down(void);
const lsm6dsox_scale_t *lsm6dsox_scale(void); const lsm6dsox_scale_t *lsm6dsox_scale(void);
soc/software/steval_mki217v1_test/main.c
+169 -15
View File
@@ -1,7 +1,9 @@
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include "bsp/irq.h"
#include "bsp/mmio.h" #include "bsp/mmio.h"
#include "bsp/plic.h"
#include "bsp/soc.h" #include "bsp/soc.h"
#include "bsp/uart.h" #include "bsp/uart.h"
#include "lsm6dsox.h" #include "lsm6dsox.h"
@@ -14,11 +16,106 @@
#define CAPTURE_DURATION_S 60u #define CAPTURE_DURATION_S 60u
#endif #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) static uint64_t time_now(void)
{ {
return bsp_read64(BSP_CLINT_MTIME); 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) static void print_identity_probe(void)
{ {
printf("WHO_AM_I probes:"); printf("WHO_AM_I probes:");
@@ -30,9 +127,10 @@ static void print_identity_probe(void)
int main(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("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", printf("SPI MMIO=0x%08lx CLINT=%lu Hz UART=%lu baud\n",
(unsigned long)BSP_PMOD_SPI_BASE, (unsigned long)BSP_PMOD_SPI_BASE,
(unsigned long)BSP_CLINT_TIMEBASE_HZ, (unsigned long)BSP_CLINT_TIMEBASE_HZ,
@@ -53,6 +151,42 @@ int main(void)
lsm6dsox_read_register(0x12u), lsm6dsox_read_register(0x12u),
lsm6dsox_read_register(0x1eu)); 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(); const lsm6dsox_scale_t *scale = lsm6dsox_scale();
printf("IMU_META,LSM6DSOX,tick_hz=%lu,odr_hz=%lu," printf("IMU_META,LSM6DSOX,tick_hz=%lu,odr_hz=%lu,"
"accel_ug_lsb=%lu,gyro_udps_lsb=%lu,temp_lsb_c=%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 = const uint64_t duration_ticks =
(uint64_t)BSP_CLINT_TIMEBASE_HZ * CAPTURE_DURATION_S; (uint64_t)BSP_CLINT_TIMEBASE_HZ * CAPTURE_DURATION_S;
uint32_t samples = 0u; uint32_t samples = 0u;
uint32_t sequence_discontinuities = 0u;
uint32_t expected_sequence = 0u;
uint64_t previous_tick = 0u; uint64_t previous_tick = 0u;
uint64_t maximum_gap = 0u; uint64_t maximum_gap = 0u;
while ((time_now() - start) < duration_ticks) { while ((time_now() - start) < duration_ticks) {
lsm6dsox_sample_t sample; /* This diagnostic must still time out and report zero IRQs when INT1 is
if (!lsm6dsox_poll_sample(&sample)) { * disconnected. The production application can sleep on its other IRQs. */
continue; 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++;
} }
expected_sequence = queued.sequence + 1u;
if (previous_tick != 0u) { if (previous_tick != 0u) {
const uint64_t gap = sample.timestamp_ticks - previous_tick; const uint64_t gap = sample->timestamp_ticks - previous_tick;
if (gap > maximum_gap) { if (gap > maximum_gap) {
maximum_gap = gap; maximum_gap = gap;
} }
} }
previous_tick = sample.timestamp_ticks; previous_tick = sample->timestamp_ticks;
printf("IMU_RAW,LSM6DSOX,%lu,%lu,%d,%d,%d,%d,%d,%d,%d\n", printf("IMU_RAW,LSM6DSOX,%lu,%lu,%d,%d,%d,%d,%d,%d,%d\n",
(unsigned long)samples, (unsigned long)queued.sequence,
(unsigned long)sample.timestamp_ticks, (unsigned long)sample->timestamp_ticks,
(int)sample.accel[0], (int)sample.accel[1], (int)sample->accel[0], (int)sample->accel[1],
(int)sample.accel[2], (int)sample.gyro[0], (int)sample->accel[2], (int)sample->gyro[0],
(int)sample.gyro[1], (int)sample.gyro[2], (int)sample->gyro[1], (int)sample->gyro[2],
(int)sample.temperature); (int)sample->temperature);
samples++; samples++;
} }
}
bsp_plic_disable(BSP_PMOD_IRQ);
lsm6dsox_disable_data_ready_interrupt();
lsm6dsox_power_down(); 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)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(); bsp_uart_wait_tx_idle();
return 0; return 0;
} }
soc/software/steval_mki217v1_test/src/lsm6dsox.c
+109 -13
View File
@@ -9,12 +9,17 @@
#define LSM6DSOX_SPI_READ 0x80u #define LSM6DSOX_SPI_READ 0x80u
#define LSM6DSOX_SPI_DUMMY 0xffu #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_CTRL1_XL 0x10u
#define LSM6DSOX_CTRL2_G 0x11u #define LSM6DSOX_CTRL2_G 0x11u
#define LSM6DSOX_CTRL3_C 0x12u #define LSM6DSOX_CTRL3_C 0x12u
#define LSM6DSOX_STATUS_REG 0x1eu #define LSM6DSOX_STATUS_REG 0x1eu
#define LSM6DSOX_TAP_CFG2 0x58u
#define LSM6DSOX_OUT_TEMP_L 0x20u #define LSM6DSOX_OUT_TEMP_L 0x20u
#define LSM6DSOX_OUTX_L_G 0x22u #define LSM6DSOX_OUTX_L_G 0x22u
#define LSM6DSOX_OUTX_L_A 0x28u
#define LSM6DSOX_WHO_AM_I 0x0fu #define LSM6DSOX_WHO_AM_I 0x0fu
#define LSM6DSOX_CTRL3_SW_RESET 0x01u #define LSM6DSOX_CTRL3_SW_RESET 0x01u
@@ -22,6 +27,9 @@
#define LSM6DSOX_CTRL3_BDU 0x40u #define LSM6DSOX_CTRL3_BDU 0x40u
#define LSM6DSOX_STATUS_XLDA 0x01u #define LSM6DSOX_STATUS_XLDA 0x01u
#define LSM6DSOX_STATUS_GDA 0x02u #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). */ /* CTRL1_XL/CTRL2_G ODR fields (bits 7:4). */
#define LSM6DSOX_ODR_52 0x30u #define LSM6DSOX_ODR_52 0x30u
@@ -147,21 +155,9 @@ int lsm6dsox_init(uint32_t odr_hz)
return 1; return 1;
} }
int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample) static void read_output_registers(lsm6dsox_sample_t *sample)
{ {
uint8_t raw[14]; 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, bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ,
LSM6DSOX_SPI_DUMMY, LSM6DSOX_SPI_DUMMY,
raw, raw,
@@ -180,11 +176,111 @@ int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample)
sample->accel[0] = little_endian_i16(&raw[8]); sample->accel[0] = little_endian_i16(&raw[8]);
sample->accel[1] = little_endian_i16(&raw[10]); sample->accel[1] = little_endian_i16(&raw[10]);
sample->accel[2] = little_endian_i16(&raw[12]); 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; return 1;
} }
void lsm6dsox_power_down(void) void lsm6dsox_power_down(void)
{ {
lsm6dsox_disable_data_ready_interrupt();
write_reg(LSM6DSOX_CTRL1_XL, 0u); write_reg(LSM6DSOX_CTRL1_XL, 0u);
write_reg(LSM6DSOX_CTRL2_G, 0u); write_reg(LSM6DSOX_CTRL2_G, 0u);
} }
targets/asic/rocket/Makefile
+27 -6
View File
@@ -47,6 +47,9 @@ APP_FLASH_HEX ?= $(FLASH_DIR)/$(SIM_APP).flash.hex
BOOTLOADER_IMG ?= $(SOC_OUT_DIR)/software/bootloader/bootloader.img BOOTLOADER_IMG ?= $(SOC_OUT_DIR)/software/bootloader/bootloader.img
GPS_DEBUG ?= 1 GPS_DEBUG ?= 1
GPS_DEBUG_LEVEL ?= 4 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 RTL_FILELIST := $(FILELIST_DIR)/asic_rtl.f
TB_FILELIST := $(FILELIST_DIR)/asic_tb.f TB_FILELIST := $(FILELIST_DIR)/asic_tb.f
SIM_DPI := $(OUT_DIR)/dpi/libsimjtag.so 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) CHISEL_SRCS := $(shell find $(SOC_SCALA_DIR) -type f -name '*.scala' 2>/dev/null)
APP_SW_DIR := $(SOC_SW_DIR)/$(SIM_APP) APP_SW_DIR := $(SOC_SW_DIR)/$(SIM_APP)
APP_SW_SRCS := $(wildcard $(APP_SW_DIR)/*) 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/*) BOOTLOADER_SW_SRCS := $(wildcard $(SOC_SW_DIR)/bootloader/*)
MAKE_FLASH_IMAGE := $(SOC_SW_DIR)/tools/make_flash_image.py MAKE_FLASH_IMAGE := $(SOC_SW_DIR)/tools/make_flash_image.py
OBJCOPY := $(CROSS)objcopy OBJCOPY := $(CROSS)objcopy
@@ -66,8 +73,9 @@ READELF := $(CROSS)readelf
VLOG_FLAGS ?= +define+PRINTF_COND=0 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_ 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 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: help:
@printf '%s\n' \ @printf '%s\n' \
@@ -80,6 +88,8 @@ help:
' make vlog-soc Compile synthesis-style SoC RTL with Questa vlog' \ ' make vlog-soc Compile synthesis-style SoC RTL with Questa vlog' \
' make vlog-tb Compile simulation RTL, models, SimJTAG, and TB' \ ' make vlog-tb Compile simulation RTL, models, SimJTAG, and TB' \
' make sim Run UART/SPI flash sanity simulation' \ ' 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=1 Open Questa/ModelSim GUI, load wave.do, run 0 ns' \
' make sim-gui Alias for make sim GUI=1' \ ' make sim-gui Alias for make sim GUI=1' \
' make clean Remove generated ASIC outputs' ' make clean Remove generated ASIC outputs'
@@ -88,12 +98,16 @@ all: rtl vlog-tb
software: $(BOOTLOADER_IMG) $(APP_FLASH_HEX) software: $(BOOTLOADER_IMG) $(APP_FLASH_HEX)
$(BOOTLOADER_IMG): $(BOOTLOADER_SW_SRCS) $(BOOTLOADER_IMG): FORCE $(BOOTLOADER_SW_SRCS)
$(MAKE) -C "$(SOC_SW_DIR)" TOOLCHAIN="$(TOOLCHAIN)" APP="$(SIM_APP)" APP_FLASH_OFFSET="$(APP_FLASH_OFFSET)" bootloader $(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) $(APP_ELF): $(APP_SW_SRCS)
test -d "$(APP_SW_DIR)" 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) $(APP_FLASH_BIN): $(APP_ELF)
mkdir -p "$(FLASH_DIR)" mkdir -p "$(FLASH_DIR)"
@@ -122,7 +136,7 @@ $(FIRRTL_DIR)/VCU118RocketSystem.fir: $(BOOTLOADER_IMG) $(CHISEL_SRCS)
rtl: $(ASIC_TOP) $(RTL_FILELIST) 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)" cd "$(REPO_DIR)"
rm -rf "$(VERILOG_DIR)" rm -rf "$(VERILOG_DIR)"
mkdir -p "$(VERILOG_DIR)" "$(OUT_DIR)/generated" 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-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)" cd "$(REPO_DIR)"
rm -rf "$(SIM_VERILOG_DIR)" rm -rf "$(SIM_VERILOG_DIR)"
mkdir -p "$(SIM_VERILOG_DIR)" "$(OUT_DIR)/generated" mkdir -p "$(SIM_VERILOG_DIR)" "$(OUT_DIR)/generated"
@@ -223,6 +237,7 @@ sim sanity: vlog-tb $(SIM_DPI)
else else
"$(VSIM)" -c -suppress 3009 -lib "$(ASIC_DIR)/work" -sv_lib "$(OUT_DIR)/dpi/libsimjtag" asic_rocket_tb \ "$(VSIM)" -c -suppress 3009 -lib "$(ASIC_DIR)/work" -sv_lib "$(OUT_DIR)/dpi/libsimjtag" asic_rocket_tb \
+flash_hex="$(APP_FLASH_HEX)" \ +flash_hex="$(APP_FLASH_HEX)" \
$(SIM_PLUSARGS) \
-do "run -all; quit -f" | tee "$(LOG_DIR)/vsim_sanity.log" -do "run -all; quit -f" | tee "$(LOG_DIR)/vsim_sanity.log"
grep -q "ASIC sanity PASS" "$(LOG_DIR)/vsim_sanity.log" grep -q "ASIC sanity PASS" "$(LOG_DIR)/vsim_sanity.log"
fi fi
@@ -230,6 +245,12 @@ sim sanity: vlog-tb $(SIM_DPI)
sim-gui: sim-gui:
$(MAKE) sim GUI=1 $(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: clean:
rm -rf "$(OUT_DIR)" "$(FIRRTL_DIR)" "$(VERILOG_DIR)" "$(SIM_VERILOG_DIR)" rm -rf "$(OUT_DIR)" "$(FIRRTL_DIR)" "$(VERILOG_DIR)" "$(SIM_VERILOG_DIR)"
targets/asic/rocket/README.md
+40
View File
@@ -15,11 +15,51 @@ make -C targets/asic/rocket sim-rtl
make -C targets/asic/rocket vlog-soc make -C targets/asic/rocket vlog-soc
make -C targets/asic/rocket vlog-tb make -C targets/asic/rocket vlog-tb
make -C targets/asic/rocket sim 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 make -C targets/asic/rocket sim GUI=1
``` ```
The default simulation app is `hello_uart`. 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, `make -C targets/asic/rocket sim GUI=1` opens the Questa/ModelSim graphical simulator,
loads `asic_rocket_tb` with `-voptargs="+acc"`, sources loads `asic_rocket_tb` with `-voptargs="+acc"`, sources
`hardware/sim/asic/rocket/wave.do`, sets `onfinish stop`, and runs only `0 ns` `hardware/sim/asic/rocket/wave.do`, sets `onfinish stop`, and runs only `0 ns`
targets/asic/rocket/tools/gen_asic_wrapper.py
+3
View File
@@ -83,6 +83,8 @@ def main():
value = "1'b0" value = "1'b0"
elif name == "gps_sample_counter": elif name == "gps_sample_counter":
value = "32'd0" value = "32'd0"
elif name == "pmod_irq":
value = "pmod_irq"
elif name == "resetctrl_hartIsInReset_0": elif name == "resetctrl_hartIsInReset_0":
value = "core_reset" value = "core_reset"
elif name == "debug_systemjtag_jtag_TCK": elif name == "debug_systemjtag_jtag_TCK":
@@ -116,6 +118,7 @@ module asic_rocket_top (
input wire board_reset, input wire board_reset,
input wire gps_rf_clk, input wire gps_rf_clk,
input wire [3:0] gps_rf_data, input wire [3:0] gps_rf_data,
input wire pmod_irq,
input wire uart_rxd, input wire uart_rxd,
output wire uart_txd, output wire uart_txd,
output wire spi_sck, output wire spi_sck,
targets/fpga/genesys2/tools/gen_genesys2_wrapper.py
+20 -1
View File
@@ -218,7 +218,10 @@ def main():
value = const_for(width) value = const_for(width)
assigns.append(f" assign {name} = {value};") 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 = [] led_debug = []
for candidate in ["debug_ndreset", "debug_dmactive", "debug_clockeddmi_dmi_req_ready"]: for candidate in ["debug_ndreset", "debug_dmactive", "debug_clockeddmi_dmi_req_ready"]:
@@ -361,10 +364,25 @@ def main():
""" """
pmod_irq_top_ports = "" pmod_irq_top_ports = ""
pmod_irq_block = ""
if "pmod_irq" in port_by_name: if "pmod_irq" in port_by_name:
pmod_irq_top_ports = """\ pmod_irq_top_ports = """\
input wire pmod_irq, 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 = "" pps_top_ports = ""
if port_by_name.get("gps_pps") == ("output", ""): if port_by_name.get("gps_pps") == ("output", ""):
@@ -788,6 +806,7 @@ module genesys2_rocket_top (
{bscan_block} {bscan_block}
{startup_block} {startup_block}
{pmod_spi_block} {pmod_spi_block}
{pmod_irq_block}
{chr(10).join(assigns)} {chr(10).join(assigns)}
assign led = {{ assign led = {{