115 lines
3.6 KiB
C
115 lines
3.6 KiB
C
/* 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);
|
|
}
|