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
|
/* 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 "motion_sense.h"
#include "newton_fit.h"
#include "test_util.h"
#include <stdio.h>
/*
* Need to define motion sensor globals just to compile.
* We include motion task to force the inclusion of math_util.c
*/
struct motion_sensor_t motion_sensors[] = {};
const unsigned int motion_sensor_count = ARRAY_SIZE(motion_sensors);
#define ACC(FIT, X, Y, Z, EXPECTED) \
TEST_EQ(newton_fit_accumulate(FIT, X, Y, Z), EXPECTED, "%d")
static int test_newton_fit_reset(void)
{
struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);
newton_fit_reset(&fit);
newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);
TEST_EQ(queue_count(fit.orientations), (size_t)1, "%zu");
newton_fit_reset(&fit);
TEST_EQ(queue_count(fit.orientations), (size_t)0, "%zu");
return EC_SUCCESS;
}
static int test_newton_fit_accumulate(void)
{
struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);
struct queue_iterator it;
newton_fit_reset(&fit);
newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);
TEST_EQ(queue_count(fit.orientations), (size_t)1, "%zu");
queue_begin(fit.orientations, &it);
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
return EC_SUCCESS;
}
static int test_newton_fit_accumulate_merge(void)
{
struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);
struct queue_iterator it;
newton_fit_reset(&fit);
newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);
newton_fit_accumulate(&fit, 1.05f, 0.0f, 0.0f);
TEST_EQ(queue_count(fit.orientations), (size_t)1, "%zu");
queue_begin(fit.orientations, &it);
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 2, "%u");
return EC_SUCCESS;
}
static int test_newton_fit_accumulate_prune(void)
{
struct newton_fit fit = NEWTON_FIT(4, 15, 0.01f, 0.25f, 1.0e-8f, 100);
struct queue_iterator it;
newton_fit_reset(&fit);
newton_fit_accumulate(&fit, 1.0f, 0.0f, 0.0f);
newton_fit_accumulate(&fit, -1.0f, 0.0f, 0.0f);
newton_fit_accumulate(&fit, 0.0f, 1.0f, 0.0f);
newton_fit_accumulate(&fit, 0.0f, -1.0f, 0.0f);
TEST_EQ(queue_is_full(fit.orientations), 1, "%d");
queue_begin(fit.orientations, &it);
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
queue_next(fit.orientations, &it);
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
queue_next(fit.orientations, &it);
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
queue_next(fit.orientations, &it);
TEST_EQ(((struct newton_fit_orientation *)it.ptr)->nsamples, 1, "%u");
newton_fit_accumulate(&fit, 0.0f, 0.0f, 1.0f);
TEST_EQ(queue_is_full(fit.orientations), 0, "%d");
return EC_SUCCESS;
}
static int test_newton_fit_calculate(void)
{
struct newton_fit fit = NEWTON_FIT(4, 3, 0.01f, 0.25f, 1.0e-8f, 100);
floatv3_t bias;
float radius;
newton_fit_reset(&fit);
ACC(&fit, 1.01f, 0.01f, 0.01f, false);
ACC(&fit, 1.01f, 0.01f, 0.01f, false);
ACC(&fit, 1.01f, 0.01f, 0.01f, false);
ACC(&fit, -0.99f, 0.01f, 0.01f, false);
ACC(&fit, -0.99f, 0.01f, 0.01f, false);
ACC(&fit, -0.99f, 0.01f, 0.01f, false);
ACC(&fit, 0.01f, 1.01f, 0.01f, false);
ACC(&fit, 0.01f, 1.01f, 0.01f, false);
ACC(&fit, 0.01f, 1.01f, 0.01f, false);
ACC(&fit, 0.01f, 0.01f, 1.01f, false);
ACC(&fit, 0.01f, 0.01f, 1.01f, false);
ACC(&fit, 0.01f, 0.01f, 1.01f, true);
fpv3_init(bias, 0.0f, 0.0f, 0.0f);
newton_fit_compute(&fit, bias, &radius);
TEST_NEAR(bias[0], 0.01f, 0.0001f, "%f");
TEST_NEAR(bias[1], 0.01f, 0.0001f, "%f");
TEST_NEAR(bias[2], 0.01f, 0.0001f, "%f");
TEST_NEAR(radius, 1.0f, 0.0001f, "%f");
return EC_SUCCESS;
}
void run_test(int argc, const char **argv)
{
test_reset();
RUN_TEST(test_newton_fit_reset);
RUN_TEST(test_newton_fit_accumulate);
RUN_TEST(test_newton_fit_accumulate_merge);
RUN_TEST(test_newton_fit_accumulate_prune);
RUN_TEST(test_newton_fit_calculate);
test_print_result();
}
|
