/* * 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; }