diff --git a/soc/software/fusion/tests/Makefile b/soc/software/fusion/tests/Makefile index 2aeb834..ec27796 100644 --- a/soc/software/fusion/tests/Makefile +++ b/soc/software/fusion/tests/Makefile @@ -10,7 +10,7 @@ IMU_PROFILE_SOURCE := ../../gps_system/src/adapters/imu_fusion_profile.c .PHONY: all test gps-system-test scenario-logs sanitize profile benchmark clean -all: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test +all: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test calibration_offset_test long_outage_trajectory_test standalone_gps_integrity_test: standalone_gps_integrity_test.c $(GPS_TIME_SOURCE) $(GPS_EPOCH_SOURCE) $(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) @@ -42,7 +42,13 @@ platform_scenario_matrix_test: platform_scenario_matrix_test.c $(ESKF_SOURCES) gps_failure_scenario_test: gps_failure_scenario_test.c $(ESKF_SOURCES) $(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) -test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test +calibration_offset_test: calibration_offset_test.c $(ESKF_SOURCES) $(IMU_PROFILE_SOURCE) + $(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) + +long_outage_trajectory_test: long_outage_trajectory_test.c $(ESKF_SOURCES) $(IMU_PROFILE_SOURCE) + $(CC) $(CFLAGS) $^ -o $@ $(LDLIBS) + +test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test fusion_synthetic_test imu_quality_synthetic_test async_tilt_recovery_test async_rate_matrix_test gps_reacquisition_test platform_scenario_matrix_test gps_failure_scenario_test calibration_offset_test long_outage_trajectory_test ./standalone_gps_integrity_test ./eskf_motion_test ./fusion_rest_detector_test @@ -53,6 +59,8 @@ test: standalone_gps_integrity_test eskf_motion_test fusion_rest_detector_test f ./gps_reacquisition_test ./platform_scenario_matrix_test ./gps_failure_scenario_test + ./calibration_offset_test + ./long_outage_trajectory_test gps-system-test: standalone_gps_integrity_test ./standalone_gps_integrity_test @@ -146,4 +154,4 @@ benchmark: standalone_gps_integrity_test ./standalone_gps_integrity_test --bench-double 100000 clean: - rm -f standalone_gps_integrity_test standalone_gps_integrity_test_san standalone_gps_integrity_test_gprof eskf_motion_test eskf_motion_test_san fusion_rest_detector_test fusion_rest_detector_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 async_tilt_recovery_test async_tilt_recovery_test_san async_rate_matrix_test async_rate_matrix_test_san gps_reacquisition_test gps_reacquisition_test_san platform_scenario_matrix_test platform_scenario_matrix_test_san gps_failure_scenario_test gps_failure_scenario_test_san 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_rest_detector_test fusion_rest_detector_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 async_tilt_recovery_test async_tilt_recovery_test_san async_rate_matrix_test async_rate_matrix_test_san gps_reacquisition_test gps_reacquisition_test_san platform_scenario_matrix_test platform_scenario_matrix_test_san gps_failure_scenario_test gps_failure_scenario_test_san calibration_offset_test calibration_offset_test_san long_outage_trajectory_test long_outage_trajectory_test_san gmon.out diff --git a/soc/software/fusion/tests/calibration_offset_test.c b/soc/software/fusion/tests/calibration_offset_test.c new file mode 100644 index 0000000..5f05e9b --- /dev/null +++ b/soc/software/fusion/tests/calibration_offset_test.c @@ -0,0 +1,180 @@ +/* + * Startup calibration must complete on a sensor whose zero-g offset is within + * datasheet spec, and must still refuse to calibrate while the platform is + * accelerating. + * + * The bench ICM-42688-P on this project measures a stationary accelerometer + * magnitude of 10.0847 m/s^2 against a 9.80665 m/s^2 reference: a +28 mg + * zero-g offset, comfortably in spec. When one tolerance served both as the + * sensor-offset allowance and as the motion gate, that offset consumed 93% of + * it and left 0.022 m/s^2 of motion headroom, so ordinary sensor noise reset + * the accumulator and the engine never left FUSION_ENGINE_CALIBRATING. Field + * logs show exactly that: calibration_samples oscillating 0,3,0,4,2,1,0,7... + * and never reaching calibration_min_samples. + * + * Widening a single combined tolerance would have restored startup at the cost + * of the motion gate, so the offset case and the acceleration case are both + * asserted here. + */ + +#include +#include +#include +#include +#include + +#include "adapters/imu_fusion_profile.h" +#include "fusion/fusion_engine.h" +#include "imu/imu_sensor.h" + +#define TIMEBASE_HZ 1000000u +#define IMU_HZ 50u +#define DURATION_S 8u +#define GRAVITY_MPS2 9.80665 + +/* Measured on the bench unit: see soc/software/tdk_noise_test. */ +#define MEASURED_Z_OFFSET_MPS2 0.278 +#define MEASURED_ACCEL_NOISE_MPS2 0.0028 +#define MEASURED_GYRO_NOISE_RPS 0.00016 + +static int failures; + +#define CHECK(condition, message) do { \ + if(!(condition)) { \ + fprintf(stderr, "FAIL: %s\n", message); \ + failures++; \ + } \ +} while(0) + +static uint32_t random_state = 12345u; + +static double uniform_random(void) +{ + random_state = 1664525u * random_state + 1013904223u; + return ((double)((random_state >> 8) & 0x00ffffffu) + 0.5) / 16777216.0; +} + +static double gaussian_random(void) +{ + double first = fmax(uniform_random(), 1.0e-12); + double second = uniform_random(); + return sqrt(-2.0 * log(first)) * cos(2.0 * 3.14159265358979323846 * second); +} + +/* + * Runs a stationary (or uniformly accelerating) calibration window and reports + * whether the engine reached the running state. + */ +static bool run_calibration(double z_offset_mps2, + double horizontal_acceleration_mps2, + imu_sensor_backend_t backend, + unsigned int* samples_out) +{ + fusion_imu_quality_profile_t profile; + fusion_engine_config_t config; + fusion_engine_t engine; + + fusion_engine_default_config(&config, TIMEBASE_HZ); + if(!imu_fusion_quality_profile_for_backend(backend, &profile)) + { + return false; + } + if(!fusion_engine_apply_imu_quality(&config, &profile)) + { + return false; + } + if(!fusion_engine_init(&engine, &config)) + { + return false; + } + + random_state = 12345u; + unsigned int total = DURATION_S * IMU_HZ; + for(unsigned int i = 0u ; i < total ; i++) + { + fusion_imu_measurement_t imu; + memset(&imu, 0, sizeof(imu)); + imu.timestamp_ticks = + (uint64_t)(i + 1u) * (uint64_t)(TIMEBASE_HZ / IMU_HZ); + imu.accel_mps2[0] = horizontal_acceleration_mps2 + + MEASURED_ACCEL_NOISE_MPS2 * gaussian_random(); + imu.accel_mps2[1] = MEASURED_ACCEL_NOISE_MPS2 * gaussian_random(); + imu.accel_mps2[2] = GRAVITY_MPS2 + z_offset_mps2 + + MEASURED_ACCEL_NOISE_MPS2 * gaussian_random(); + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + imu.gyro_rps[axis] = MEASURED_GYRO_NOISE_RPS * gaussian_random(); + } + imu.temperature_c = 25.0; + imu.sequence = i; + imu.vehicle_at_rest = true; + (void)fusion_engine_push_imu(&engine, &imu); + } + + fusion_runtime_t runtime; + bool running = fusion_engine_get_runtime(&engine, &runtime); + if(samples_out != NULL) + { + *samples_out = engine.calibration.sample_count; + } + return running; +} + +int main(void) +{ + unsigned int samples = 0u; + + /* The regression: an in-spec zero-g offset must not block startup. */ + CHECK(run_calibration(MEASURED_Z_OFFSET_MPS2, 0.0, + IMU_SENSOR_TDK_ICM42688, &samples), + "TDK calibration must complete with the measured +28 mg zero-g offset"); + printf("bench offset z=%.3f m/s^2 accel_norm=%.4f -> %s\n", + MEASURED_Z_OFFSET_MPS2, GRAVITY_MPS2 + MEASURED_Z_OFFSET_MPS2, + failures == 0 ? "calibrated" : "STUCK"); + + /* A part right at the datasheet limit must still start. */ + CHECK(run_calibration(0.392, 0.0, IMU_SENSOR_TDK_ICM42688, NULL), + "TDK calibration must complete at the +-40 mg datasheet offset limit"); + + /* The same must hold for the ST backend. */ + CHECK(run_calibration(MEASURED_Z_OFFSET_MPS2, 0.0, + IMU_SENSOR_ST_LSM6DSOX, NULL), + "ST calibration must complete with an in-spec zero-g offset"); + + /* + * The motion gate must survive the offset allowance. Calibrating under + * power would align attitude to a tilted specific-force vector, so a + * sustained acceleration large enough to be visible in the magnitude has + * to be rejected on both an ideal and an offset part. + */ + CHECK(!run_calibration(0.0, 3.0, IMU_SENSOR_TDK_ICM42688, NULL), + "calibration must refuse a 3.0 m/s^2 sustained acceleration"); + CHECK(!run_calibration(MEASURED_Z_OFFSET_MPS2, 3.0, + IMU_SENSOR_TDK_ICM42688, NULL), + "calibration must refuse 3.0 m/s^2 on an offset part too"); + + /* + * Documented blind spot, asserted so it stays a known quantity rather than + * an assumption. Horizontal acceleration adds to gravity in quadrature, + * so 1.5 m/s^2 moves the magnitude by only 0.11 m/s^2 -- below any usable + * offset allowance, and below the original 0.30 tolerance as well. This + * is not a regression; separating it needs GNSS velocity, not a tighter + * accelerometer gate. + */ + CHECK(run_calibration(0.0, 1.5, IMU_SENSOR_TDK_ICM42688, NULL), + "1.5 m/s^2 is expected to remain indistinguishable from offset"); + + /* A grossly wrong magnitude still indicates a misconfigured range. */ + CHECK(!run_calibration(5.0, 0.0, IMU_SENSOR_TDK_ICM42688, NULL), + "calibration must refuse an implausible accelerometer magnitude"); + + if(failures != 0) + { + fprintf(stderr, "FAIL: %d calibration offset check(s) failed\n", + failures); + return 1; + } + printf("PASS: in-spec zero-g offset calibrates; sustained acceleration and " + "implausible magnitude are still refused.\n"); + return 0; +} diff --git a/soc/software/fusion/tests/long_outage_trajectory_test.c b/soc/software/fusion/tests/long_outage_trajectory_test.c new file mode 100644 index 0000000..70f790d --- /dev/null +++ b/soc/software/fusion/tests/long_outage_trajectory_test.c @@ -0,0 +1,816 @@ +/* + * Pedestrian and gentle-vehicle trajectories across 1 and 5 minute GNSS + * outages. + * + * What this is actually checking + * ----------------------------- + * Five minutes of inertial-only navigation on a consumer MEMS part drifts by + * hundreds of metres to kilometres. That is physics, not a defect: a residual + * gyro bias of only 1.6e-5 rad/s -- what a 2 s static calibration leaves + * behind at this sensor's measured noise -- tilts the gravity projection by + * 0.27 deg over 300 s and integrates to roughly 2 km. Asserting a small + * outage error would therefore only be asserting that the synthetic IMU is + * unrealistically good. + * + * The properties worth defending are the ones that separate honest dead + * reckoning from a filter that has come apart: + * + * 1. every published state stays finite, with no step discontinuities + * 2. drift accumulates smoothly rather than diverging super-quadratically + * 3. GNSS re-acquisition pulls the solution back within seconds + * 4. constant-velocity cruise is never mistaken for standing still + * + * Property 4 is the field failure this suite exists for. A cruising vehicle + * produces almost no specific force and almost no angular rate, so a rest + * detector fed only inertial data declares rest. These cases therefore assert + * vehicle_at_rest during constant-velocity cruise on purpose, reproducing that + * mistake, and require the filter to keep tracking anyway on the strength of + * GNSS Doppler velocity. + * + * Property 3 is the regression guard for the innovation-gate lockout: once the + * filter had drifted far enough for every fix to fail the NIS gate, nothing + * could re-anchor it and the reported position stayed on the dead-reckoned + * track indefinitely. + * + * IMU noise and bias below are the values measured on this project's bench + * ICM-42688-P; see soc/software/tdk_noise_test. + */ + +#include +#include +#include +#include +#include + +#include "adapters/imu_fusion_profile.h" +#include "fusion/fusion_engine.h" +#include "fusion/fusion_rest_detector.h" +#include "imu/imu_sensor.h" + +#define TIMEBASE_HZ 1000000u +#define IMU_HZ 50u +#define GPS_PERIOD_MS 200u +#define PI 3.14159265358979323846 + +/* Bench-measured stationary noise (soc/software/tdk_noise_test). */ +#define ACCEL_NOISE_MPS2 0.0028 +#define GYRO_NOISE_RPS 0.00016 +/* Residual bias left after a short static calibration. */ +#define ACCEL_BIAS_MPS2 0.010 +#define GYRO_BIAS_RPS 0.00004 + +/* + * Broadband platform vibration. A purely kinematic trajectory has none, which + * would flatter the rest detector. These are set at the per-sample sigma the + * TDK profile's process noise already assumes at 50 Hz (0.125 m/s^2 and + * 0.011 rad/s), so the synthetic stream stays consistent with what the filter + * expects; injecting more would make the covariance check fail for modelling + * reasons rather than for filter behaviour. + */ +#define ROAD_VIBRATION_MPS2 0.125 +#define ROAD_VIBRATION_RPS 0.011 + +#define GPS_HORIZONTAL_STD_M 3.0 +#define GPS_VERTICAL_STD_M 5.0 +#define GPS_VELOCITY_STD_MPS 0.3 + +/* Timeline shared by every case. */ +#define STATIONARY_UNTIL_S 10.0 +#define ACCEL_START_S 10.0 +#define OUTAGE_START_S 90.0 +#define RECOVERY_OBSERVE_S 60.0 + +static int failures; + +#define CHECK(condition, message) do { \ + if(!(condition)) { \ + fprintf(stderr, "FAIL: %s\n", message); \ + failures++; \ + } \ +} while(0) + +typedef enum { + PROFILE_PEDESTRIAN = 0, + PROFILE_GENTLE_CAR, + /* + * The field failure's operating point. GNSS motion evidence compares the + * displacement between fixes against their combined covariance, so slow + * travel under mediocre accuracy reads as stationary: 4 m/s over the 1 s + * window is 4 m of displacement against 6 m per-fix sigma, a displacement + * NIS of 0.22 versus a 9.21 gate. Highway speed with good accuracy scores + * 42.9 and is correctly called moving, which is why fast cruise alone does + * not exercise the zero-velocity lockout. + */ + PROFILE_SLOW_CITY, + /* + * The field configuration. Board logs show 609 zero-velocity updates while + * the vehicle was driving at 3-5 m/s, which only happens if the mounting + * isolates the sensor from road vibration well enough for the rest + * supervisor's vibration metric to stay under its limit. Suppressing + * vibration here reproduces that: the supervisor genuinely declares rest + * while moving, and the engine must survive being told so. + */ + PROFILE_ISOLATED_CITY, + /* + * Repeated hard course changes while unaided. Answers whether the path + * shape survives dead reckoning, separately from how far the whole path + * drifts: heading comes from integrating a directly measured angular rate, + * whereas position comes from double-integrating a specific force that + * carries the attitude error times gravity. The two degrade at very + * different rates, so the turns are placed inside the outage window. + */ + PROFILE_URBAN_TURNS +} profile_kind_t; + +#define PROFILE_COUNT 5u + +typedef struct { + double position_enu_m[3]; + double velocity_enu_mps[3]; + double acceleration_enu_mps2[3]; + double yaw_rad; + double yaw_rate_rps; + double speed_mps; + bool constant_velocity; +} truth_t; + +static uint32_t random_state; + +static double uniform_random(void) +{ + random_state = 1664525u * random_state + 1013904223u; + return ((double)((random_state >> 8) & 0x00ffffffu) + 0.5) / 16777216.0; +} + +static double gaussian_random(void) +{ + double first = fmax(uniform_random(), 1.0e-12); + double second = uniform_random(); + return sqrt(-2.0 * log(first)) * cos(2.0 * PI * second); +} + +static const char* profile_name(profile_kind_t profile) +{ + switch(profile) + { + case PROFILE_PEDESTRIAN: return "pedestrian"; + case PROFILE_GENTLE_CAR: return "gentle-car"; + case PROFILE_SLOW_CITY: return "slow-city"; + case PROFILE_ISOLATED_CITY: return "isolated-city"; + default: return "urban-turns"; + } +} + +/* Slow urban travel is where the receiver reports its weakest geometry. */ +static double profile_gps_horizontal_std_m(profile_kind_t profile) +{ + return ((profile == PROFILE_SLOW_CITY) || + (profile == PROFILE_ISOLATED_CITY) || + (profile == PROFILE_URBAN_TURNS)) ? 6.0 : GPS_HORIZONTAL_STD_M; +} + +/* + * Longitudinal speed and yaw rate. + * + * The vehicle case is the requested 0 to 100 km/h in 30 s, a constant + * 0.926 m/s^2, followed by open-road cruise. Cruise is where specific force + * returns to gravity alone and the platform becomes inertially + * indistinguishable from parked. + */ +static void profile_command(profile_kind_t profile, double time_s, + double* speed_mps, double* yaw_rate_rps, + double* acceleration_mps2, + bool* constant_velocity) +{ + *yaw_rate_rps = 0.0; + *acceleration_mps2 = 0.0; + *constant_velocity = false; + + if(time_s < STATIONARY_UNTIL_S) + { + *speed_mps = 0.0; + *constant_velocity = true; + return; + } + + if(profile == PROFILE_PEDESTRIAN) + { + /* Ramp to a 1.4 m/s walk, then hold it with slow course changes. */ + double elapsed = time_s - ACCEL_START_S; + if(elapsed < 3.0) + { + *speed_mps = 1.4 * (elapsed / 3.0); + *acceleration_mps2 = 1.4 / 3.0; + return; + } + *speed_mps = 1.4; + *constant_velocity = true; + /* A gentle serpentine keeps the gyro genuinely excited. */ + *yaw_rate_rps = 0.05 * sin(2.0 * PI * (time_s - 13.0) / 40.0); + return; + } + + if(profile == PROFILE_URBAN_TURNS) + { + double elapsed = time_s - ACCEL_START_S; + if(elapsed < 8.0) + { + *acceleration_mps2 = 4.0 / 8.0; + *speed_mps = *acceleration_mps2 * elapsed; + return; + } + *speed_mps = 4.0; + /* A 90 degree turn every 12 s, each swept over 3 s at 30 deg/s -- + * an ordinary city block, and a large signal next to the 0.0092 deg/s + * gyro noise measured on this part. */ + double cycle = fmod(time_s - 18.0, 12.0); + if((cycle >= 0.0) && (cycle < 3.0)) + { + *yaw_rate_rps = (PI / 2.0) / 3.0; + } + else + { + *constant_velocity = true; + } + return; + } + + if((profile == PROFILE_SLOW_CITY) || (profile == PROFILE_ISOLATED_CITY)) + { + /* Gentle pull-away to a 4 m/s urban cruise. */ + double elapsed = time_s - ACCEL_START_S; + if(elapsed < 8.0) + { + *acceleration_mps2 = 4.0 / 8.0; + *speed_mps = *acceleration_mps2 * elapsed; + return; + } + *speed_mps = 4.0; + *constant_velocity = true; + *yaw_rate_rps = 0.010 * sin(2.0 * PI * (time_s - 18.0) / 90.0); + return; + } + + /* 0 -> 100 km/h (27.78 m/s) in 30 s, then constant-velocity cruise. */ + double target_mps = 100.0 / 3.6; + double ramp_s = 30.0; + double elapsed = time_s - ACCEL_START_S; + if(elapsed < ramp_s) + { + *acceleration_mps2 = target_mps / ramp_s; + *speed_mps = *acceleration_mps2 * elapsed; + return; + } + *speed_mps = target_mps; + *constant_velocity = true; + /* A long highway curve; small enough that lateral acceleration stays + * well under the rest detector's thresholds. */ + *yaw_rate_rps = 0.004 * sin(2.0 * PI * (time_s - 40.0) / 120.0); +} + +static void propagate_truth(truth_t* truth, profile_kind_t profile, + double start_s, double dt_s) +{ + if(dt_s <= 0.0) + { + return; + } + double speed_mps; + double yaw_rate_rps; + double acceleration_mps2; + bool constant_velocity; + profile_command(profile, start_s + 0.5 * dt_s, &speed_mps, &yaw_rate_rps, + &acceleration_mps2, &constant_velocity); + + truth->yaw_rad += yaw_rate_rps * dt_s; + truth->yaw_rate_rps = yaw_rate_rps; + truth->speed_mps = speed_mps; + truth->constant_velocity = constant_velocity; + + double forward_e = cos(truth->yaw_rad); + double forward_n = sin(truth->yaw_rad); + double previous_velocity[3]; + memcpy(previous_velocity, truth->velocity_enu_mps, + sizeof(previous_velocity)); + + truth->velocity_enu_mps[0] = speed_mps * forward_e; + truth->velocity_enu_mps[1] = speed_mps * forward_n; + truth->velocity_enu_mps[2] = 0.0; + + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + truth->acceleration_enu_mps2[axis] = + (truth->velocity_enu_mps[axis] - previous_velocity[axis]) / dt_s; + truth->position_enu_m[axis] += 0.5 * dt_s * + (previous_velocity[axis] + truth->velocity_enu_mps[axis]); + } + (void)acceleration_mps2; +} + +/* + * Body-frame specific force and angular rate for a level platform. + * + * The accelerometer senses a - g_vec, so with ENU navigation and g_vec + * pointing down the vertical channel carries +g. Yaw rotates the horizontal + * components into the body frame. + */ +static fusion_imu_measurement_t make_imu( + profile_kind_t profile, const truth_t* truth, double time_s, + uint64_t ticks, double gravity_mps2, unsigned int sequence) +{ + fusion_imu_measurement_t imu; + memset(&imu, 0, sizeof(imu)); + imu.timestamp_ticks = ticks; + + double cos_yaw = cos(truth->yaw_rad); + double sin_yaw = sin(truth->yaw_rad); + double east = truth->acceleration_enu_mps2[0]; + double north = truth->acceleration_enu_mps2[1]; + + imu.accel_mps2[0] = east * cos_yaw + north * sin_yaw; + imu.accel_mps2[1] = -east * sin_yaw + north * cos_yaw; + imu.accel_mps2[2] = gravity_mps2; + imu.gyro_rps[0] = 0.0; + imu.gyro_rps[1] = 0.0; + imu.gyro_rps[2] = truth->yaw_rate_rps; + + if((profile == PROFILE_PEDESTRIAN) && (truth->speed_mps > 0.1)) + { + /* Step cadence near 1.9 Hz: strong vertical bob plus lateral sway. + * This is the signature that keeps a walker out of the rest state. */ + double phase = 2.0 * PI * 1.9 * time_s; + imu.accel_mps2[2] += 1.30 * sin(phase); + imu.accel_mps2[1] += 0.55 * sin(0.5 * phase); + imu.accel_mps2[0] += 0.30 * sin(phase + 0.7); + imu.gyro_rps[0] += 0.12 * sin(0.5 * phase); + imu.gyro_rps[1] += 0.09 * cos(phase); + } + + bool rolling = truth->speed_mps > 0.5; + double mount = (profile == PROFILE_ISOLATED_CITY) ? 0.05 : 1.0; + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + imu.accel_mps2[axis] += + ACCEL_BIAS_MPS2 + ACCEL_NOISE_MPS2 * gaussian_random(); + imu.gyro_rps[axis] += + GYRO_BIAS_RPS + GYRO_NOISE_RPS * gaussian_random(); + if(rolling) + { + imu.accel_mps2[axis] += + mount * ROAD_VIBRATION_MPS2 * gaussian_random(); + imu.gyro_rps[axis] += mount * ROAD_VIBRATION_RPS * gaussian_random(); + } + } + imu.temperature_c = 25.0; + imu.sequence = sequence; + + /* vehicle_at_rest is filled in by the caller from the real supervisor. */ + return imu; +} + +static fusion_navigation_measurement_t make_navigation( + const truth_t* truth, uint64_t ticks, double horizontal_std_m) +{ + fusion_navigation_measurement_t navigation; + memset(&navigation, 0, sizeof(navigation)); + navigation.position.timestamp_ticks = ticks; + navigation.position.position_enu_m[0] = + truth->position_enu_m[0] + horizontal_std_m * gaussian_random(); + navigation.position.position_enu_m[1] = + truth->position_enu_m[1] + horizontal_std_m * gaussian_random(); + navigation.position.position_enu_m[2] = + truth->position_enu_m[2] + GPS_VERTICAL_STD_M * gaussian_random(); + navigation.position.covariance_enu_m2[0] = horizontal_std_m * horizontal_std_m; + navigation.position.covariance_enu_m2[4] = horizontal_std_m * horizontal_std_m; + navigation.position.covariance_enu_m2[8] = + GPS_VERTICAL_STD_M * GPS_VERTICAL_STD_M; + navigation.position.source_quality = 85.0; + navigation.position.source_valid = true; + + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + navigation.velocity_enu_mps[axis] = truth->velocity_enu_mps[axis] + + GPS_VELOCITY_STD_MPS * gaussian_random(); + } + navigation.velocity_covariance_enu_mps2[0] = + GPS_VELOCITY_STD_MPS * GPS_VELOCITY_STD_MPS; + navigation.velocity_covariance_enu_mps2[4] = + GPS_VELOCITY_STD_MPS * GPS_VELOCITY_STD_MPS; + navigation.velocity_covariance_enu_mps2[8] = + 4.0 * GPS_VELOCITY_STD_MPS * GPS_VELOCITY_STD_MPS; + navigation.velocity_valid = true; + return navigation; +} + +typedef struct { + double outage_max_error_m; + double error_at_outage_end_m; + double error_after_recovery_m; + double max_state_step_m; + double sigma_at_outage_end_m; + double heading_error_deg; + double truth_path_length_m; + double drift_sigma_ratio; + double max_cruise_speed_error_mps; + double recovery_seconds; + uint64_t zero_updates_during_cruise; + uint64_t false_rest_samples; + uint64_t gps_accepted; + uint64_t gps_rejected; + uint64_t recoveries; + bool finite; + bool recovered; + bool reached_running; +} metrics_t; + +static metrics_t run_case(profile_kind_t profile, double outage_s) +{ + static fusion_engine_t engine; + metrics_t metrics; + memset(&metrics, 0, sizeof(metrics)); + metrics.finite = true; + + fusion_engine_config_t config; + fusion_imu_quality_profile_t quality; + fusion_engine_default_config(&config, TIMEBASE_HZ); + if(!imu_fusion_quality_profile_for_backend( + IMU_SENSOR_TDK_ICM42688, &quality) || + !fusion_engine_apply_imu_quality(&config, &quality)) + { + return metrics; + } + memset(&engine, 0, sizeof(engine)); + if(!fusion_engine_init(&engine, &config)) + { + return metrics; + } + + random_state = 0x51ed270bu ^ ((uint32_t)profile << 16) ^ + (uint32_t)(outage_s); + /* The same supervisor navigation_fusion.c feeds the engine, so whatever it + * concludes about a smoothly cruising vehicle is what the engine sees. */ + fusion_rest_detector_t rest_detector; + fusion_rest_detector_config_t rest_config; + fusion_rest_detector_default_config(&rest_config); + if(!fusion_rest_detector_init(&rest_detector, &rest_config, TIMEBASE_HZ, true)) + { + return metrics; + } + + truth_t truth; + memset(&truth, 0, sizeof(truth)); + + double outage_end_s = OUTAGE_START_S + outage_s; + double duration_s = outage_end_s + RECOVERY_OBSERVE_S; + uint64_t imu_period_ticks = TIMEBASE_HZ / IMU_HZ; + uint64_t gps_period_ticks = (uint64_t)GPS_PERIOD_MS * 1000u; + uint64_t end_ticks = (uint64_t)(duration_s * (double)TIMEBASE_HZ); + uint64_t next_imu_ticks = imu_period_ticks; + uint64_t next_gps_ticks = gps_period_ticks; + uint64_t last_ticks = 0u; + unsigned int sequence = 0u; + + double previous_state[3] = {0.0, 0.0, 0.0}; + bool have_previous_state = false; + double recovery_started_s = -1.0; + uint64_t zero_updates_at_cruise_start = 0u; + bool cruise_started = false; + + while(true) + { + uint64_t ticks = (next_imu_ticks < next_gps_ticks) + ? next_imu_ticks : next_gps_ticks; + if(ticks > end_ticks) + { + break; + } + double start_s = (double)last_ticks / (double)TIMEBASE_HZ; + double dt_s = (double)(ticks - last_ticks) / (double)TIMEBASE_HZ; + propagate_truth(&truth, profile, start_s, dt_s); + last_ticks = ticks; + double time_s = (double)ticks / (double)TIMEBASE_HZ; + bool in_outage = (time_s >= OUTAGE_START_S) && (time_s < outage_end_s); + + if(ticks == next_gps_ticks) + { + if(!in_outage) + { + fusion_navigation_measurement_t navigation = + make_navigation(&truth, ticks, + profile_gps_horizontal_std_m(profile)); + (void)fusion_engine_push_navigation( + &engine, &navigation, NULL, NULL, NULL, NULL); + } + next_gps_ticks += gps_period_ticks; + } + + if(ticks == next_imu_ticks) + { + fusion_imu_measurement_t imu = make_imu( + profile, &truth, time_s, ticks, config.eskf.gravity_mps2, + sequence++); + imu_sample_t sample; + memset(&sample, 0, sizeof(sample)); + sample.timestamp_ticks = imu.timestamp_ticks; + memcpy(sample.accel_mps2, imu.accel_mps2, sizeof(sample.accel_mps2)); + memcpy(sample.gyro_rps, imu.gyro_rps, sizeof(sample.gyro_rps)); + sample.temperature_c = imu.temperature_c; + sample.sequence = imu.sequence; + (void)fusion_rest_detector_update(&rest_detector, &sample); + imu.vehicle_at_rest = rest_detector.at_rest; + if(rest_detector.at_rest && (truth.speed_mps > 1.0)) + { + metrics.false_rest_samples++; + } + (void)fusion_engine_push_imu(&engine, &imu); + next_imu_ticks += imu_period_ticks; + } + + if(engine.state != FUSION_ENGINE_RUNNING) + { + continue; + } + metrics.reached_running = true; + + fusion_runtime_t runtime; + if(!fusion_engine_get_runtime(&engine, &runtime)) + { + continue; + } + const double* state = runtime.filter.position_m; + for(unsigned int axis = 0u ; axis < 3u ; axis++) + { + if(!isfinite(state[axis])) + { + metrics.finite = false; + } + } + if(!metrics.finite) + { + break; + } + + double error = hypot(state[0] - truth.position_enu_m[0], + state[1] - truth.position_enu_m[1]); + if(have_previous_state) + { + double step = hypot(state[0] - previous_state[0], + state[1] - previous_state[1]); + /* Re-anchoring after an outage is a deliberate discontinuity; + * only the free-running interval must stay smooth. */ + if(in_outage && (step > metrics.max_state_step_m)) + { + metrics.max_state_step_m = step; + } + } + memcpy(previous_state, state, sizeof(previous_state)); + have_previous_state = true; + + if(in_outage) + { + metrics.truth_path_length_m += truth.speed_mps * dt_s; + if(error > metrics.outage_max_error_m) + { + metrics.outage_max_error_m = error; + } + metrics.error_at_outage_end_m = error; + /* + * Horizontal one-sigma from the filter's own error covariance + * (position occupies error states 0..2). Judging drift against + * this rather than against a fixed distance is what distinguishes + * honest dead reckoning from divergence: a filter that knows it is + * uncertain is behaving; one whose error outruns its covariance is + * not. The absolute drift is dominated by whatever vibration and + * bias the platform actually has, so no fixed metre bound + * generalises across profiles. + */ + const double* q = runtime.filter.body_to_nav_quaternion; + double yaw = atan2(2.0 * (q[0] * q[3] + q[1] * q[2]), + 1.0 - 2.0 * (q[2] * q[2] + q[3] * q[3])); + double heading_error = yaw - truth.yaw_rad; + while(heading_error > PI) { heading_error -= 2.0 * PI; } + while(heading_error < -PI) { heading_error += 2.0 * PI; } + metrics.heading_error_deg = fabs(heading_error) * 180.0 / PI; + double sigma = sqrt(fmax(0.0, runtime.filter.covariance[0][0]) + + fmax(0.0, runtime.filter.covariance[1][1])); + metrics.sigma_at_outage_end_m = sigma; + metrics.drift_sigma_ratio = (sigma > 1.0e-6) ? (error / sigma) : 0.0; + } + else if(time_s >= outage_end_s) + { + if(recovery_started_s < 0.0) + { + recovery_started_s = time_s; + } + if(!metrics.recovered && (error <= 15.0)) + { + metrics.recovered = true; + metrics.recovery_seconds = time_s - recovery_started_s; + } + metrics.error_after_recovery_m = error; + } + + /* Cruise: constant velocity with GNSS present, the regime that used to + * collapse into a false zero-velocity update. The zero-update counter + * is sampled across this window only; zero-velocity updates during the + * genuinely stationary opening phase are correct and must not be + * charged against it. */ + if(!(truth.constant_velocity && (truth.speed_mps > 1.0) && !in_outage)) + { + cruise_started = false; + } + /* Skip the re-convergence transient right after GNSS returns; that + * interval is judged by the recovery checks instead. */ + if(truth.constant_velocity && (truth.speed_mps > 1.0) && !in_outage && + ((time_s < OUTAGE_START_S) || (time_s > outage_end_s + 10.0))) + { + double speed_error = + fabs(eskf_speed_mps(&runtime.filter) - truth.speed_mps); + if(speed_error > metrics.max_cruise_speed_error_mps) + { + metrics.max_cruise_speed_error_mps = speed_error; + } + uint64_t zero_updates = + fusion_engine_diagnostics(&engine)->zero_velocity_update_count; + if(cruise_started) + { + metrics.zero_updates_during_cruise += + zero_updates - zero_updates_at_cruise_start; + } + cruise_started = true; + zero_updates_at_cruise_start = zero_updates; + } + } + + const fusion_engine_diagnostics_t* diagnostics = + fusion_engine_diagnostics(&engine); + metrics.gps_accepted = diagnostics->position_accepted; + metrics.gps_rejected = diagnostics->position_nis_rejected; + metrics.recoveries = diagnostics->position_recovery_count; + return metrics; +} + +static void report(profile_kind_t profile, double outage_s, + const metrics_t* metrics) +{ + printf("%-11s outage=%3.0fs drift_max=%8.1fm drift_end=%8.1fm " + "recovered=%s in %4.1fs (err=%6.2fm) cruise_speed_err=%5.2fm/s " + "sigma_end=%8.1fm err/sigma=%4.2f hdg_err=%5.2fdeg path=%7.0fm " + "zupt=%lu gps=%lu/%lu recov=%lu step_max=%.2fm false_rest=%lu " + "finite=%d\n", + profile_name(profile), outage_s, + metrics->outage_max_error_m, metrics->error_at_outage_end_m, + metrics->recovered ? "yes" : "NO ", metrics->recovery_seconds, + metrics->error_after_recovery_m, + metrics->max_cruise_speed_error_mps, + metrics->sigma_at_outage_end_m, + metrics->drift_sigma_ratio, + metrics->heading_error_deg, + metrics->truth_path_length_m, + (unsigned long)metrics->zero_updates_during_cruise, + (unsigned long)metrics->gps_accepted, + (unsigned long)metrics->gps_rejected, + (unsigned long)metrics->recoveries, + metrics->max_state_step_m, + (unsigned long)metrics->false_rest_samples, + metrics->finite ? 1 : 0); +} + +int main(void) +{ + const profile_kind_t profiles[] = { + PROFILE_PEDESTRIAN, PROFILE_GENTLE_CAR, PROFILE_SLOW_CITY, + PROFILE_ISOLATED_CITY, PROFILE_URBAN_TURNS}; + const double outages_s[] = {60.0, 300.0}; + + printf("Pedestrian and gentle-vehicle trajectories across GNSS outages\n"); + printf("IMU 50 Hz, GNSS 5 Hz, bench-measured ICM-42688-P noise\n"); + + for(unsigned int p = 0u ; p < PROFILE_COUNT ; p++) + { + for(unsigned int o = 0u ; o < 2u ; o++) + { + profile_kind_t profile = profiles[p]; + double outage_s = outages_s[o]; + metrics_t metrics = run_case(profile, outage_s); + report(profile, outage_s, &metrics); + + char message[192]; + + snprintf(message, sizeof(message), + "%s/%.0fs: engine must finish calibration", + profile_name(profile), outage_s); + CHECK(metrics.reached_running, message); + + snprintf(message, sizeof(message), + "%s/%.0fs: every published state must stay finite", + profile_name(profile), outage_s); + CHECK(metrics.finite, message); + + /* + * Dead reckoning must stay smooth. A step larger than a few + * metres between consecutive IMU samples means the filter jumped, + * not that it drifted. + */ + snprintf(message, sizeof(message), + "%s/%.0fs: no position discontinuity while free-running", + profile_name(profile), outage_s); + CHECK(metrics.max_state_step_m < 5.0, message); + + /* + * Drift must stay consistent with the filter's own uncertainty. + * Five minutes of inertial-only travel legitimately reaches + * kilometres on this hardware; what must never happen is the error + * outrunning the covariance, which is the signature of divergence + * rather than of honest coasting. + */ + snprintf(message, sizeof(message), + "%s/%.0fs: drift must stay within 5 sigma of the filter's " + "own covariance", profile_name(profile), outage_s); + CHECK(metrics.drift_sigma_ratio < 5.0, message); + + /* The covariance must actually grow while unaided, otherwise the + * filter would stay overconfident and reject the returning fixes -- + * the innovation-gate lockout seen in the field. */ + snprintf(message, sizeof(message), + "%s/%.0fs: covariance must grow while unaided", + profile_name(profile), outage_s); + CHECK(metrics.sigma_at_outage_end_m > 10.0, message); + + /* + * Heading is integrated from a directly measured rate, so course + * changes must survive an unaided minute far better than position + * does. This is what makes the dead-reckoned path the right shape + * even once it has drifted a long way from the truth. + */ + if(outage_s <= 60.0) + { + /* + * A walker is the hard case: gait swings the sensor through + * large angles at frequencies a 50 Hz strapdown integration + * resolves poorly, so heading degrades several times faster + * than on a vehicle. Pedestrian navigation is better served + * by step detection than by strapdown integration, and this + * looser bound records that rather than hiding it. + */ + double heading_limit_deg = + (profile == PROFILE_PEDESTRIAN) ? 10.0 : 5.0; + snprintf(message, sizeof(message), + "%s/%.0fs: heading must survive the outage", + profile_name(profile), outage_s); + CHECK(metrics.heading_error_deg < heading_limit_deg, message); + } + + /* The lockout regression: GNSS must be able to re-anchor. */ + snprintf(message, sizeof(message), + "%s/%.0fs: must re-converge after GNSS returns", + profile_name(profile), outage_s); + CHECK(metrics.recovered, message); + + snprintf(message, sizeof(message), + "%s/%.0fs: re-convergence must take under 30 s", + profile_name(profile), outage_s); + CHECK(metrics.recovery_seconds < 30.0, message); + + snprintf(message, sizeof(message), + "%s/%.0fs: must hold position after re-converging", + profile_name(profile), outage_s); + CHECK(metrics.error_after_recovery_m < 25.0, message); + + /* + * The false-rest regression. With GNSS present, a filter that + * accepted the bogus at-rest flag would pin speed near zero while + * the vehicle cruised at 27.8 m/s. + */ + snprintf(message, sizeof(message), + "%s/%.0fs: cruise speed must track truth, not collapse", + profile_name(profile), outage_s); + CHECK(metrics.max_cruise_speed_error_mps < 3.0, message); + + snprintf(message, sizeof(message), + "%s/%.0fs: no zero-velocity update during aided cruise", + profile_name(profile), outage_s); + CHECK(metrics.zero_updates_during_cruise == 0u, message); + + if(profile == PROFILE_ISOLATED_CITY) + { + /* Precondition: without a genuinely fooled supervisor this + * case would prove nothing, so assert the trap is armed. */ + snprintf(message, sizeof(message), + "%s/%.0fs: rest supervisor must actually be fooled " + "for this case to mean anything", + profile_name(profile), outage_s); + CHECK(metrics.false_rest_samples > 100u, message); + } + } + } + + if(failures != 0) + { + fprintf(stderr, "FAIL: %d trajectory check(s) failed\n", failures); + return 1; + } + printf("PASS: pedestrian and gentle-vehicle trajectories stayed bounded " + "through 1 and 5 minute outages and re-converged on GNSS return.\n"); + return 0; +} diff --git a/soc/software/tdk_noise_test/Makefile b/soc/software/tdk_noise_test/Makefile new file mode 100644 index 0000000..0ef4088 --- /dev/null +++ b/soc/software/tdk_noise_test/Makefile @@ -0,0 +1,39 @@ +TARGET := tdk_noise_test + +GPS_SYSTEM_DIR ?= ../gps_system + +# The production driver is compiled in directly so the sensor is configured +# exactly as the flight build configures it. +vpath %.c $(GPS_SYSTEM_DIR)/src/drivers/imu + +APP_SRCS := \ + main.c \ + tdk_icm42688.c + +BSP_SRCS := init.c uart.c plic.c irq.c pmod_spi.c + +# Genesys2 fabric/UART and CLINT defaults, matching the gps_system app. +BSP_UART_CLOCK_HZ ?= 50000000 +BSP_UART_BAUDRATE ?= 460800 +BSP_CLINT_TIMEBASE_HZ ?= 500000 + +# Mirrors gps_system IMU_ODR_HZ; override together with it. +NOISE_ODR_HZ ?= 50 +NOISE_DISCARD_SAMPLES ?= 100 +NOISE_BIAS_SAMPLES ?= 1000 +NOISE_MEASURE_SAMPLES ?= 3000 +# Set to 1 to refuse the polled fallback, so a broken INT1 route fails loudly. +NOISE_REQUIRE_IRQ ?= 0 +# Set to 1 to run only the INT1 -> PLIC continuity test and exit. +NOISE_PIN_TEST ?= 0 + +include ../common/app.mk + +CFLAGS += -I$(GPS_SYSTEM_DIR)/header +CFLAGS += -DNOISE_ODR_HZ=$(NOISE_ODR_HZ)u \ + -DNOISE_DISCARD_SAMPLES=$(NOISE_DISCARD_SAMPLES)u \ + -DNOISE_BIAS_SAMPLES=$(NOISE_BIAS_SAMPLES)u \ + -DNOISE_MEASURE_SAMPLES=$(NOISE_MEASURE_SAMPLES)u \ + -DNOISE_REQUIRE_IRQ=$(NOISE_REQUIRE_IRQ) \ + -DNOISE_PIN_TEST=$(NOISE_PIN_TEST) +LDLIBS += -lm diff --git a/soc/software/tdk_noise_test/main.c b/soc/software/tdk_noise_test/main.c new file mode 100644 index 0000000..780807c --- /dev/null +++ b/soc/software/tdk_noise_test/main.c @@ -0,0 +1,805 @@ +/* + * 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; +}