1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
|
/* 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_H
#define __CROS_EC_GYRO_CAL_H
#include "common.h"
#include "gyro_still_det.h"
#include "math_util.h"
#include "stdbool.h"
#include "stddef.h"
#include "vec3.h"
struct temperature_mean_data {
int16_t temperature_min_kelvin;
int16_t temperature_max_kelvin;
int16_t latest_temperature_kelvin;
int mean_accumulator;
size_t num_points;
};
/** Data structure for tracking min/max window mean during device stillness. */
struct min_max_window_mean_data {
fpv3_t gyro_winmean_min;
fpv3_t gyro_winmean_max;
};
struct gyro_cal {
/** Stillness detector for accelerometer. */
struct gyro_still_det accel_stillness_detect;
/** Stillness detector for magnetometer. */
struct gyro_still_det mag_stillness_detect;
/** Stillness detector for gyroscope. */
struct gyro_still_det gyro_stillness_detect;
/**
* Data for tracking temperature mean during periods of device
* stillness.
*/
struct temperature_mean_data temperature_mean_tracker;
/** Data for tracking gyro mean during periods of device stillness. */
struct min_max_window_mean_data window_mean_tracker;
/**
* Aggregated sensor stillness threshold required for gyro bias
* calibration.
*/
fp_t stillness_threshold;
/** Min and max durations for gyro bias calibration. */
uint32_t min_still_duration_us;
uint32_t max_still_duration_us;
/** Duration of the stillness processing windows. */
uint32_t window_time_duration_us;
/** Timestamp when device started a still period. */
uint64_t start_still_time_us;
/**
* Gyro offset estimate, and the associated calibration temperature,
* timestamp, and stillness confidence values.
* [rad/sec]
*/
fp_t bias_x, bias_y, bias_z;
int bias_temperature_kelvin;
fp_t stillness_confidence;
uint32_t calibration_time_us;
/**
* Current window end-time for all sensors. Used to assist in keeping
* sensor data collection in sync. On initialization this will be set to
* zero indicating that sensor data will be dropped until a valid
* end-time is set from the first gyro timestamp received.
*/
uint32_t stillness_win_endtime_us;
/**
* Watchdog timer to reset to a known good state when data capture
* stalls.
*/
uint32_t gyro_window_start_us;
uint32_t gyro_window_timeout_duration_us;
/** Flag is "true" when the magnetometer is used. */
bool using_mag_sensor;
/** Flag set by user to control whether calibrations are used. */
bool gyro_calibration_enable;
/** Flag is 'true' when a new calibration update is ready. */
bool new_gyro_cal_available;
/** Flag to indicate if device was previously still. */
bool prev_still;
/**
* Min and maximum stillness window mean. This is used to check the
* stability of the mean values computed for the gyroscope (i.e.
* provides further validation for stillness).
*/
fpv3_t gyro_winmean_min;
fpv3_t gyro_winmean_max;
fp_t stillness_mean_delta_limit;
/**
* The mean temperature over the stillness period. The limit is used to
* check for temperature stability and provide a gate for when
* temperature is rapidly changing.
*/
fp_t temperature_mean_kelvin;
fp_t temperature_delta_limit_kelvin;
};
/**
* Data structure used to configure the gyroscope calibration in individual
* sensors.
*/
struct gyro_cal_data {
/** The gyro_cal struct to use. */
struct gyro_cal gyro_cal;
/** The sensor ID of the accelerometer to use. */
uint8_t accel_sensor_id;
/**
* The sensor ID of the accelerometer to use (use a number greater than
* SENSOR_COUNT to skip).
*/
uint8_t mag_sensor_id;
};
/** Reset trackers. */
void init_gyro_cal(struct gyro_cal *gyro_cal);
/** Get the most recent bias calibration value. */
void gyro_cal_get_bias(struct gyro_cal *gyro_cal, fpv3_t bias,
int *temperature_kelvin, uint32_t *calibration_time_us);
/** Set an initial bias calibration value. */
void gyro_cal_set_bias(struct gyro_cal *gyro_cal, fpv3_t bias,
int temperature_kelvin, uint32_t calibration_time_us);
/** Remove gyro bias from the calibration [rad/sec]. */
void gyro_cal_remove_bias(struct gyro_cal *gyro_cal, fpv3_t in, fpv3_t out);
/** Returns true when a new gyro calibration is available. */
bool gyro_cal_new_bias_available(struct gyro_cal *gyro_cal);
/** Update the gyro calibration with gyro data [rad/sec]. */
void gyro_cal_update_gyro(struct gyro_cal *gyro_cal, uint32_t sample_time_us,
fp_t x, fp_t y, fp_t z, int temperature_kelvin);
/** Update the gyro calibration with mag data [micro Tesla]. */
void gyro_cal_update_mag(struct gyro_cal *gyro_cal, uint32_t sample_time_us,
fp_t x, fp_t y, fp_t z);
/** Update the gyro calibration with accel data [m/sec^2]. */
void gyro_cal_update_accel(struct gyro_cal *gyro_cal, uint32_t sample_time_us,
fp_t x, fp_t y, fp_t z);
#endif /* __CROS_EC_GYRO_CAL_H */
|
