diff options
Diffstat (limited to 'test/gyro_cal.c')
| -rw-r--r-- | test/gyro_cal.c | 511 |
1 files changed, 0 insertions, 511 deletions
diff --git a/test/gyro_cal.c b/test/gyro_cal.c deleted file mode 100644 index 10d48ca18e..0000000000 --- a/test/gyro_cal.c +++ /dev/null @@ -1,511 +0,0 @@ -/* Copyright 2020 The Chromium OS Authors. All rights reserved. - * Use of this source code is governed by a BSD-style license that can be - * found in the LICENSE file. - */ - -#include "common.h" -#include "gyro_cal.h" -#include "gyro_still_det.h" -#include "gyro_cal_init_for_test.h" -#include "motion_sense.h" -#include "test_util.h" -#include <string.h> -#include <stdlib.h> -#include <math.h> -#include <stdio.h> - -float kToleranceGyroRps = 1e-6f; -float kDefaultGravityMps2 = 9.81f; -int kDefaultTemperatureKelvin = 298; - -#define NANOS_TO_SEC (1.0e-9f) -#define NANO_PI (3.14159265359f) -/** Unit conversion: milli-degrees to radians. */ -#define MDEG_TO_RAD (NANO_PI / 180.0e3f) - -#define MSEC_TO_NANOS(x) ((uint64_t)((x) * (uint64_t)(1000000))) -#define SEC_TO_NANOS(x) MSEC_TO_NANOS((x) * (uint64_t)(1000)) -#define HZ_TO_PERIOD_NANOS(hz) (SEC_TO_NANOS(1024) / ((uint64_t)((hz)*1024))) - -struct motion_sensor_t motion_sensors[] = { - [BASE] = {}, - [LID] = {}, -}; - -const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); - -/** - * This function will return a uniformly distributed random value in the range - * of (0,1). This is important that 0 and 1 are excluded because of how the - * value is used in normal_random. For references: - * - rand() / RAND_MAX yields the range [0,1] - * - rand() / (RAND_MAX + 1) yields the range [0,1) - * - (rand() + 1) / (RAND_MAX + 1) yields the range (0, 1) - * - * @return A uniformly distributed random value. - */ -static double rand_gen(void) -{ - return ((double)(rand()) + 1.0) / ((double)(RAND_MAX) + 1.0); -} - -/** - * @return A normally distributed random value - */ -static float normal_random(void) -{ - double v1 = rand_gen(); - double v2 = rand_gen(); - - return (float)(cos(2 * 3.14 * v2) * sqrt(-2.0 * log(v1))); -} - -/** - * @param mean The mean to use for the normal distribution. - * @param stddev The standard deviation to use for the normal distribution. - * @return A normally distributed random value based on mean and stddev. - */ -static float normal_random2(float mean, float stddev) -{ - return normal_random() * stddev + mean; -} - -/** - * Tests that a calibration is updated after a period where the IMU device is - * stationary. Accelerometer and gyroscope measurements are simulated with data - * sheet specs for the BMI160 at their respective noise floors. A magnetometer - * sensor is also included in this test. - * - * @return EC_SUCCESS on success. - */ -static int test_gyro_cal_calibration(void) -{ - int i; - struct gyro_cal gyro_cal; - - /* - * Statistics for simulated gyroscope data. - * RMS noise = 70mDPS, offset = 150mDPS. - */ - /* [Hz] */ - const float sample_rate = 400.0f; - /* [rad/sec] */ - const float gyro_bias = MDEG_TO_RAD * 150.0f; - /* [rad/sec] */ - const float gyro_rms_noise = MDEG_TO_RAD * 70.0f; - const uint64_t sample_interval_nanos = HZ_TO_PERIOD_NANOS(sample_rate); - - /* - * Statistics for simulated accelerometer data. - * noise density = 200ug/rtHz, offset = 50mg. - */ - /* [m/sec^2] */ - const float accel_bias = 0.05f * kDefaultGravityMps2; - /* [m/sec^2] */ - const float accel_rms_noise = - 0.0002f * kDefaultGravityMps2 * fp_sqrtf(0.5f * sample_rate); - - /* - * Statistics for simulated magnetometer data. - * RMS noise = 0.4 micro Tesla (uT), offset = 0.2uT. - */ - const float mag_bias = 0.2f; /* [uT] */ - const float mag_rms_noise = 0.4f; /* [uT] */ - - float bias[3]; - float bias_residual[3]; - int temperature_kelvin; - uint32_t calibration_time_us = 0; - - bool calibration_received = false; - - gyro_cal_initialization_for_test(&gyro_cal); - - /* No calibration should be available yet. */ - TEST_EQ(gyro_cal_new_bias_available(&gyro_cal), false, "%d"); - - /* - * Simulate up to 20 seconds of sensor data (zero mean, additive white - * Gaussian noise). - */ - for (i = 0; i < (int)(20.0f * sample_rate); ++i) { - const uint32_t timestamp_us = - (i * sample_interval_nanos) / 1000; - - /* Generate and add an accelerometer sample. */ - gyro_cal_update_accel( - &gyro_cal, timestamp_us, - normal_random2(accel_bias, accel_rms_noise), - normal_random2(accel_bias, accel_rms_noise), - normal_random2(accel_bias, accel_rms_noise)); - - /* Generate and add a gyroscrope sample. */ - gyro_cal_update_gyro(&gyro_cal, timestamp_us, - normal_random2(gyro_bias, gyro_rms_noise), - normal_random2(gyro_bias, gyro_rms_noise), - normal_random2(gyro_bias, gyro_rms_noise), - kDefaultTemperatureKelvin); - - /* - * The simulated magnetometer here has a sampling rate that is - * 4x slower than the accel/gyro - */ - if (i % 4 == 0) { - gyro_cal_update_mag( - &gyro_cal, timestamp_us, - normal_random2(mag_bias, mag_rms_noise), - normal_random2(mag_bias, mag_rms_noise), - normal_random2(mag_bias, mag_rms_noise)); - } - calibration_received = gyro_cal_new_bias_available(&gyro_cal); - if (calibration_received) - break; - } - - TEST_EQ(calibration_received, true, "%d"); - - gyro_cal_get_bias(&gyro_cal, bias, &temperature_kelvin, - &calibration_time_us); - bias_residual[0] = gyro_bias - bias[0]; - bias_residual[1] = gyro_bias - bias[1]; - bias_residual[2] = gyro_bias - bias[2]; - - /* - * Make sure that the bias estimate is within 20 milli-degrees per - * second. - */ - TEST_LT(bias_residual[0], 20.f * MDEG_TO_RAD, "%f"); - TEST_LT(bias_residual[1], 20.f * MDEG_TO_RAD, "%f"); - TEST_LT(bias_residual[2], 20.f * MDEG_TO_RAD, "%f"); - - TEST_NEAR(gyro_cal.stillness_confidence, 1.0f, 0.0001f, "%f"); - - TEST_EQ(temperature_kelvin, kDefaultTemperatureKelvin, "%d"); - - return EC_SUCCESS; -} - -/** - * Tests that calibration does not falsely occur for low-level motion. - * - * @return EC_SUCCESS on success. - */ -static int test_gyro_cal_no_calibration(void) -{ - int i; - struct gyro_cal gyro_cal; - - /* Statistics for simulated gyroscope data. */ - /* RMS noise = 70mDPS, offset = 150mDPS. */ - const float sample_rate = 400.0f; /* [Hz] */ - const float gyro_bias = MDEG_TO_RAD * 150.0f; /* [rad/sec] */ - const float gyro_rms_noise = MDEG_TO_RAD * 70.0f; /* [rad/sec] */ - const uint64_t sample_interval_nanos = HZ_TO_PERIOD_NANOS(sample_rate); - - /* Statistics for simulated accelerometer data. */ - /* noise density = 200ug/rtHz, offset = 50mg. */ - /* [m/sec^2] */ - const float accel_bias = 0.05f * kDefaultGravityMps2; - /* [m/sec^2] */ - const float accel_rms_noise = - 200.0e-6f * kDefaultGravityMps2 * fp_sqrtf(0.5f * sample_rate); - - /* Define sinusoidal gyroscope motion parameters. */ - const float omega_dt = - 2.0f * NANO_PI * sample_interval_nanos * NANOS_TO_SEC; - const float amplitude = MDEG_TO_RAD * 550.0f; /* [rad/sec] */ - - bool calibration_received = false; - - gyro_cal_initialization_for_test(&gyro_cal); - - for (i = 0; i < (int)(20.0f * sample_rate); ++i) { - const uint32_t timestamp_us = - (i * sample_interval_nanos) / 1000; - - /* Generate and add an accelerometer sample. */ - gyro_cal_update_accel( - &gyro_cal, timestamp_us, - normal_random2(accel_bias, accel_rms_noise), - normal_random2(accel_bias, accel_rms_noise), - normal_random2(accel_bias, accel_rms_noise)); - - /* Generate and add a gyroscope sample. */ - gyro_cal_update_gyro( - &gyro_cal, timestamp_us, - normal_random2(gyro_bias, gyro_rms_noise) + - amplitude * sin(2.0f * omega_dt * i), - normal_random2(gyro_bias, gyro_rms_noise) - - amplitude * sin(2.1f * omega_dt * i), - normal_random2(gyro_bias, gyro_rms_noise) + - amplitude * cos(4.3f * omega_dt * i), - kDefaultTemperatureKelvin); - - /* Check for calibration update. Break after first one. */ - calibration_received = gyro_cal_new_bias_available(&gyro_cal); - if (calibration_received) - break; - } - - /* Determine that NO calibration had occurred. */ - TEST_EQ(calibration_received, false, "%d"); - - /* Make sure that the device was NOT classified as "still". */ - TEST_GT(1.0f, gyro_cal.stillness_confidence, "%f"); - - return EC_SUCCESS; -} - -/** - * Tests that a shift in a stillness window mean does not trigger a calibration. - * - * @return EC_SUCCESS on success. - */ -static int test_gyro_cal_win_mean_shift(void) -{ - struct gyro_cal gyro_cal; - int i; - - /* Statistics for simulated gyroscope data. */ - const float sample_rate = 400.0f; /* [Hz] */ - const float gyro_bias = MDEG_TO_RAD * 150.0f; /* [rad/sec] */ - const float gyro_bias_shift = MDEG_TO_RAD * 60.0f; /* [rad/sec] */ - const uint64_t sample_interval_nanos = HZ_TO_PERIOD_NANOS(sample_rate); - - /* Initialize the gyro calibration. */ - gyro_cal_initialization_for_test(&gyro_cal); - - /* - * Simulates 8 seconds of sensor data (no noise, just a gyro mean shift - * after 4 seconds). - * Assumptions: The max stillness period is 6 seconds, and the mean - * delta limit is 50mDPS. The mean shift should be detected and exceed - * the 50mDPS limit, and no calibration should be triggered. NOTE: This - * step is not large enough to trip the variance checking within the - * stillness detectors. - */ - for (i = 0; i < (int)(8.0f * sample_rate); i++) { - const uint32_t timestamp_us = - (i * sample_interval_nanos) / 1000; - - /* Generate and add a accelerometer sample. */ - gyro_cal_update_accel(&gyro_cal, timestamp_us, 0.0f, 0.0f, - 9.81f); - - /* Generate and add a gyroscope sample. */ - if (timestamp_us > 4 * SECOND) { - gyro_cal_update_gyro(&gyro_cal, timestamp_us, - gyro_bias + gyro_bias_shift, - gyro_bias + gyro_bias_shift, - gyro_bias + gyro_bias_shift, - kDefaultTemperatureKelvin); - } else { - gyro_cal_update_gyro(&gyro_cal, timestamp_us, gyro_bias, - gyro_bias, gyro_bias, - kDefaultTemperatureKelvin); - } - } - - /* Determine that NO calibration had occurred. */ - TEST_EQ(gyro_cal_new_bias_available(&gyro_cal), false, "%d"); - - return EC_SUCCESS; -} - -/** - * Tests that a temperature variation outside the acceptable range prevents a - * calibration. - * - * @return EC_SUCCESS on success. - */ -static int test_gyro_cal_temperature_shift(void) -{ - int i; - struct gyro_cal gyro_cal; - - /* Statistics for simulated gyroscope data. */ - const float sample_rate = 400.0f; /* [Hz] */ - const float gyro_bias = MDEG_TO_RAD * 150.0f; /* [rad/sec] */ - const float temperature_shift_kelvin = 2.6f; - const uint64_t sample_interval_nanos = HZ_TO_PERIOD_NANOS(sample_rate); - - gyro_cal_initialization_for_test(&gyro_cal); - - /* - * Simulates 8 seconds of sensor data (no noise, just a temperature - * shift after 4 seconds). - * Assumptions: The max stillness period is 6 seconds, and the - * temperature delta limit is 1.5C. The shift should be detected and - * exceed the limit, and no calibration should be triggered. - */ - for (i = 0; i < (int)(8.0f * sample_rate); i++) { - const uint32_t timestamp_us = - (i * sample_interval_nanos) / 1000; - float temperature_kelvin = kDefaultTemperatureKelvin; - - /* Generate and add a accelerometer sample. */ - gyro_cal_update_accel(&gyro_cal, timestamp_us, 0.0f, 0.0f, - 9.81f); - - /* Sets the temperature value. */ - if (timestamp_us > 4 * SECOND) - temperature_kelvin += temperature_shift_kelvin; - - /* Generate and add a gyroscope sample. */ - gyro_cal_update_gyro(&gyro_cal, timestamp_us, gyro_bias, - gyro_bias, gyro_bias, - (int)temperature_kelvin); - } - - /* Determine that NO calibration had occurred. */ - TEST_EQ(gyro_cal_new_bias_available(&gyro_cal), false, "%d"); - - return EC_SUCCESS; -} - -/** - * Verifies that complete sensor stillness results in the correct bias estimate - * and produces the correct timestamp. - * - * @return EC_SUCCESS on success; - */ -static int test_gyro_cal_stillness_timestamp(void) -{ - struct gyro_cal gyro_cal; - int64_t time_us; - - /* - * 10Hz update rate for 11 seconds should trigger the in-situ - * algorithms. - */ - const float gyro_bias_x = 0.09f; - const float gyro_bias_y = -0.04f; - const float gyro_bias_z = 0.05f; - - float bias[3]; - int temperature_kelvin = 273; - uint32_t calibration_time_us = 0; - - gyro_cal_initialization_for_test(&gyro_cal); - for (time_us = 0; time_us < 11 * SECOND; time_us += 100 * MSEC) { - /* Generate and add a accelerometer sample. */ - gyro_cal_update_accel(&gyro_cal, time_us, 0.0f, 0.0f, 9.81f); - - /* Generate and add a gyroscope sample. */ - gyro_cal_update_gyro(&gyro_cal, time_us, gyro_bias_x, - gyro_bias_y, gyro_bias_z, - kDefaultTemperatureKelvin); - } - - /* Determine if there is a new calibration. Get the calibration value. - */ - TEST_EQ(gyro_cal_new_bias_available(&gyro_cal), 1, "%d"); - - gyro_cal_get_bias(&gyro_cal, bias, &temperature_kelvin, - &calibration_time_us); - - /* Make sure that the bias estimate is within kToleranceGyroRps. */ - TEST_NEAR(gyro_bias_x - bias[0], 0.0f, 0.0001f, "%f"); - TEST_NEAR(gyro_bias_y - bias[1], 0.0f, 0.0001f, "%f"); - TEST_NEAR(gyro_bias_z - bias[2], 0.0f, 0.0001f, "%f"); - - /* Checks that the calibration occurred at the expected time. */ - TEST_EQ(6 * SECOND, gyro_cal.calibration_time_us, "%u"); - - /* Make sure that the device was classified as 100% "still". */ - TEST_NEAR(1.0f, gyro_cal.stillness_confidence, 0.0001f, "%f"); - - /* Make sure that the calibration temperature is correct. */ - TEST_EQ(kDefaultTemperatureKelvin, temperature_kelvin, "%d"); - - return EC_SUCCESS; -} - -/** - * Verifies that setting an initial bias works. - * - * @return EC_SUCCESS on success. - */ -static int test_gyro_cal_set_bias(void) -{ - struct gyro_cal gyro_cal; - - /* Get the initialized bias value; should be zero. */ - float bias[3] = { 0.0f, 0.0f, 0.0f }; - int temperature_kelvin = 273; - uint32_t calibration_time_us = 10; - - /* Initialize the gyro calibration. */ - gyro_cal_initialization_for_test(&gyro_cal); - gyro_cal_get_bias(&gyro_cal, bias, &temperature_kelvin, - &calibration_time_us); - TEST_NEAR(0.0f, bias[0], 0.0001f, "%f"); - TEST_NEAR(0.0f, bias[1], 0.0001f, "%f"); - TEST_NEAR(0.0f, bias[2], 0.0001f, "%f"); - TEST_EQ(0, temperature_kelvin, "%d"); - TEST_EQ(0, calibration_time_us, "%d"); - - /* Set the calibration bias estimate. */ - bias[0] = 1.0f; - bias[1] = 2.0f; - bias[2] = 3.0f; - gyro_cal_set_bias(&gyro_cal, bias, 31, 3 * 60 * SECOND); - - bias[0] = 0.0f; - bias[1] = 0.0f; - bias[2] = 0.0f; - /* Check that it was set correctly. */ - gyro_cal_get_bias(&gyro_cal, bias, &temperature_kelvin, - &calibration_time_us); - TEST_NEAR(1.0f, bias[0], 0.0001f, "%f"); - TEST_NEAR(2.0f, bias[1], 0.0001f, "%f"); - TEST_NEAR(3.0f, bias[2], 0.0001f, "%f"); - TEST_EQ(31, temperature_kelvin, "%d"); - TEST_EQ(3 * 60 * SECOND, calibration_time_us, "%u"); - - return EC_SUCCESS; -} - -/** - * Verifies that the gyroCalRemoveBias function works as intended. - * - * @return EC_SUCCESS on success - */ -static int test_gyro_cal_remove_bias(void) -{ - struct gyro_cal gyro_cal; - float bias[3] = { 1.0f, 2.0f, 3.0f }; - float bias_out[3]; - - /* Initialize the gyro calibration. */ - gyro_cal_initialization_for_test(&gyro_cal); - - /* Set an calibration bias estimate. */ - gyro_cal_set_bias(&gyro_cal, bias, kDefaultTemperatureKelvin, - 5 * 60 * SECOND); - - /* Correct the bias, and check that it has been adequately removed. */ - gyro_cal_remove_bias(&gyro_cal, bias, bias_out); - - /* Make sure that the bias estimate is within kToleranceGyroRps. */ - TEST_NEAR(0.0f, bias_out[0], 0.0001f, "%f"); - TEST_NEAR(0.0f, bias_out[1], 0.0001f, "%f"); - TEST_NEAR(0.0f, bias_out[2], 0.0001f, "%f"); - - return EC_SUCCESS; -} - -void run_test(int argc, char **argv) -{ - test_reset(); - - RUN_TEST(test_gyro_cal_calibration); - RUN_TEST(test_gyro_cal_no_calibration); - RUN_TEST(test_gyro_cal_win_mean_shift); - RUN_TEST(test_gyro_cal_temperature_shift); - RUN_TEST(test_gyro_cal_stillness_timestamp); - RUN_TEST(test_gyro_cal_set_bias); - RUN_TEST(test_gyro_cal_remove_bias); - - test_print_result(); -} |