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
|
/* 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 "accel_cal.h"
#include "common.h"
#include "motion_sense.h"
#include "test_util.h"
#include <math.h>
struct motion_sensor_t motion_sensors[] = {};
const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
struct accel_cal_algo algos[2] = {
{
.newton_fit = NEWTON_FIT(8, 1, 0.01f, 0.25f, 1.0e-8f, 100),
},
{
.newton_fit = NEWTON_FIT(8, 1, 0.01f, 0.25f, 1.0e-8f, 100),
}
};
struct accel_cal cal = {
.still_det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5),
.algos = algos,
.num_temp_windows = ARRAY_SIZE(algos),
};
static bool accumulate(float x, float y, float z, float temperature)
{
return accel_cal_accumulate(&cal, 0, x, y, z, temperature) ||
accel_cal_accumulate(&cal, 200 * MSEC, x, y, z, temperature) ||
accel_cal_accumulate(&cal, 400 * MSEC, x, y, z, temperature) ||
accel_cal_accumulate(&cal, 600 * MSEC, x, y, z, temperature) ||
accel_cal_accumulate(&cal, 800 * MSEC, x, y, z, temperature) ||
accel_cal_accumulate(&cal, 1000 * MSEC, x, y, z, temperature);
}
DECLARE_EC_TEST(test_calibrated_correctly_with_kasa)
{
bool has_bias;
accumulate(1.01f, 0.01f, 0.01f, 21.0f);
accumulate(-0.99f, 0.01f, 0.01f, 21.0f);
accumulate(0.01f, 1.01f, 0.01f, 21.0f);
accumulate(0.01f, -0.99f, 0.01f, 21.0f);
accumulate(0.01f, 0.01f, 1.01f, 21.0f);
accumulate(0.01f, 0.01f, -0.99f, 21.0f);
accumulate(0.7171f, 0.7171f, 0.7171f, 21.0f);
has_bias = accumulate(-0.6971f, -0.6971f, -0.6971f, 21.0f);
zassert_true(has_bias);
zassert_within(cal.bias[X], 0.01f, 0.0001f, "%f", cal.bias[X]);
zassert_within(cal.bias[Y], 0.01f, 0.0001f, "%f", cal.bias[Y]);
zassert_within(cal.bias[Z], 0.01f, 0.0001f, "%f", cal.bias[Z]);
return EC_SUCCESS;
}
DECLARE_EC_TEST(test_calibrated_correctly_with_newton)
{
bool has_bias = false;
struct kasa_fit kasa;
fpv3_t kasa_bias;
float kasa_radius;
int i;
float data[] = {
1.00290f, 0.09170f, 0.09649f, 0.95183f, 0.23626f, 0.25853f,
0.95023f, 0.15387f, 0.31865f, 0.97374f, 0.01639f, 0.27675f,
0.88521f, 0.30212f, 0.39558f, 0.92787f, 0.35157f, 0.21209f,
0.95162f, 0.33173f, 0.10924f, 0.98397f, 0.22644f, 0.07737f,
};
kasa_reset(&kasa);
for (i = 0; i < ARRAY_SIZE(data); i += 3) {
zassert_false(has_bias);
kasa_accumulate(&kasa, data[i], data[i + 1], data[i + 2]);
has_bias = accumulate(data[i], data[i + 1], data[i + 2], 21.0f);
}
kasa_compute(&kasa, kasa_bias, &kasa_radius);
zassert_true(has_bias);
/* Check that the bias is right */
zassert_within(cal.bias[X], 0.01f, 0.001f, "%f", cal.bias[X]);
zassert_within(cal.bias[Y], 0.01f, 0.001f, "%f", cal.bias[Y]);
zassert_within(cal.bias[Z], 0.01f, 0.001f, "%f", cal.bias[Z]);
/* Demonstrate that we got a better bias compared to kasa */
zassert_true(sqrtf(powf(cal.bias[X] - 0.01f, 2.0f) +
powf(cal.bias[Y] - 0.01f, 2.0f) +
powf(cal.bias[Z] - 0.01f, 2.0f)) <
sqrtf(powf(kasa_bias[X] - 0.01f, 2.0f) +
powf(kasa_bias[Y] - 0.01f, 2.0f) +
powf(kasa_bias[Z] - 0.01f, 2.0f)),
NULL);
return EC_SUCCESS;
}
DECLARE_EC_TEST(test_temperature_gates)
{
bool has_bias;
accumulate(1.01f, 0.01f, 0.01f, 21.0f);
accumulate(-0.99f, 0.01f, 0.01f, 21.0f);
accumulate(0.01f, 1.01f, 0.01f, 21.0f);
accumulate(0.01f, -0.99f, 0.01f, 21.0f);
accumulate(0.01f, 0.01f, 1.01f, 21.0f);
accumulate(0.01f, 0.01f, -0.99f, 21.0f);
accumulate(0.7171f, 0.7171f, 0.7171f, 21.0f);
has_bias = accumulate(-0.6971f, -0.6971f, -0.6971f, 31.0f);
zassert_false(has_bias);
return EC_SUCCESS;
}
void before_test(void)
{
cal.still_det = STILL_DET(0.00025f, 800 * MSEC, 1200 * MSEC, 5);
accel_cal_reset(&cal);
}
void after_test(void)
{
}
TEST_MAIN()
{
ztest_test_suite(test_accel_cal,
ztest_unit_test_setup_teardown(
test_calibrated_correctly_with_kasa,
before_test, after_test),
ztest_unit_test_setup_teardown(
test_calibrated_correctly_with_newton,
before_test, after_test),
ztest_unit_test_setup_teardown(test_temperature_gates,
before_test,
after_test));
ztest_run_test_suite(test_accel_cal);
}
|
