gps/soc/software/fusion/tests/calibration_offset_test.c

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

#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;
}