/* * 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 #include #include #include #include "bsp/irq.h" #include "bsp/mmio.h" #include "bsp/plic.h" #include "bsp/pmod_spi.h" #include "bsp/soc.h" #include "bsp/uart.h" #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; }