gps/soc/software/tdk_noise_test/main.c

/*
 * Stationary noise characterization for the TDK ICM-42688-P.
 *
 * The board must stay still for the whole run.
 *
 * This app drives the production tdk_icm42688 driver, so ODR, full-scale
 * ranges, UI filter configuration and LSB scaling are identical to the flight
 * build.  Measuring through a hand-rolled register setup would characterize a
 * different bandwidth and produce thresholds that do not transfer.
 *
 * It reports exactly the three quantities fusion_motion_update() compares
 * against its thresholds:
 *
 *   accel_norm = || accel - mean(accel) ||   linear acceleration at rest, i.e.
 *                                            what remains once the ESKF has
 *                                            absorbed gravity and accel bias
 *   gyro_norm  = || gyro  - mean(gyro)  ||   bias-removed angular rate
 *   jerk_norm  = || d/dt IIR(accel_res) ||   same 0.10 s time constant the
 *                                            detector uses
 *
 * A stationary limit must sit above the observed quiescent distribution or the
 * platform is never recognised as still; a motion threshold must sit above it
 * by enough margin that noise alone cannot trip it.  Both therefore follow
 * from the numbers printed here rather than from datasheet noise densities,
 * which describe the bare part and not this board, mounting or bandwidth.
 *
 * All values print as integers in micro-units to keep the report independent
 * of newlib float formatting: um/s^2, urad/s, um/s^3.
 */

#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <string.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"

#include "drivers/imu/tdk_icm42688.h"

/* Mirrors soc/software/gps_system/Makefile IMU_ODR_HZ. */
#ifndef NOISE_ODR_HZ
#define NOISE_ODR_HZ 50u
#endif
/* Discards sensor and UI-filter settling after start. */
#ifndef NOISE_DISCARD_SAMPLES
#define NOISE_DISCARD_SAMPLES 100u
#endif
#ifndef NOISE_BIAS_SAMPLES
#define NOISE_BIAS_SAMPLES 1000u
#endif
#ifndef NOISE_MEASURE_SAMPLES
#define NOISE_MEASURE_SAMPLES 3000u
#endif

/* Matches fusion_motion_default_config()->jerk_filter_time_constant_s. */
#define JERK_TIME_CONSTANT_S 0.10

#define HISTOGRAM_BINS 400u
/* Full-scale of each histogram; samples above it still count in the last bin
 * and the exact maximum is tracked separately, so an overflow is visible. */
#define ACCEL_HISTOGRAM_FULL_SCALE 0.40   /* m/s^2  -> 1.0 mm/s^2 per bin */
#define GYRO_HISTOGRAM_FULL_SCALE 0.040   /* rad/s  -> 100 urad/s per bin */
#define JERK_HISTOGRAM_FULL_SCALE 8.0     /* m/s^3  -> 20 mm/s^3 per bin */

typedef struct {
	const char* name;
	const char* unit;
	double full_scale;
	double sum;
	double sum_squares;
	double maximum;
	uint32_t count;
	uint32_t overflow;
	uint32_t bins[HISTOGRAM_BINS];
} statistic_t;

static volatile uint64_t unexpected_irq_count;
static volatile uint64_t unexpected_trap_cause;

/*
 * Must carry interrupt("machine"): without it GCC ends the function with ret
 * instead of mret, so mstatus.MIE -- cleared by hardware on trap entry -- is
 * never restored and exactly one interrupt is ever taken.  aligned(4) is
 * required because mtvec holds the address.
 */
void tdk_noise_trap_handler(void)
	__attribute__((interrupt("machine"), aligned(4)));

void tdk_noise_trap_handler(void)
{
	asm volatile(
		".option push\n"
		".option norelax\n"
		"la gp, __global_pointer$\n"
		".option pop\n"
		: : : "memory");

	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)
	{
		tdk_icm42688_irq_handler();
	}
	else
	{
		__atomic_fetch_add(&unexpected_irq_count, 1u, __ATOMIC_RELAXED);
	}
	bsp_plic_complete(irq_id);
}

static void statistic_init(statistic_t* statistic, const char* name,
	                       const char* unit, double full_scale)
{
	memset(statistic, 0, sizeof(*statistic));
	statistic->name = name;
	statistic->unit = unit;
	statistic->full_scale = full_scale;
}

static void statistic_add(statistic_t* statistic, double value)
{
	statistic->sum += value;
	statistic->sum_squares += value * value;
	if(value > statistic->maximum)
	{
		statistic->maximum = value;
	}
	statistic->count++;
	double scaled = value / statistic->full_scale * (double)HISTOGRAM_BINS;
	if(!(scaled >= 0.0))
	{
		scaled = 0.0;
	}
	unsigned int bin = (unsigned int)scaled;
	if(bin >= HISTOGRAM_BINS)
	{
		bin = HISTOGRAM_BINS - 1u;
		statistic->overflow++;
	}
	statistic->bins[bin]++;
}

/* Upper edge of the bin holding the requested quantile. */
static double statistic_percentile(const statistic_t* statistic,
	                               double fraction)
{
	if(statistic->count == 0u)
	{
		return 0.0;
	}
	uint32_t target = (uint32_t)(fraction * (double)statistic->count);
	uint32_t cumulative = 0u;
	for(unsigned int bin = 0u ; bin < HISTOGRAM_BINS ; bin++)
	{
		cumulative += statistic->bins[bin];
		if(cumulative >= target)
		{
			return (double)(bin + 1u) * statistic->full_scale /
			       (double)HISTOGRAM_BINS;
		}
	}
	return statistic->full_scale;
}

static long micro(double value)
{
	double scaled = value * 1.0e6;
	if(scaled > 2.0e9)
	{
		scaled = 2.0e9;
	}
	if(scaled < -2.0e9)
	{
		scaled = -2.0e9;
	}
	return (long)scaled;
}

static void statistic_report(const statistic_t* statistic)
{
	double mean = (statistic->count != 0u)
	            ? statistic->sum / (double)statistic->count : 0.0;
	double variance = (statistic->count > 1u)
	                ? (statistic->sum_squares / (double)statistic->count) -
	                  (mean * mean)
	                : 0.0;
	if(variance < 0.0)
	{
		variance = 0.0;
	}
	printf("STAT,%s,unit=%s,n=%lu,mean=%ld,std=%ld,"
	       "p50=%ld,p95=%ld,p99=%ld,p999=%ld,max=%ld,overflow=%lu\n",
	       statistic->name, statistic->unit,
	       (unsigned long)statistic->count,
	       micro(mean), micro(sqrt(variance)),
	       micro(statistic_percentile(statistic, 0.50)),
	       micro(statistic_percentile(statistic, 0.95)),
	       micro(statistic_percentile(statistic, 0.99)),
	       micro(statistic_percentile(statistic, 0.999)),
	       micro(statistic->maximum),
	       (unsigned long)statistic->overflow);
}

/*
 * Polled fallback.
 *
 * The data-ready INT1 route is not delivering on this bitstream (the driver
 * reports a single spurious IRQ and then silence), so the interrupt path
 * cannot feed the characterization.  Polling INT_STATUS reads the identical
 * registers the ISR would read, at the identical ODR, full scale and UI
 * filter, so the noise statistics are unaffected; only the delivery mechanism
 * differs.  Sample timestamps still come from CLINT.
 */
#define ICM_INT_STATUS_REG 0x2du
#define ICM_TEMP_DATA1_REG 0x1du
#define ICM_SPI_READ_BIT 0x80u
#define ICM_SPI_DUMMY_BYTE 0xffu
#define ICM_DATA_READY_BIT 0x08u

static bool polled_mode;

static int16_t big_endian_i16(const uint8_t* bytes)
{
	return (int16_t)(((uint16_t)bytes[0] << 8) | bytes[1]);
}

static bool poll_for_sample(imu_sample_t* sample, uint64_t deadline)
{
	while(bsp_read64(BSP_CLINT_MTIME) < deadline)
	{
		uint8_t status = bsp_pmod_spi_read_reg8(
			ICM_INT_STATUS_REG | ICM_SPI_READ_BIT, ICM_SPI_DUMMY_BYTE);
		if((status & ICM_DATA_READY_BIT) == 0u)
		{
			continue;
		}
		uint64_t timestamp = bsp_read64(BSP_CLINT_MTIME);
		uint8_t bytes[14];
		bsp_pmod_spi_read_regs(ICM_TEMP_DATA1_REG | ICM_SPI_READ_BIT,
		                       ICM_SPI_DUMMY_BYTE, bytes, sizeof(bytes));
		/* Same scaling the driver applies: full scale over a 15-bit half range. */
		double accel_scale = 4.0 * 9.80665 / 32768.0;
		double gyro_scale = 250.0 * 0.017453292519943295769236907684886 / 32768.0;
		sample->timestamp_ticks = timestamp;
		sample->temperature_c = (double)big_endian_i16(&bytes[0]) / 132.48 + 25.0;
		for(unsigned int axis = 0u ; axis < 3u ; axis++)
		{
			sample->accel_mps2[axis] =
				(double)big_endian_i16(&bytes[2u + 2u * axis]) * accel_scale;
			sample->gyro_rps[axis] =
				(double)big_endian_i16(&bytes[8u + 2u * axis]) * gyro_scale;
		}
		return true;
	}
	return false;
}

/*
 * Why the data-ready interrupt stopped after the first one.
 *
 * Three causes produce the same silence and are told apart by whether the
 * sensor is still generating a data-ready condition at all:
 *
 *   sensor stopped   PWR_MGMT0 or the ODR fields no longer hold their
 *                    configured values, and DRDY never sets
 *   routing lost     DRDY sets in INT_STATUS but INT_SOURCE0 no longer routes
 *                    it to the pin
 *   latch stuck      DRDY sets and routing is intact, so the pin is asserted
 *                    but the PLIC is not seeing a fresh edge/level
 */
/*
 * Continuity test for the sensor INT1 -> FPGA pmod_irq -> PLIC path.
 *
 * Everything measured so far depends on the sensor raising a data-ready edge,
 * which conflates "the line is broken" with "the source never fired".  This
 * separates them by driving INT1 to a static level the sensor holds
 * regardless of data-ready, and reading the PLIC pending bit directly.
 *
 * INT_CONFIG bit 0 selects INT1 polarity.  Clearing it makes the pin active
 * low, so its idle state becomes high and it stays there.  If pmod_irq is
 * really wired to this pin, the PLIC gateway then reports pending forever; if
 * the input is floating, its pulldown keeps it clear.  The core's external
 * interrupts stay masked throughout, so a permanently asserted line cannot
 * storm the ISR -- the pending bit is polled, never taken.
 */
#define PLIC_PENDING_WORD0 (BSP_PLIC_BASE + 0x1000u)
#define ICM_INT_CONFIG_REG 0x14u
#define ICM_INT_SOURCE0_REG 0x65u
#define ICM_INT1_ACTIVE_HIGH_PP_LATCHED 0x07u
#define ICM_INT1_ACTIVE_LOW_PP_LATCHED 0x06u
#define ICM_UI_DRDY_INT1 0x08u

static uint32_t sample_pending_percent(uint64_t window_ticks)
{
	/* The gateway latches; claiming clears it so what follows reflects the
	 * line's present level rather than a stale assertion. */
	uint32_t claimed = bsp_plic_claim();
	if(claimed != 0u)
	{
		bsp_plic_complete(claimed);
	}
	uint64_t deadline = bsp_read64(BSP_CLINT_MTIME) + window_ticks;
	uint32_t samples = 0u;
	uint32_t asserted = 0u;
	while(bsp_read64(BSP_CLINT_MTIME) < deadline)
	{
		if((bsp_read32(PLIC_PENDING_WORD0) & (1u << BSP_PMOD_IRQ)) != 0u)
		{
			asserted++;
			uint32_t id = bsp_plic_claim();
			if(id != 0u)
			{
				bsp_plic_complete(id);
			}
		}
		samples++;
	}
	return (samples != 0u) ? ((asserted * 100u) / samples) : 0u;
}

static void run_pin_continuity_test(void)
{
	uint64_t window = BSP_CLINT_TIMEBASE_HZ / 5u;   /* 200 ms */

	bsp_plic_set_priority(BSP_PMOD_IRQ, 1u);
	bsp_plic_enable(BSP_PMOD_IRQ);

	/* Baseline: nothing routed to the pin, so it should rest inactive-low. */
	bsp_pmod_spi_write_reg8(ICM_INT_SOURCE0_REG, 0x00u);
	bsp_pmod_spi_write_reg8(ICM_INT_CONFIG_REG,
	                        ICM_INT1_ACTIVE_HIGH_PP_LATCHED);
	uint32_t idle_low = sample_pending_percent(window);

	/* Forced high: polarity inverted, so the idle state is asserted. */
	bsp_pmod_spi_write_reg8(ICM_INT_CONFIG_REG,
	                        ICM_INT1_ACTIVE_LOW_PP_LATCHED);
	uint32_t forced_high = sample_pending_percent(window);

	/* Restore and let data-ready drive the pin normally. */
	bsp_pmod_spi_write_reg8(ICM_INT_CONFIG_REG,
	                        ICM_INT1_ACTIVE_HIGH_PP_LATCHED);
	bsp_pmod_spi_write_reg8(ICM_INT_SOURCE0_REG, ICM_UI_DRDY_INT1);
	uint32_t data_ready = sample_pending_percent(window);

	printf("PIN_TEST,idle_low=%lu%%,forced_high=%lu%%,data_ready=%lu%%\n",
	       (unsigned long)idle_low, (unsigned long)forced_high,
	       (unsigned long)data_ready);
	printf("PIN_TEST,int_config=0x%02x,int_source0=0x%02x\n",
	       bsp_pmod_spi_read_reg8(ICM_INT_CONFIG_REG | ICM_SPI_READ_BIT,
	                              ICM_SPI_DUMMY_BYTE),
	       bsp_pmod_spi_read_reg8(ICM_INT_SOURCE0_REG | ICM_SPI_READ_BIT,
	                              ICM_SPI_DUMMY_BYTE));

	if(forced_high >= 90u)
	{
		printf("VERDICT,INT1 reaches PLIC %lu. The pin is wired correctly; "
		       "any remaining silence is the source not re-arming, not the "
		       "wire.\n", (unsigned long)BSP_PMOD_IRQ);
	}
	else if(forced_high <= 10u)
	{
		printf("VERDICT,INT1 does NOT reach PLIC %lu. Holding the sensor pin "
		       "statically asserted produced no pending bit, so the FPGA input "
		       "(package pin AK30) is not connected to the sensor INT1 pin. "
		       "Confirm which physical PMOD position AK30 is in Digilent's "
		       "Genesys2 master XDC -- the comment in genesys2_pmod_irq.xdc is "
		       "not authoritative.\n", (unsigned long)BSP_PMOD_IRQ);
	}
	else
	{
		printf("VERDICT,intermittent. A marginal or noisy connection.\n");
	}
	bsp_plic_disable(BSP_PMOD_IRQ);
}

/* MEIE (bit 11) tells us whether the hart is actually accepting the PLIC's
 * external interrupt, which a pending bit alone does not. */
static uint64_t bsp_read_mie(void)
{
	uint64_t value;
	asm volatile("csrr %0, mie" : "=r"(value));
	return value;
}

static void delay_ticks(uint64_t ticks)
{
	uint64_t start = bsp_read64(BSP_CLINT_MTIME);
	while((bsp_read64(BSP_CLINT_MTIME) - start) < ticks)
	{
	}
}

static void dump_sensor_state(void)
{
	static const struct { uint8_t address; const char* name; } registers[] = {
		{0x14u, "INT_CONFIG"},
		{0x2du, "INT_STATUS"},
		{0x4eu, "PWR_MGMT0"},
		{0x4fu, "GYRO_CONFIG0"},
		{0x50u, "ACCEL_CONFIG0"},
		{0x63u, "INT_CONFIG0"},
		{0x64u, "INT_CONFIG1"},
		{0x65u, "INT_SOURCE0"},
		{0x75u, "WHO_AM_I"}
	};
	for(unsigned int i = 0u ;
	    i < sizeof(registers) / sizeof(registers[0]) ; i++)
	{
		printf("REG,%s,0x%02x=0x%02x\n", registers[i].name,
		       registers[i].address,
		       bsp_pmod_spi_read_reg8(registers[i].address | ICM_SPI_READ_BIT,
		                              ICM_SPI_DUMMY_BYTE));
	}

	/*
	 * Sample INT_STATUS over several ODR periods.  Reading it also clears it,
	 * so a healthy sensor sets DATA_RDY again on the very next sample and this
	 * count lands near the number of periods observed.
	 */
	uint64_t window_ticks = (BSP_CLINT_TIMEBASE_HZ * 10u) / NOISE_ODR_HZ;
	uint64_t deadline = bsp_read64(BSP_CLINT_MTIME) + window_ticks;
	uint32_t polls = 0u;
	uint32_t drdy_seen = 0u;
	while(bsp_read64(BSP_CLINT_MTIME) < deadline)
	{
		uint8_t status = bsp_pmod_spi_read_reg8(
			ICM_INT_STATUS_REG | ICM_SPI_READ_BIT, ICM_SPI_DUMMY_BYTE);
		polls++;
		if((status & ICM_DATA_READY_BIT) != 0u)
		{
			drdy_seen++;
		}
	}
	printf("DRDY_POLL,window_periods=10,polls=%lu,data_ready=%lu\n",
	       (unsigned long)polls, (unsigned long)drdy_seen);
}

static bool wait_for_sample(imu_sample_t* sample)
{
	/* The bound covers roughly ten ODR periods. */
	uint64_t deadline = bsp_read64(BSP_CLINT_MTIME) +
	                    (BSP_CLINT_TIMEBASE_HZ * 10u) / NOISE_ODR_HZ +
	                    BSP_CLINT_TIMEBASE_HZ / 10u;
	if(polled_mode)
	{
		return poll_for_sample(sample, deadline);
	}
	while(bsp_read64(BSP_CLINT_MTIME) < deadline)
	{
		if(tdk_icm42688_take_measurement(sample))
		{
			return true;
		}
	}
	return false;
}

static double norm3(const double vector[3])
{
	return sqrt(vector[0] * vector[0] + vector[1] * vector[1] +
	            vector[2] * vector[2]);
}

int main(void)
{
	printf("\nTDK ICM-42688-P stationary noise characterization\n");
	printf("KEEP THE BOARD COMPLETELY STILL FOR THE ENTIRE RUN\n");

	tdk_icm42688_config_t config;
	tdk_icm42688_default_config(&config);
	config.odr_hz = NOISE_ODR_HZ;

	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
	 * between claim and complete. */
	bsp_plic_complete(BSP_PMOD_IRQ);
	bsp_set_mtvec(tdk_noise_trap_handler);

	if(!tdk_icm42688_init(&config))
	{
		tdk_icm42688_diagnostics_t diagnostics;
		tdk_icm42688_get_diagnostics(&diagnostics);
		printf("INIT FAIL: who_am_i=0x%02x (expected 0x47)\n",
		       diagnostics.who_am_i);
		bsp_uart_wait_tx_idle();
		return 2;
	}

#if NOISE_PIN_TEST
	/* Runs with the core's external interrupts still masked, so a statically
	 * asserted line is polled rather than taken. */
	printf("CONFIG,odr_hz=%lu,accel_fs_g=%d,gyro_fs_mdps=%d,ui_filter=0x%02x\n",
	       (unsigned long)config.odr_hz, (int)config.accel_full_scale_g,
	       (int)config.gyro_full_scale_mdps, config.ui_filter_config);
	run_pin_continuity_test();
	tdk_icm42688_stop();
	bsp_uart_wait_tx_idle();
	return 0;
#endif

	bsp_enable_machine_external_interrupts();

	/*
	 * Separate a stale PLIC pending bit from a real interrupt.
	 *
	 * tdk_icm42688_init() leaves INT_SOURCE0 clear, so the sensor is not
	 * driving INT1 yet.  Enabling the PLIC route now and counting what arrives
	 * measures everything that is not the sensor: a pending bit left latched
	 * by a previous GDB-loaded image, or a floating input picking up noise.
	 * Without this baseline a single stale interrupt looks exactly like a
	 * working data-ready line, which is how one sample was mistaken for a
	 * healthy route.
	 */
	bsp_plic_enable(BSP_PMOD_IRQ);
	delay_ticks(BSP_CLINT_TIMEBASE_HZ / 2u);
	tdk_icm42688_diagnostics_t baseline;
	tdk_icm42688_get_diagnostics(&baseline);

	if(!tdk_icm42688_start())
	{
		printf("START FAIL\n");
		bsp_uart_wait_tx_idle();
		return 3;
	}
	/*
	 * Sample the PLIC pending bit while the sensor is armed and the core's
	 * external interrupts are unmasked.  This separates the two remaining
	 * layers: pending asserting without the trap count moving means the
	 * gateway sees the line but delivery to the hart is broken, whereas
	 * pending never asserting means the gateway is not reacting to a pin the
	 * continuity test already proved is toggling.
	 */
	uint64_t armed_deadline = bsp_read64(BSP_CLINT_MTIME) + BSP_CLINT_TIMEBASE_HZ;
	uint32_t pending_samples = 0u;
	uint32_t pending_asserted = 0u;
	while(bsp_read64(BSP_CLINT_MTIME) < armed_deadline)
	{
		if((bsp_read32(PLIC_PENDING_WORD0) & (1u << BSP_PMOD_IRQ)) != 0u)
		{
			pending_asserted++;
		}
		pending_samples++;
	}
	tdk_icm42688_diagnostics_t armed;
	tdk_icm42688_get_diagnostics(&armed);
	printf("PLIC_PENDING,armed_samples=%lu,asserted=%lu,percent=%lu,"
	       "mie_meie=%lu,threshold_ok=1\n",
	       (unsigned long)pending_samples, (unsigned long)pending_asserted,
	       (unsigned long)((pending_samples != 0u)
	                       ? (pending_asserted * 100u) / pending_samples : 0u),
	       (unsigned long)((bsp_read_mie() >> 11) & 1u));
	printf("IRQ_PROBE,quiet_500ms_irq=%lu,armed_1s_irq=%lu,armed_samples=%lu,"
	       "spurious=%lu,expected_hz=%lu\n",
	       (unsigned long)baseline.irq_count,
	       (unsigned long)(armed.irq_count - baseline.irq_count),
	       (unsigned long)armed.sample_count,
	       (unsigned long)armed.spurious_irq_count,
	       (unsigned long)NOISE_ODR_HZ);

	printf("CONFIG,odr_hz=%lu,accel_fs_g=%d,gyro_fs_mdps=%d,"
	       "ui_filter=0x%02x,tick_hz=%lu\n",
	       (unsigned long)config.odr_hz,
	       (int)config.accel_full_scale_g,
	       (int)config.gyro_full_scale_mdps,
	       config.ui_filter_config,
	       (unsigned long)BSP_CLINT_TIMEBASE_HZ);
	printf("PLAN,discard=%lu,bias=%lu,measure=%lu,seconds=%lu\n",
	       (unsigned long)NOISE_DISCARD_SAMPLES,
	       (unsigned long)NOISE_BIAS_SAMPLES,
	       (unsigned long)NOISE_MEASURE_SAMPLES,
	       (unsigned long)((NOISE_DISCARD_SAMPLES + NOISE_BIAS_SAMPLES +
	                        NOISE_MEASURE_SAMPLES) / NOISE_ODR_HZ));

	imu_sample_t sample;
	uint32_t dropouts = 0u;

	/*
	 * Decide on the sustained rate measured above, not on a single sample.
	 * A lone interrupt is exactly what a stale pending bit produces, and
	 * whether it happens to carry DATA_RDY is chance -- so one sample was
	 * previously enough to report a healthy route that did not exist.
	 */
	{
		uint64_t observed_hz = armed.sample_count;
		uint64_t stale_hz = baseline.irq_count;
		if(observed_hz < (NOISE_ODR_HZ / 2u))
		{
#if NOISE_REQUIRE_IRQ
			printf("DELIVERY,mode=FAILED,reason=rate_too_low,"
			       "observed_hz=%lu,expected_hz=%lu,stale_irq=%lu\n",
			       (unsigned long)observed_hz, (unsigned long)NOISE_ODR_HZ,
			       (unsigned long)stale_hz);
			if((stale_hz != 0u) && (observed_hz == 0u))
			{
				printf("VERDICT,the only interrupts arrived before the sensor "
				       "was driving INT1, so they came from a stale PLIC "
				       "pending bit or a floating input -- not from the "
				       "sensor. INT1 is still not reaching PLIC %lu.\n",
				       (unsigned long)BSP_PMOD_IRQ);
			}
			else if(observed_hz == 0u)
			{
				printf("VERDICT,no interrupts at all. INT1 is not reaching "
				       "PLIC %lu; check the pin named in "
				       "hardware/fpga/genesys2/constraints/"
				       "genesys2_pmod_irq.xdc.\n",
				       (unsigned long)BSP_PMOD_IRQ);
			}
			else
			{
				printf("VERDICT,the route works but the source is not "
				       "re-arming at the configured rate.\n");
			}
			dump_sensor_state();
			tdk_icm42688_stop();
			bsp_uart_wait_tx_idle();
			return 5;
#else
			polled_mode = true;
			printf("DELIVERY,mode=polled,reason=rate_too_low,"
			       "observed_hz=%lu,expected_hz=%lu,stale_irq=%lu\n",
			       (unsigned long)observed_hz, (unsigned long)NOISE_ODR_HZ,
			       (unsigned long)stale_hz);
			/* Drop only the PLIC route.  INT_SOURCE0 stays enabled so the
			 * DATA_RDY condition keeps setting INT_STATUS for polling, and
			 * no ISR competes to read (and clear) that register. */
			bsp_plic_disable(BSP_PMOD_IRQ);
#endif
		}
		else
		{
			printf("DELIVERY,mode=interrupt,rate_hz=%lu,stale_irq=%lu\n",
			       (unsigned long)observed_hz, (unsigned long)stale_hz);
		}
	}

	for(uint32_t i = 0u ; i < NOISE_DISCARD_SAMPLES ; i++)
	{
		if(!wait_for_sample(&sample))
		{
			dropouts++;
		}
	}

	/* Pass 1: the stationary mean is gravity plus accel bias, and gyro bias.
	 * Removing it reproduces the residual the ESKF sees once calibrated. */
	double accel_sum[3] = {0.0, 0.0, 0.0};
	double gyro_sum[3] = {0.0, 0.0, 0.0};
	double accel_sum_squares[3] = {0.0, 0.0, 0.0};
	double gyro_sum_squares[3] = {0.0, 0.0, 0.0};
	uint32_t bias_count = 0u;
	for(uint32_t i = 0u ; i < NOISE_BIAS_SAMPLES ; i++)
	{
		if(!wait_for_sample(&sample))
		{
			dropouts++;
			continue;
		}
		for(unsigned int axis = 0u ; axis < 3u ; axis++)
		{
			accel_sum[axis] += sample.accel_mps2[axis];
			gyro_sum[axis] += sample.gyro_rps[axis];
			accel_sum_squares[axis] +=
				sample.accel_mps2[axis] * sample.accel_mps2[axis];
			gyro_sum_squares[axis] +=
				sample.gyro_rps[axis] * sample.gyro_rps[axis];
		}
		bias_count++;
	}
	if(bias_count == 0u)
	{
		printf("NO SAMPLES: check PMOD wiring and INT1 routing\n");
		bsp_uart_wait_tx_idle();
		return 4;
	}

	double accel_mean[3];
	double gyro_mean[3];
	for(unsigned int axis = 0u ; axis < 3u ; axis++)
	{
		accel_mean[axis] = accel_sum[axis] / (double)bias_count;
		gyro_mean[axis] = gyro_sum[axis] / (double)bias_count;
		double accel_variance =
			(accel_sum_squares[axis] / (double)bias_count) -
			(accel_mean[axis] * accel_mean[axis]);
		double gyro_variance =
			(gyro_sum_squares[axis] / (double)bias_count) -
			(gyro_mean[axis] * gyro_mean[axis]);
		printf("AXIS,%u,accel_mean=%ld,accel_std=%ld,"
		       "gyro_mean=%ld,gyro_std=%ld\n",
		       axis, micro(accel_mean[axis]),
		       micro(sqrt(accel_variance > 0.0 ? accel_variance : 0.0)),
		       micro(gyro_mean[axis]),
		       micro(sqrt(gyro_variance > 0.0 ? gyro_variance : 0.0)));
	}
	printf("GRAVITY,measured=%ld,expected=9806650\n", micro(norm3(accel_mean)));

	/* Pass 2: the detector inputs themselves. */
	statistic_t accel_statistic;
	statistic_t gyro_statistic;
	statistic_t jerk_statistic;
	statistic_init(&accel_statistic, "accel_norm", "um/s2",
	               ACCEL_HISTOGRAM_FULL_SCALE);
	statistic_init(&gyro_statistic, "gyro_norm", "urad/s",
	               GYRO_HISTOGRAM_FULL_SCALE);
	statistic_init(&jerk_statistic, "jerk_norm", "um/s3",
	               JERK_HISTOGRAM_FULL_SCALE);

	double filtered[3] = {0.0, 0.0, 0.0};
	bool have_filtered = false;
	uint64_t previous_ticks = 0u;

	for(uint32_t i = 0u ; i < NOISE_MEASURE_SAMPLES ; i++)
	{
		if(!wait_for_sample(&sample))
		{
			dropouts++;
			continue;
		}
		double residual_accel[3];
		double residual_gyro[3];
		for(unsigned int axis = 0u ; axis < 3u ; axis++)
		{
			residual_accel[axis] = sample.accel_mps2[axis] - accel_mean[axis];
			residual_gyro[axis] = sample.gyro_rps[axis] - gyro_mean[axis];
		}
		statistic_add(&accel_statistic, norm3(residual_accel));
		statistic_add(&gyro_statistic, norm3(residual_gyro));

		if(previous_ticks != 0u)
		{
			double dt_s = (double)(sample.timestamp_ticks - previous_ticks) /
			              (double)BSP_CLINT_TIMEBASE_HZ;
			if(dt_s > 0.0)
			{
				double alpha = dt_s / (JERK_TIME_CONSTANT_S + dt_s);
				double delta[3];
				for(unsigned int axis = 0u ; axis < 3u ; axis++)
				{
					double previous = filtered[axis];
					filtered[axis] += alpha * (residual_accel[axis] - previous);
					delta[axis] = filtered[axis] - previous;
				}
				if(have_filtered)
				{
					statistic_add(&jerk_statistic, norm3(delta) / dt_s);
				}
				have_filtered = true;
			}
		}
		else
		{
			memcpy(filtered, residual_accel, sizeof(filtered));
		}
		previous_ticks = sample.timestamp_ticks;

		if(((i + 1u) % (NOISE_ODR_HZ * 10u)) == 0u)
		{
			printf("PROGRESS,%lu/%lu\n", (unsigned long)(i + 1u),
			       (unsigned long)NOISE_MEASURE_SAMPLES);
		}
	}

	statistic_report(&accel_statistic);
	statistic_report(&gyro_statistic);
	statistic_report(&jerk_statistic);

	tdk_icm42688_diagnostics_t diagnostics;
	tdk_icm42688_get_diagnostics(&diagnostics);
	printf("HEALTH,irq=%lu,samples=%lu,invalid=%lu,spurious=%lu,"
	       "overflow=%lu,missed=%lu,dropouts=%lu,unexpected_irq=%lu,"
	       "trap_cause=%lu\n",
	       (unsigned long)diagnostics.irq_count,
	       (unsigned long)diagnostics.sample_count,
	       (unsigned long)diagnostics.invalid_sample_count,
	       (unsigned long)diagnostics.spurious_irq_count,
	       (unsigned long)diagnostics.queue_overflow_count,
	       (unsigned long)diagnostics.estimated_missed_sample_count,
	       (unsigned long)dropouts,
	       (unsigned long)unexpected_irq_count,
	       (unsigned long)unexpected_trap_cause);
	printf("DONE\n");

	tdk_icm42688_stop();
	bsp_uart_wait_tx_idle();
	return 0;
}