| /* Copyright 2020 The ChromiumOS Authors |
| * 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_init_for_test.h" |
| #include "timer.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); |
| } |