diff options
-rw-r--r-- | common/online_calibration.c | 136 | ||||
-rw-r--r-- | include/config.h | 13 | ||||
-rw-r--r-- | test/build.mk | 4 | ||||
-rw-r--r-- | test/gyro_cal.c | 102 | ||||
-rw-r--r-- | test/gyro_cal_init_for_test.c | 114 | ||||
-rw-r--r-- | test/gyro_cal_init_for_test.h | 39 | ||||
-rw-r--r-- | test/online_calibration_spoof.c | 196 | ||||
-rw-r--r-- | test/online_calibration_spoof.tasklist | 10 | ||||
-rw-r--r-- | test/test_config.h | 8 |
9 files changed, 476 insertions, 146 deletions
diff --git a/common/online_calibration.c b/common/online_calibration.c index f026f1adf7..3bc56f85c7 100644 --- a/common/online_calibration.c +++ b/common/online_calibration.c @@ -83,11 +83,10 @@ static void data_fp_to_int16(const struct motion_sensor_t *s, const fpv3_t data, int32_t iv; fp_t v = fp_div(data[i], range); - v = fp_mul(v, INT_TO_FP((data[i] >= INT_TO_FP(0)) ? 0x7fff : - 0x8000)); + v = fp_mul(v, INT_TO_FP(0x7fff)); iv = FP_TO_INT(v); /* Check for overflow */ - out[i] = CLAMP(iv, (int32_t)0xffff8000, (int32_t)0x00007fff); + out[i] = ec_motion_sensor_clamp_i16(iv); } } @@ -98,28 +97,36 @@ static void data_fp_to_int16(const struct motion_sensor_t *s, const fpv3_t data, * * @param sensor Pointer to the gyroscope sensor to check. */ -static void check_gyro_cal_new_bias(struct motion_sensor_t *sensor) +static bool check_gyro_cal_new_bias(struct motion_sensor_t *sensor, + fpv3_t bias_out) { struct online_calib_data *calib_data = (struct online_calib_data *)sensor->online_calib_data; struct gyro_cal_data *data = (struct gyro_cal_data *)calib_data->type_specific_data; - int sensor_num = sensor - motion_sensors; int temp_out; - fpv3_t bias_out; uint32_t timestamp_out; /* Check that we have a new bias. */ if (data == NULL || calib_data == NULL || !gyro_cal_new_bias_available(&data->gyro_cal)) - return; + return false; /* Read the calibration values. */ gyro_cal_get_bias(&data->gyro_cal, bias_out, &temp_out, ×tamp_out); + return true; +} + +static void set_gyro_cal_cache_values(struct motion_sensor_t *sensor, + fpv3_t bias) +{ + size_t sensor_num = sensor - motion_sensors; + struct online_calib_data *calib_data = + (struct online_calib_data *)sensor->online_calib_data; mutex_lock(&g_calib_cache_mutex); - /* Convert result to the right scale. */ - data_fp_to_int16(sensor, bias_out, calib_data->cache); + /* Convert result to the right scale and save to cache. */ + data_fp_to_int16(sensor, bias, calib_data->cache); /* Set valid and dirty. */ sensor_calib_cache_valid_map |= BIT(sensor_num); sensor_calib_cache_dirty_map |= BIT(sensor_num); @@ -140,6 +147,7 @@ static void update_gyro_cal(struct motion_sensor_t *sensor, fpv3_t data, uint32_t timestamp) { int i; + fpv3_t gyro_cal_data_out; /* * Find gyroscopes, while we don't currently have instance where more @@ -151,6 +159,7 @@ static void update_gyro_cal(struct motion_sensor_t *sensor, fpv3_t data, struct gyro_cal_data *gyro_cal_data = (struct gyro_cal_data *) s->online_calib_data->type_specific_data; + bool has_new_gyro_cal_bias = false; /* * If we're not looking at a gyroscope OR if the calibration @@ -168,14 +177,19 @@ static void update_gyro_cal(struct motion_sensor_t *sensor, fpv3_t data, gyro_cal_update_accel(&gyro_cal_data->gyro_cal, timestamp, data[X], data[Y], data[Z]); - check_gyro_cal_new_bias(s); + has_new_gyro_cal_bias = + check_gyro_cal_new_bias(s, gyro_cal_data_out); } else if (sensor->type == MOTIONSENSE_TYPE_MAG && gyro_cal_data->mag_sensor_id == sensor - motion_sensors) { gyro_cal_update_mag(&gyro_cal_data->gyro_cal, timestamp, data[X], data[Y], data[Z]); - check_gyro_cal_new_bias(s); + has_new_gyro_cal_bias = + check_gyro_cal_new_bias(s, gyro_cal_data_out); } + + if (has_new_gyro_cal_bias) + set_gyro_cal_cache_values(s, gyro_cal_data_out); } } @@ -256,27 +270,44 @@ int online_calibration_process_data(struct ec_response_motion_sensor_data *data, int rc; int temperature; struct online_calib_data *calib_data; + fpv3_t fdata; + bool is_spoofed = IS_ENABLED(CONFIG_ONLINE_CALIB_SPOOF_MODE) && + (sensor->flags & MOTIONSENSE_FLAG_IN_SPOOF_MODE); + bool has_new_calibration_values = false; + + /* Convert data to fp. */ + data_int16_to_fp(sensor, data->data, fdata); calib_data = sensor->online_calib_data; switch (sensor->type) { case MOTIONSENSE_TYPE_ACCEL: { struct accel_cal *cal = (struct accel_cal *)(calib_data->type_specific_data); - fpv3_t fdata; - - /* Convert data to fp. */ - data_int16_to_fp(sensor, data->data, fdata); - - /* Possibly update the gyroscope calibration. */ - update_gyro_cal(sensor, fdata, timestamp); - /* Temperature is required for accelerometer calibration. */ - rc = get_temperature(sensor, &temperature); - if (rc != EC_SUCCESS) - return rc; + if (is_spoofed) { + /* Copy the data to the calibration result. */ + cal->bias[X] = fdata[X]; + cal->bias[Y] = fdata[Y]; + cal->bias[Z] = fdata[Z]; + has_new_calibration_values = true; + } else { + /* Possibly update the gyroscope calibration. */ + update_gyro_cal(sensor, fdata, timestamp); + + /* + * Temperature is required for accelerometer + * calibration. + */ + rc = get_temperature(sensor, &temperature); + if (rc != EC_SUCCESS) + return rc; + + has_new_calibration_values = accel_cal_accumulate( + cal, timestamp, fdata[X], fdata[Y], fdata[Z], + temperature); + } - if (accel_cal_accumulate(cal, timestamp, fdata[X], fdata[Y], - fdata[Z], temperature)) { + if (has_new_calibration_values) { mutex_lock(&g_calib_cache_mutex); /* Convert result to the right scale. */ data_fp_to_int16(sensor, cal->bias, calib_data->cache); @@ -297,15 +328,21 @@ int online_calibration_process_data(struct ec_response_motion_sensor_data *data, (int)data->data[Y], (int)data->data[Z], }; - fpv3_t fdata; - /* Convert data to fp. */ - data_int16_to_fp(sensor, data->data, fdata); + if (is_spoofed) { + /* Copy the data to the calibration result. */ + cal->bias[X] = INT_TO_FP(idata[X]); + cal->bias[Y] = INT_TO_FP(idata[Y]); + cal->bias[Z] = INT_TO_FP(idata[Z]); + has_new_calibration_values = true; + } else { + /* Possibly update the gyroscope calibration. */ + update_gyro_cal(sensor, fdata, timestamp); - /* Possibly update the gyroscope calibration. */ - update_gyro_cal(sensor, fdata, timestamp); + has_new_calibration_values = mag_cal_update(cal, idata); + } - if (mag_cal_update(cal, idata)) { + if (has_new_calibration_values) { mutex_lock(&g_calib_cache_mutex); /* Copy the values */ calib_data->cache[X] = cal->bias[X]; @@ -321,21 +358,32 @@ int online_calibration_process_data(struct ec_response_motion_sensor_data *data, break; } case MOTIONSENSE_TYPE_GYRO: { - fpv3_t fdata; - - /* Temperature is required for gyro calibration. */ - rc = get_temperature(sensor, &temperature); - if (rc != EC_SUCCESS) - return rc; - - /* Convert data to fp. */ - data_int16_to_fp(sensor, data->data, fdata); + if (is_spoofed) { + /* + * Gyroscope uses fdata to store the calibration + * result, so there's no need to copy anything. + */ + has_new_calibration_values = true; + } else { + struct gyro_cal_data *gyro_cal_data = + (struct gyro_cal_data *) + calib_data->type_specific_data; + struct gyro_cal *gyro_cal = &gyro_cal_data->gyro_cal; + + /* Temperature is required for gyro calibration. */ + rc = get_temperature(sensor, &temperature); + if (rc != EC_SUCCESS) + return rc; + + /* Update gyroscope calibration. */ + gyro_cal_update_gyro(gyro_cal, timestamp, fdata[X], + fdata[Y], fdata[Z], temperature); + has_new_calibration_values = + check_gyro_cal_new_bias(sensor, fdata); + } - /* Update gyroscope calibration. */ - gyro_cal_update_gyro( - &((struct gyro_cal_data *)calib_data->type_specific_data)->gyro_cal, - timestamp, fdata[X], fdata[Y], fdata[Z], temperature); - check_gyro_cal_new_bias(sensor); + if (has_new_calibration_values) + set_gyro_cal_cache_values(sensor, fdata); break; } default: diff --git a/include/config.h b/include/config.h index 2ec5bb38ad..8606ac52b0 100644 --- a/include/config.h +++ b/include/config.h @@ -2910,6 +2910,19 @@ #undef CONFIG_ONLINE_CALIB /* + * Spoof the data for online calibration. When this flag is enabled, every + * reading with the flag MOTIONSENSE_FLAG_IN_SPOOF_MODE will be treated as a + * new calibration point. This should be used in conjunction with + * CONFIG_ACCEL_SPOOF_MODE. To trigger an accelerometer calibration for + * example, enable both config flags, connect to the cr50 terminal and run: + * $ accelspoof id on X Y Z + * This will spoof a reading of (X, Y, Z) from the sensor and treat those + * values as the calibration result (bypassing the calibration for the given + * sensor ID). + */ +#undef CONFIG_ONLINE_CALIB_SPOOF_MODE + +/* * Duration after which an entry in the temperature cache is considered stale. * Defaults to 5 minutes if not set. */ diff --git a/test/build.mk b/test/build.mk index e994695ab0..fa0e3500cf 100644 --- a/test/build.mk +++ b/test/build.mk @@ -60,6 +60,7 @@ test-list-host += motion_sense_fifo test-list-host += mutex test-list-host += newton_fit test-list-host += online_calibration +test-list-host += online_calibration_spoof test-list-host += pingpong test-list-host += power_button test-list-host += printf @@ -145,7 +146,7 @@ flash_write_protect-y=flash_write_protect.o fpsensor-y=fpsensor.o fpsensor_crypto-y=fpsensor_crypto.o fpsensor_state-y=fpsensor_state.o -gyro_cal-y=gyro_cal.o +gyro_cal-y=gyro_cal.o gyro_cal_init_for_test.o hooks-y=hooks.o host_command-y=host_command.o i2c_bitbang-y=i2c_bitbang.o @@ -165,6 +166,7 @@ motion_angle_tablet-y=motion_angle_tablet.o motion_angle_data_literals_tablet.o motion_lid-y=motion_lid.o motion_sense_fifo-y=motion_sense_fifo.o online_calibration-y=online_calibration.o +online_calibration_spoof-y=online_calibration_spoof.o gyro_cal_init_for_test.o kasa-y=kasa.o mpu-y=mpu.o mutex-y=mutex.o diff --git a/test/gyro_cal.c b/test/gyro_cal.c index 85d53789ef..10d48ca18e 100644 --- a/test/gyro_cal.c +++ b/test/gyro_cal.c @@ -6,6 +6,7 @@ #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> @@ -70,107 +71,6 @@ static float normal_random2(float mean, float stddev) } /** - * - * @param det Pointer to the stillness detector - * @param var_threshold The variance threshold in units^2 - * @param confidence_delta The confidence delta in units^2 - */ -static void gyro_still_det_initialization_for_test(struct gyro_still_det *det, - float var_threshold, - float confidence_delta) -{ - /* Clear all data structure variables to 0. */ - memset(det, 0, sizeof(struct gyro_still_det)); - - /* - * Set the delta about the variance threshold for calculation - * of the stillness confidence score. - */ - if (confidence_delta < var_threshold) - det->confidence_delta = confidence_delta; - else - det->confidence_delta = var_threshold; - - /* - * Set the variance threshold parameter for the stillness - * confidence score. - */ - det->var_threshold = var_threshold; - - /* Signal to start capture of next stillness data window. */ - det->start_new_window = true; -} - -static void gyro_cal_initialization_for_test(struct gyro_cal *gyro_cal) -{ - /* GyroCal initialization. */ - memset(gyro_cal, 0, sizeof(struct gyro_cal)); - - /* - * Initialize the stillness detectors. - * Gyro parameter input units are [rad/sec]. - * Accel parameter input units are [m/sec^2]. - * Magnetometer parameter input units are [uT]. - */ - gyro_still_det_initialization_for_test(&gyro_cal->gyro_stillness_detect, - /* var_threshold */ 5e-5f, - /* confidence_delta */ 1e-5f); - gyro_still_det_initialization_for_test( - &gyro_cal->accel_stillness_detect, - /* var_threshold */ 8e-3f, - /* confidence_delta */ 1.6e-3f); - gyro_still_det_initialization_for_test(&gyro_cal->mag_stillness_detect, - /* var_threshold */ 1.4f, - /* confidence_delta */ 0.25f); - - /* Reset stillness flag and start timestamp. */ - gyro_cal->prev_still = false; - gyro_cal->start_still_time_us = 0; - - /* Set the min and max window stillness duration. */ - gyro_cal->min_still_duration_us = 5 * SECOND; - gyro_cal->max_still_duration_us = 6 * SECOND; - - /* Sets the duration of the stillness processing windows. */ - gyro_cal->window_time_duration_us = 1500000; - - /* Set the window timeout duration. */ - gyro_cal->gyro_window_timeout_duration_us = 5 * SECOND; - - /* Load the last valid cal from system memory. */ - gyro_cal->bias_x = 0.0f; /* [rad/sec] */ - gyro_cal->bias_y = 0.0f; /* [rad/sec] */ - gyro_cal->bias_z = 0.0f; /* [rad/sec] */ - gyro_cal->calibration_time_us = 0; - - /* Set the stillness threshold required for gyro bias calibration. */ - gyro_cal->stillness_threshold = 0.95f; - - /* - * Current window end-time used to assist in keeping sensor data - * collection in sync. Setting this to zero signals that sensor data - * will be dropped until a valid end-time is set from the first gyro - * timestamp received. - */ - gyro_cal->stillness_win_endtime_us = 0; - - /* Gyro calibrations will be applied (see, gyro_cal_remove_bias()). */ - gyro_cal->gyro_calibration_enable = true; - - /* - * Sets the stability limit for the stillness window mean acceptable - * delta. - */ - gyro_cal->stillness_mean_delta_limit = 50.0f * MDEG_TO_RAD; - - /* Sets the min/max temperature delta limit for the stillness period. */ - gyro_cal->temperature_delta_limit_kelvin = 1.5f; - - /* Ensures that the data tracking functionality is reset. */ - init_gyro_cal(gyro_cal); -} - -/** * 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 diff --git a/test/gyro_cal_init_for_test.c b/test/gyro_cal_init_for_test.c new file mode 100644 index 0000000000..3963e5a207 --- /dev/null +++ b/test/gyro_cal_init_for_test.c @@ -0,0 +1,114 @@ +/* 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 "timer.h" +#include "gyro_cal_init_for_test.h" +#include <string.h> + +#define NANO_PI (3.14159265359f) +/** Unit conversion: milli-degrees to radians. */ +#define MDEG_TO_RAD (NANO_PI / 180.0e3f) + +/** + * + * @param det Pointer to the stillness detector + * @param var_threshold The variance threshold in units^2 + * @param confidence_delta The confidence delta in units^2 + */ +static void gyro_still_det_initialization_for_test(struct gyro_still_det *det, + float var_threshold, + float confidence_delta) +{ + /* Clear all data structure variables to 0. */ + memset(det, 0, sizeof(struct gyro_still_det)); + + /* + * Set the delta about the variance threshold for calculation + * of the stillness confidence score. + */ + if (confidence_delta < var_threshold) + det->confidence_delta = confidence_delta; + else + det->confidence_delta = var_threshold; + + /* + * Set the variance threshold parameter for the stillness + * confidence score. + */ + det->var_threshold = var_threshold; + + /* Signal to start capture of next stillness data window. */ + det->start_new_window = true; +} + +void gyro_cal_initialization_for_test(struct gyro_cal *gyro_cal) +{ + /* GyroCal initialization. */ + memset(gyro_cal, 0, sizeof(struct gyro_cal)); + + /* + * Initialize the stillness detectors. + * Gyro parameter input units are [rad/sec]. + * Accel parameter input units are [m/sec^2]. + * Magnetometer parameter input units are [uT]. + */ + gyro_still_det_initialization_for_test(&gyro_cal->gyro_stillness_detect, + /* var_threshold */ 5e-5f, + /* confidence_delta */ 1e-5f); + gyro_still_det_initialization_for_test( + &gyro_cal->accel_stillness_detect, + /* var_threshold */ 8e-3f, + /* confidence_delta */ 1.6e-3f); + gyro_still_det_initialization_for_test(&gyro_cal->mag_stillness_detect, + /* var_threshold */ 1.4f, + /* confidence_delta */ 0.25f); + + /* Reset stillness flag and start timestamp. */ + gyro_cal->prev_still = false; + gyro_cal->start_still_time_us = 0; + + /* Set the min and max window stillness duration. */ + gyro_cal->min_still_duration_us = 5 * SECOND; + gyro_cal->max_still_duration_us = 6 * SECOND; + + /* Sets the duration of the stillness processing windows. */ + gyro_cal->window_time_duration_us = 1500000; + + /* Set the window timeout duration. */ + gyro_cal->gyro_window_timeout_duration_us = 5 * SECOND; + + /* Load the last valid cal from system memory. */ + gyro_cal->bias_x = 0.0f; /* [rad/sec] */ + gyro_cal->bias_y = 0.0f; /* [rad/sec] */ + gyro_cal->bias_z = 0.0f; /* [rad/sec] */ + gyro_cal->calibration_time_us = 0; + + /* Set the stillness threshold required for gyro bias calibration. */ + gyro_cal->stillness_threshold = 0.95f; + + /* + * Current window end-time used to assist in keeping sensor data + * collection in sync. Setting this to zero signals that sensor data + * will be dropped until a valid end-time is set from the first gyro + * timestamp received. + */ + gyro_cal->stillness_win_endtime_us = 0; + + /* Gyro calibrations will be applied (see, gyro_cal_remove_bias()). */ + gyro_cal->gyro_calibration_enable = true; + + /* + * Sets the stability limit for the stillness window mean acceptable + * delta. + */ + gyro_cal->stillness_mean_delta_limit = 50.0f * MDEG_TO_RAD; + + /* Sets the min/max temperature delta limit for the stillness period. */ + gyro_cal->temperature_delta_limit_kelvin = 1.5f; + + /* Ensures that the data tracking functionality is reset. */ + init_gyro_cal(gyro_cal); +} diff --git a/test/gyro_cal_init_for_test.h b/test/gyro_cal_init_for_test.h new file mode 100644 index 0000000000..e32040bab9 --- /dev/null +++ b/test/gyro_cal_init_for_test.h @@ -0,0 +1,39 @@ +/* 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. + */ + +#ifndef __CROS_EC_GYRO_CAL_INIT_FOR_TEST +#define __CROS_EC_GYRO_CAL_INIT_FOR_TEST + +#include "gyro_cal.h" +#include "gyro_still_det.h" + +/** + * Initialization function used for testing the gyroscope calibration. + * This function will initialize to the following values: + * - Gyrscope stillness detector + * - variance threshold: 5e-5 + * - confidence delta: 1e-5 + * - Accelerometer stillness detector + * - variance threshold: 8e-3 + * - confidence delta: 1.6e-3 + * - Magnetometer stillness detector + * - variance threshold: 1.4 + * - confidence delta: 2.5e-1 + * - Minimum stillness duration: 5 seconds + * - Maximum stillness duration: 6 seconds + * - Window duration: 1.5 seconds + * - Window timeout duration: 5 seconds + * - Stillness threshold: 0.95 + * - Stillness mean delta limit: 50 millidegrees + * - Temperature delta limit: 1.5K + * + * Once all the values are set, this function will call init_gyro_cal() + * to finish initializing/resetting the struct data. + * + * @param gyro_cal Pointer to the calibration data structure to initialize. + */ +void gyro_cal_initialization_for_test(struct gyro_cal *gyro_cal); + +#endif /* __CROS_EC_GYRO_CAL_INIT_FOR_TEST */ diff --git a/test/online_calibration_spoof.c b/test/online_calibration_spoof.c new file mode 100644 index 0000000000..66ef5d01de --- /dev/null +++ b/test/online_calibration_spoof.c @@ -0,0 +1,196 @@ +/* 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 "accel_cal.h" +#include "accelgyro.h" +#include "hwtimer.h" +#include "mag_cal.h" +#include "online_calibration.h" +#include "test_util.h" +#include "gyro_cal_init_for_test.h" +#include "gyro_cal.h" +#include "timer.h" +#include <stdio.h> + +int mkbp_send_event(uint8_t event_type) +{ + return 1; +} + +/* + * Mocked driver (can be re-used for all sensors). + */ + +static int mock_read_temp(const struct motion_sensor_t *s, int *temp) +{ + if (temp) + *temp = 200; + return EC_SUCCESS; +} + +static struct accelgyro_drv mock_sensor_driver = { + .read_temp = mock_read_temp, +}; + +/* + * Accelerometer, magnetometer, and gyroscope data structs. + */ + +static struct accel_cal_algo accel_cal_algos[] = { { + .newton_fit = NEWTON_FIT(4, 15, FLOAT_TO_FP(0.01f), FLOAT_TO_FP(0.25f), + FLOAT_TO_FP(1.0e-8f), 100), +} }; + +static struct accel_cal accel_cal_data = { + .still_det = + STILL_DET(FLOAT_TO_FP(0.00025f), 800 * MSEC, 1200 * MSEC, 5), + .algos = accel_cal_algos, + .num_temp_windows = ARRAY_SIZE(accel_cal_algos), +}; + +static struct mag_cal_t mag_cal_data; + +static struct gyro_cal gyro_cal_data; + +/* + * Motion sensor array and count. + */ + +struct motion_sensor_t motion_sensors[] = { + { + .type = MOTIONSENSE_TYPE_ACCEL, + .default_range = 4, + .drv = &mock_sensor_driver, + .online_calib_data[0] = { + .type_specific_data = &accel_cal_data, + }, + }, + { + .type = MOTIONSENSE_TYPE_MAG, + .default_range = 4, + .drv = &mock_sensor_driver, + .online_calib_data[0] = { + .type_specific_data = &mag_cal_data, + }, + }, + { + .type = MOTIONSENSE_TYPE_GYRO, + .default_range = 4, + .drv = &mock_sensor_driver, + .online_calib_data[0] = { + .type_specific_data = &gyro_cal_data, + }, + }, +}; + +const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors); + +static void spoof_sensor_data(struct motion_sensor_t *s, int x, int y, int z) +{ + struct ec_response_motion_sensor_data data; + uint32_t timestamp = 0; + + /* Set the data and flags. */ + data.data[X] = x; + data.data[Y] = y; + data.data[Z] = z; + s->flags |= MOTIONSENSE_FLAG_IN_SPOOF_MODE; + + /* Pass the data to online_calibdation. */ + online_calibration_process_data(&data, s, timestamp); +} + +/* + * Begin testing. + */ + +static int test_accel_calibration_on_spoof(void) +{ + struct ec_response_online_calibration_data out; + + /* Send spoof data (1, 2, 3). */ + spoof_sensor_data(&motion_sensors[0], 1, 2, 3); + + /* Check that we have new values. */ + TEST_ASSERT(online_calibration_has_new_values()); + + /* Get the new values for sensor 0. */ + TEST_ASSERT(online_calibration_read(&motion_sensors[0], &out)); + + /* Validate the new values. */ + TEST_EQ(out.data[X], 1, "%d"); + TEST_EQ(out.data[Y], 2, "%d"); + TEST_EQ(out.data[Z], 3, "%d"); + + /* Validate that no other sensors have data. */ + TEST_ASSERT(!online_calibration_has_new_values()); + + return EC_SUCCESS; +} + +static int test_mag_calibration_on_spoof(void) +{ + struct ec_response_online_calibration_data out; + + /* Send spoof data (4, 5, 6). */ + spoof_sensor_data(&motion_sensors[1], 4, 5, 6); + + /* Check that we have new values. */ + TEST_ASSERT(online_calibration_has_new_values()); + + /* Get the new values for sensor 0. */ + TEST_ASSERT(online_calibration_read(&motion_sensors[1], &out)); + + /* Validate the new values. */ + TEST_EQ(out.data[X], 4, "%d"); + TEST_EQ(out.data[Y], 5, "%d"); + TEST_EQ(out.data[Z], 6, "%d"); + + /* Validate that no other sensors have data. */ + TEST_ASSERT(!online_calibration_has_new_values()); + + return EC_SUCCESS; +} + +static int test_gyro_calibration_on_spoof(void) +{ + struct ec_response_online_calibration_data out; + + /* Send spoof data (7, 8, 9). */ + spoof_sensor_data(&motion_sensors[2], 7, 8, 9); + + /* Check that we have new values. */ + TEST_ASSERT(online_calibration_has_new_values()); + + /* Get the new values for sensor 0. */ + TEST_ASSERT(online_calibration_read(&motion_sensors[2], &out)); + + /* Validate the new values. */ + TEST_EQ(out.data[X], 7, "%d"); + TEST_EQ(out.data[Y], 8, "%d"); + TEST_EQ(out.data[Z], 9, "%d"); + + /* Validate that no other sensors have data. */ + TEST_ASSERT(!online_calibration_has_new_values()); + + return EC_SUCCESS; +} + +void before_test(void) +{ + online_calibration_init(); + gyro_cal_initialization_for_test(&gyro_cal_data); +} + +void run_test(int argc, char **argv) +{ + test_reset(); + + RUN_TEST(test_accel_calibration_on_spoof); + RUN_TEST(test_mag_calibration_on_spoof); + RUN_TEST(test_gyro_calibration_on_spoof); + + test_print_result(); +} diff --git a/test/online_calibration_spoof.tasklist b/test/online_calibration_spoof.tasklist new file mode 100644 index 0000000000..7d28eb5b64 --- /dev/null +++ b/test/online_calibration_spoof.tasklist @@ -0,0 +1,10 @@ +/* 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. + */ + +/** + * See CONFIG_TASK_LIST in config.h for details. + */ +#define CONFIG_TEST_TASK_LIST \ + TASK_TEST(MOTIONSENSE, motion_sense_task, NULL, TASK_STACK_SIZE) diff --git a/test/test_config.h b/test/test_config.h index 51c2e5603d..4e9024f6c1 100644 --- a/test/test_config.h +++ b/test/test_config.h @@ -142,6 +142,14 @@ #define CONFIG_MKBP_USE_GPIO #endif +#ifdef TEST_ONLINE_CALIBRATION_SPOOF +#define CONFIG_FPU +#define CONFIG_ONLINE_CALIB +#define CONFIG_MKBP_EVENT +#define CONFIG_MKBP_USE_GPIO +#define CONFIG_ONLINE_CALIB_SPOOF_MODE +#endif /* TEST_ONLINE_CALIBRATION_SPOOF */ + #ifdef TEST_GYRO_CAL #define CONFIG_FPU #define CONFIG_ONLINE_CALIB |