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
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
|
#ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_
#define LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_
/*
* Copyright (C) 2016 The Android Open Source Project
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
///////////////////////////////////////////////////////////////
/*
* This module contains the algorithms for producing a
* gyroscope offset calibration. The algorithm looks
* for periods of stillness as indicated by accelerometer,
* magnetometer and gyroscope, and computes a bias estimate
* by taking the average of the gyroscope during the
* stillness times.
*
* Currently, this algorithm is tuned such that the device
* is only considered still when the device is on a
* stationary surface (e.g., not on a person).
*
* NOTE - Time units are agnostic (i.e., determined by the
* user's application and usage). However, typical time units
* are nanoseconds.
*
* Required Sensors and Units:
* - Gyroscope [rad/sec]
* - Accelerometer [m/sec^2]
*
* Optional Sensors and Units:
* - Magnetometer [micro-Tesla, uT]
* - Temperature [Celcius]
*/
///////////////////////////////////////////////////////////////
#include "calibration/gyroscope/gyro_stillness_detect.h"
#ifdef __cplusplus
extern "C" {
#endif
#ifdef GYRO_CAL_DBG_ENABLED
// Debug: Number of past calibrations to store.
#define DEBUG_GYRO_SHORTTERM_NUM_CAL 30
#define DEBUG_GYRO_LONGTERM_NUM_CAL 30
#define DEBUG_GYRO_CAL_LIMIT 7200000000000 // 2hours [nsec]
#define DEBUG_GYRO_TUNE_UPDATES 120000000000 // 120sec [nsec]
// Gyro Cal debug information/data tracking structure.
struct DebugGyroCal {
float temperature_celcius;
float accel_stillness_conf;
float gyro_stillness_conf;
float mag_stillness_conf;
uint64_t calibration_time;
uint64_t calibration_time_duration;
bool used_mag_sensor;
float calibration[3];
float accel_mean[3];
float gyro_mean[3];
float mag_mean[3];
float accel_var[3];
float gyro_var[3];
float mag_var[3];
};
#endif
struct GyroCal {
// Stillness detectors.
struct GyroStillDet accel_stillness_detect;
struct GyroStillDet mag_stillness_detect;
struct GyroStillDet gyro_stillness_detect;
// Flag is "true" when the magnetometer is used.
bool using_mag_sensor;
// Flag set by user to control whether calibrations are used
// (default: "true").
bool gyro_calibration_enable;
// Latest temperature measurement.
float latest_temperature_celcius;
// Aggregated sensor stillness threshold required for gyro bias calibration.
float stillness_threshold;
// Min and max durations for gyro bias calibration.
uint64_t min_still_duration;
uint64_t max_still_duration;
// Duration of the stillness processing windows.
uint64_t window_time_duration;
// Flag to indicate if device was previously still.
bool prev_still;
// Timestamp when device started a still period.
uint64_t start_still_time;
// gyro bias estimates and last calibration timestamp.
float bias_x, bias_y, bias_z; // [rad/sec]
uint64_t calibration_time;
uint64_t calibration_time_duration;
float stillness_confidence;
bool new_gyro_cal_available; // true when a new cal is ready.
// 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.
uint64_t stillness_win_endtime;
// Watchdog timer to reset to a known good state when data capture stalls.
uint64_t gyro_watchdog_start;
uint64_t gyro_watchdog_timeout_duration;
bool gyro_watchdog_timeout;
//----------------------------------------------------------------
#ifdef GYRO_CAL_DBG_ENABLED
// Debug info.
bool debug_processed_data_available; // flag on a per window basis.
uint64_t debug_processed_data_time; // flag time stamp.
uint32_t debug_calibration_count; // total number of cals performed.
uint32_t debug_watchdog_count; // total number of watchdog timeouts.
int8_t gyro_debug_state; // debug report state machine.
// Debug short-term history data.
struct DebugGyroCal debug_cal_data[DEBUG_GYRO_SHORTTERM_NUM_CAL];
uint8_t debug_num_cals; // number of calibrations collected.
uint8_t debug_head; // index of last valid calibration.
// Debug long-term history data (limited collection frequency).
struct DebugGyroCal debug_cal_data_hist[DEBUG_GYRO_LONGTERM_NUM_CAL];
uint8_t debug_num_cals_hist; // number of calibrations collected.
uint8_t debug_head_hist; // index of last valid calibration.
#endif
};
/////// FUNCTION PROTOTYPES //////////////////////////////////////////
// Initialize the gyro calibration data structure.
void gyroCalInit(struct GyroCal* gyro_cal, uint64_t min_still_duration,
uint64_t max_still_duration, float bias_x, float bias_y,
float bias_z, uint64_t calibration_time,
uint64_t window_time_duration, float gyro_var_threshold,
float gyro_confidence_delta, float accel_var_threshold,
float accel_confidence_delta, float mag_var_threshold,
float mag_confidence_delta, float stillness_threshold,
int remove_bias_enable);
// Void all pointers in the gyro calibration data structure.
void gyroCalDestroy(struct GyroCal* gyro_cal);
// Get the most recent bias calibration value.
void gyroCalGetBias(struct GyroCal* gyro_cal, float* bias_x, float* bias_y,
float* bias_z);
// Set an initial bias calibration value.
void gyroCalSetBias(struct GyroCal* gyro_cal, float bias_x, float bias_y,
float bias_z, uint64_t calibration_time);
// Remove gyro bias from the calibration [rad/sec].
void gyroCalRemoveBias(struct GyroCal* gyro_cal, float xi, float yi, float zi,
float* xo, float* yo, float* zo);
// Returns true when a new gyro calibration is available.
bool gyroCalNewBiasAvailable(struct GyroCal* gyro_cal);
// Update the gyro calibration with gyro data [rad/sec].
void gyroCalUpdateGyro(struct GyroCal* gyro_cal, uint64_t sample_time,
float x, float y, float z, float temperature);
// Update the gyro calibration with mag data [micro Tesla].
void gyroCalUpdateMag(struct GyroCal* gyro_cal, uint64_t sample_time, float x,
float y, float z);
// Update the gyro calibration with accel data [m/sec^2].
void gyroCalUpdateAccel(struct GyroCal* gyro_cal, uint64_t sample_time,
float x, float y, float z);
#ifdef GYRO_CAL_DBG_ENABLED
// Start the debug data report state machine.
void gyroCalDebugPrintStart(struct GyroCal* gyro_cal);
// Print debug data report.
void gyroCalDebugPrint(struct GyroCal* gyro_cal, uint64_t sample_time);
#endif
#ifdef __cplusplus
}
#endif
#endif // LOCATION_LBS_CONTEXTHUB_NANOAPPS_CALIBRATION_GYROSCOPE_GYRO_CAL_H_
|
