Mercurial > public > hwos_code

comparison src/Tests/compass_trigo_test.cpp @ 282:7d9edd3b8c86

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
Make a more compact COMPASS calibration code (<7KB), and add more tests.
author jDG
date Fri, 22 May 2015 14:50:40 +0200
parents
children
comparison
equal deleted inserted replaced
281:eb758a5b44eb 282:7d9edd3b8c86
1 //////////////////////////////////////////////////////////////////////////////
2 /// compass_trigo_test.cpp
3 /// Unit test for compass various operations.
4 /// Copyright (c) 2012-2015, JD Gascuel, HeinrichsWeikamp, all right reserved.
5 //////////////////////////////////////////////////////////////////////////////
6 // HISTORY
7 // 2015-05-23 jDG: Rewrite compass testing, to allow reducing code size.
8
9 extern "C" {
10 # include "compass.h"
11 }
12
13 #include <gtest/gtest.h>
14
15 //////////////////////////////////////////////////////////////////////////////
16 // Fake assembleur fixed point multiplies.
17 extern "C" Int16 compass_umul(void);
18 extern "C" Int16 compass_imul(void);
19 extern "C" Int16 compass_a, compass_b;
20
21 // The (filtered) components of the magnetometer sensor:
22 Int16 compass_DX_f;
23 Int16 compass_DY_f;
24 Int16 compass_DZ_f;
25
26 // Found soft-iron calibration values, deduced from already filtered values.
27 Int16 compass_CX_f;
28 Int16 compass_CY_f;
29 Int16 compass_CZ_f;
30
31 // The (filtered) components of the accelerometer sensor:
32 Int16 accel_DX_f;
33 Int16 accel_DY_f;
34 Int16 accel_DZ_f;
35
36 // The compass result value.
37 Int16 compass_heading;
38 Int16 compass_roll;
39 Int16 compass_pitch;
40
41 Int16 compass_a, compass_b;
42 Int16 compass_umul()
43 {
44 unsigned int a = compass_a;
45 unsigned int b = compass_b;
46 a *= b;
47 a >>= 15;
48 return (Int16)a;
49 }
50
51 Int16 compass_imul()
52 {
53 int a = compass_a;
54 int b = compass_b;
55 a *= b;
56 a >>= 15;
57 return (Int16)a;
58 }
59
60 //////////////////////////////////////////////////////////////////////////////
61
62 TEST(ops, multiply) {
63 // Check basic sign handling:
64 EXPECT_EQ(umul( 8000, 4000), (Int16)0x03D0); // 8000/2**15 * 4000/2**15 = 0x3D0
65 EXPECT_EQ(imul(-8000, 4000), (Int16)0xFC2F); // -976 = 0xFC2F
66 EXPECT_EQ(imul( 8000, -4000), (Int16)0xFC2F); // -976 = 0xFC2F
67 EXPECT_EQ(imul(-8000, -4000), (Int16)0x03D0); // +976 = 0x3D0
68 }
69
70 TEST(ops, divide) {
71 // Check basic divides:
72 EXPECT_EQ(udiv(32000, 32001), (Int16)32766); // 0.99997 ~ 32766
73 EXPECT_EQ(udiv( 4000, 8000), (Int16)16384);
74 EXPECT_EQ(udiv( 2000, 8000), (Int16) 8192);
75 EXPECT_EQ(udiv( 1000, 8000), (Int16) 4096);
76 EXPECT_EQ(udiv( 500, 8000), (Int16) 2048);
77 }
78
79 TEST(trigo, atan) {
80 // Check angles returned by the SINGLE QUADRANT atan() function:
81 EXPECT_EQ(utan(100, 100), (Int16)4501); // +1
82 EXPECT_EQ(utan( 90, 100), (Int16)4195); // -4
83 EXPECT_EQ(utan( 80, 100), (Int16)3864); // -2
84 EXPECT_EQ(utan( 70, 100), (Int16)3500); // +1
85 EXPECT_EQ(utan( 60, 100), (Int16)3099); // +3
86 EXPECT_EQ(utan( 50, 100), (Int16)2658); // +1
87 EXPECT_EQ(utan( 40, 100), (Int16)2179); // -1
88 EXPECT_EQ(utan( 30, 100), (Int16)1667); // -3
89 EXPECT_EQ(utan( 20, 100), (Int16)1127); // -4
90 EXPECT_EQ(utan( 10, 100), (Int16) 569); // -2
91 EXPECT_EQ(utan( 0, 100), (Int16) 0);
92 }
93
94 TEST(trigo, cosx2h2) {
95 // Check ONE-OCTANT pseudo-cosinus function
96 // Note: cosxh(x**2, x**2+y**2) is computing cos(atan(y/x))
97 EXPECT_EQ(cosxh(12769, 13169), (Int16)32268); // 113, 20 --> 10.0369° --> 32267 +1
98 EXPECT_EQ(cosxh(10000, 12500), (Int16)29310); // 100, 50 --> 26.5650° --> 29309 +1
99 EXPECT_EQ(cosxh(10000, 20000), (Int16)23171); // 100, 100 --> 45.0000° --> 23170 +1
100 EXPECT_EQ(cosxh( 2500, 12500), (Int16)14658); // 50, 100 --> 63.4349° --> 14654 +4
101 EXPECT_EQ(cosxh( 400, 13169), (Int16) 5718); // 20, 113 --> 79.9631° --> 5711 +7
102 }
103
104 TEST(trigo, sinCos) {
105 Int16 sin, cos;
106
107 //---- Check sincos() FIRST QUADRANT ---------------------------------
108 sincos( 20, 113, &sin, &cos); // 80°
109 EXPECT_EQ(sin, (Int16)32269); // +2
110 EXPECT_EQ(cos, (Int16) 5727); // +16
111
112 sincos( 50, 100, &sin, &cos); // 63°
113 EXPECT_EQ(sin, (Int16)29311); // +2
114 EXPECT_EQ(cos, (Int16)14660); // +6
115
116 sincos(100, 100, &sin, &cos); // 45°
117 EXPECT_EQ(sin, (Int16)23173); // +3
118 EXPECT_EQ(cos, (Int16)23173); // +3
119
120 sincos(100, 50, &sin, &cos); // 27°
121 EXPECT_EQ(sin, (Int16)14660); // +6
122 EXPECT_EQ(cos, (Int16)29311); // +2
123
124 sincos(113, 20, &sin, &cos); // 10°
125 EXPECT_EQ(sin, (Int16) 5727); // +16
126 EXPECT_EQ(cos, (Int16)32269); // +2
127
128 //---- Check sincos() OTHER QUADRANTS --------------------------------
129 sincos(-20, 113, &sin, &cos); // 90+80°
130 EXPECT_EQ(sin, (Int16) 32269); // +2
131 EXPECT_EQ(cos, (Int16) -5727); // +16
132
133 sincos(-20,-113, &sin, &cos); // 180+80°
134 EXPECT_EQ(sin, (Int16)-32269); // +2
135 EXPECT_EQ(cos, (Int16) -5727); // +16
136
137 sincos( 20,-113, &sin, &cos); // 270+80°
138 EXPECT_EQ(sin, (Int16)-32269); // +2
139 EXPECT_EQ(cos, (Int16) 5727); // +16
140 }