Mercurial > public > hwos_code

annotate src/Tests/compass_test.cpp @ 563:f4faff954816

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
minor
author heinrichsweikamp
date Sat, 03 Feb 2018 17:02:23 +0100
parents cd2320cd6f9a
children
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
282
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
1 //////////////////////////////////////////////////////////////////////////////
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
2 /// compass_test.cpp
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
3 /// Unit test for compass calibration.
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
4 /// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
5 //////////////////////////////////////////////////////////////////////////////
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
6 // HISTORY
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
7 // 2015-05-23 jDG: Rewrite compass testing, to allow reducing code size.
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
8
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
9 extern "C" {
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
10 # include "compass.h"
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
11 }
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
12
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
13 #include <gtest/gtest.h>
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
14
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
15 #include <math.h>
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
16 #include <iostream>
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
17
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
18 //////////////////////////////////////////////////////////////////////////////
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
19
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
20 inline float uniform() {
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
21 return (rand() & 0xFFFF) / 65536.0f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
22 }
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
23 inline float sqr(float x) {
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
24 return x*x;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
25 }
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
26
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
27 static float radius = 0.21f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
28 static float cx = 0, cy = 0, cz = 0;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
29
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
30 //////////////////////////////////////////////////////////////////////////////
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
31
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
32 static void check_calib()
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
33 {
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
34 compass_reset_calibration();
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
35
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
36 //---- Generates random points on a sphere -------------------------------
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
37 // of radius,center (cx, cy, cz):
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
38 for(int i=0; i<100; ++i)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
39 {
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
40 float theta = uniform()*360.0f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
41 float phi = uniform()*180.0f - 90.0f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
42
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
43 float x = cx + radius * cosf(phi)*cosf(theta);
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
44 float y = cy + radius * cosf(phi)*sinf(theta);
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
45 float z = cz + radius * sinf(phi);
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
46
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
47 compass_DX_f = short(32768 * x);
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
48 compass_DY_f = short(32768 * y);
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
49 compass_DZ_f = short(32768 * z);
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
50 compass_add_calibration();
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
51 }
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
52
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
53 compass_solve_calibration();
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
54 float r2 = sqr(compass_CX_f/32768.0f - cx)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
55 + sqr(compass_CY_f/32768.0f - cy)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
56 + sqr(compass_CZ_f/32768.0f - cz);
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
57
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
58 // Calibration error less than 2 bits:
285
cd2320cd6f9a NEW unit tests for gas volumes...
jdg@air
parents: 282
diff changeset
59 EXPECT_NEAR(0, sqrtf(r2), 4.0f/32768.0f)
282
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
60 << "Center at (" << compass_CX_f/32768.0f << ", "
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
61 << compass_CY_f/32768.0f << ", "
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
62 << compass_CZ_f/32768.0f << ")."
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
63 << " Error = " << sqrtf(r2);
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
64 }
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
65
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
66 //////////////////////////////////////////////////////////////////////////////
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
67
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
68 TEST(compass, calibration_centered)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
69 {
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
70 compass_CX_f = compass_CY_f = compass_CZ_f = 0;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
71
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
72 // Half-unit, centered, sphere:
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
73 radius = 0.5f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
74
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
75 // Try 10 recalibration passes:
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
76 for(int p=0; p<10; ++p)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
77 check_calib();
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
78 }
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
79
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
80 //////////////////////////////////////////////////////////////////////////////
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
81
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
82 TEST(compass, calibration_near_centered)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
83 {
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
84 // Put magnetic center elsewhere, but keep position+radius < 1.0, to
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
85 // avoid Q15 overflow...
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
86 radius = 0.21f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
87 cx = 0.019f, cy = -0.026f, cz = 0.004f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
88
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
89 // Try 10 recalibration passes:
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
90 for(int p=0; p<10; ++p)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
91 check_calib();
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
92 }
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
93
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
94 //////////////////////////////////////////////////////////////////////////////
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
95
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
96 TEST(compass, calibration_far_centered)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
97 {
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
98 // Put magnetic center elsewhere, but keep position+radius < 1.0, to
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
99 // avoid Q15 overflow...
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
100 radius = 0.21f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
101 cx = -0.79f, cy = 0.79f, cz = 0.79f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
102
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
103 // Try 10 recalibration passes:
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
104 for(int p=0; p<10; ++p)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
105 check_calib();
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
106 }
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
107
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
108 //////////////////////////////////////////////////////////////////////////////
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
109
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
110 TEST(compass, calibration_small_centered_signal)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
111 {
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
112 // use a very very small magnetic signal, centered:
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
113 radius = 0.001f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
114 cx = 0.000f, cy = 0.000f, cz = 0.000f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
115
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
116 // Try 10 recalibration passes:
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
117 for(int p=0; p<10; ++p)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
118 check_calib();
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
119 }
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
120
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
121 //////////////////////////////////////////////////////////////////////////////
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
122
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
123 TEST(compass, calibration_small_off_centered_signal)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
124 {
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
125 // Have a rather small sphere radius (20%), off-centered buy 80%
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
126 radius = 0.200f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
127 cx = 0.800f, cy = -0.800f, cz = 0.800f;
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
128
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
129 // Try 10 recalibration passes:
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
130 for(int p=0; p<10; ++p)
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
131 check_calib();
7d9edd3b8c86 Make a more compact COMPASS calibration code (<7KB), and add more tests.
jDG
parents:
diff changeset
132 }