Mercurial > public > hwos_code

annotate src/Tests/compass_test.cpp @ 520:1d74d6221084

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Excel sheets for debugging nofly time from Ralph Lembcke
author heinrichsweikamp
date Fri, 04 Aug 2017 13:41:29 +0200
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 }