282
|
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 }
|