1 /*
2 * Copyright (c) 2015, Freescale Semiconductor, Inc.
3 * Copyright 2016-2017 NXP
4 * All rights reserved.
5 *
6 * SPDX-License-Identifier: BSD-3-Clause
7 */
8
9 /*! \file debug.c
10 \brief ApplyPerturbation function used to analyze dynamic performance
11
12 The ApplyPerturbation function applies a user-specified step function to
13 prior fusion results which is then "released" in the next fusion cycle.
14 When used in conjustion with the NXP Sensor Fusion Toolbox, this provides
15 a visual indication of the dynamic behavior of the library.
16
17 Also included is some code for white-box testing within the IAR debug
18 environment. It can be used to evaluate propagation delays for tilt and
19 eCompass algorithms. It makes no sense with regard to "Rotation", because
20 that algorithm is simple gyro integration, and will never return to the
21 starting point. It will also overestimate delays for the kalman filters,
22 as there is no actual gyro data corresponding to the simulated step function.
23 So those filters are not operating as they would in the normal world.
24 */
25
26 #include "sensor_fusion.h"
27 #include "control.h"
28 #include "stdlib.h"
29 #include "build.h"
30
31 /// The ApplyPerturbation function applies a user-specified step function to
32 /// prior fusion results which is then "released" in the next fusion cycle.
33 /// When used in conjustion with the NXP Sensor Fusion Toolbox, this provides
34 /// a visual indication of the dynamic behavior of the library.
35 /// This function is normally involved via the "sfg." global pointer.
ApplyPerturbation(SensorFusionGlobals * sfg)36 void ApplyPerturbation(SensorFusionGlobals *sfg)
37 {
38 #ifdef INCLUDE_DEBUG_FUNCTIONS
39 // volatile keyword used to force compiler not to optimize out these
40 // variables. this does unfortunately result in a couple of warnings (which
41 // can be ignored) farther down in this code.
42 volatile static uint16_t iTestProgress; ///< Perturbation test status
43 volatile static uint16_t iTestDelay = 0; ///< Measured delay
44 //volatile static uint16_t iTestAngle = 0; ///< Integer Residual angle associated with measured delay
45 volatile float angle=0.0f; ///< Float Residual angle associated with measured delay
46 volatile static float threshold=0;
47 static Quaternion StartingQ = {
48 .q0 = 1.0,
49 .q1 = 0.0,
50 .q2 = 0.0,
51 .q3 = 0.0
52 };
53 Quaternion CurrentQ = {
54 .q0 = 1.0,
55 .q1 = 0.0,
56 .q2 = 0.0,
57 .q3 = 0.0
58 };
59 // cache local copies of control flags so we don't have to keep dereferencing pointers below
60 quaternion_type quaternionPacketType;
61 quaternionPacketType = sfg->pControlSubsystem->QuaternionPacketType;
62
63 Quaternion ftmpq; // scratch quaternion
64
65 // calculate the test perturbation
66 switch (sfg->iPerturbation)
67 {
68 case 1: // 180 degrees about X
69 ftmpq.q0 = 0.0F;
70 ftmpq.q1 = 1.0F;
71 ftmpq.q2 = 0.0F;
72 ftmpq.q3 = 0.0F;
73 threshold = 90.0;
74 break;
75
76 case 2: // 180 degrees about Y
77 ftmpq.q0 = 0.0F;
78 ftmpq.q1 = 0.0F;
79 ftmpq.q2 = 1.0F;
80 ftmpq.q3 = 0.0F;
81 threshold = 90.0;
82 break;
83
84 case 3: // 180 degrees about Z
85 ftmpq.q0 = 0.0F;
86 ftmpq.q1 = 0.0F;
87 ftmpq.q2 = 0.0F;
88 ftmpq.q3 = 1.0F;
89 threshold = 90.0;
90 break;
91
92 case 4: // -90 degrees about X
93 ftmpq.q0 = ONEOVERSQRT2;
94 ftmpq.q1 = -ONEOVERSQRT2;
95 ftmpq.q2 = 0.0F;
96 ftmpq.q3 = 0.0F;
97 threshold = 45.0;
98 break;
99
100 case 5: // +90 degrees about X
101 ftmpq.q0 = ONEOVERSQRT2;
102 ftmpq.q1 = ONEOVERSQRT2;
103 ftmpq.q2 = 0.0F;
104 ftmpq.q3 = 0.0F;
105 threshold = 45.0;
106 break;
107
108 case 6: // -90 degrees about Y
109 ftmpq.q0 = ONEOVERSQRT2;
110 ftmpq.q1 = 0.0F;
111 ftmpq.q2 = -ONEOVERSQRT2;
112 ftmpq.q3 = 0.0F;
113 threshold = 45.0;
114 break;
115
116 case 7: // +90 degrees about Y
117 ftmpq.q0 = ONEOVERSQRT2;
118 ftmpq.q1 = 0.0F;
119 ftmpq.q2 = ONEOVERSQRT2;
120 ftmpq.q3 = 0.0F;
121 threshold = 45.0;
122 break;
123
124 case 8: // -90 degrees about Z
125 ftmpq.q0 = ONEOVERSQRT2;
126 ftmpq.q1 = 0.0F;
127 ftmpq.q2 = 0.0F;
128 ftmpq.q3 = -ONEOVERSQRT2;
129 threshold = 45.0;
130 break;
131
132 case 9: // +90 degrees about Z
133 ftmpq.q0 = ONEOVERSQRT2;
134 ftmpq.q1 = 0.0F;
135 ftmpq.q2 = 0.0F;
136 ftmpq.q3 = ONEOVERSQRT2;
137 threshold = 45.0;
138 break;
139
140 default: // No rotation
141 ftmpq.q0 = 1.0F;
142 ftmpq.q1 = 0.0F;
143 ftmpq.q2 = 0.0F;
144 ftmpq.q3 = 0.0F;
145 break;
146 }
147
148 switch (quaternionPacketType) {
149 #if F_3DOF_G_BASIC
150 case (Q3):
151 CurrentQ = sfg->SV_3DOF_G_BASIC.fLPq;
152 qAeqAxB(&(sfg->SV_3DOF_G_BASIC.fLPq), &ftmpq);
153 break;
154 #endif
155 #if F_3DOF_B_BASIC
156 case (Q3M):
157 CurrentQ = sfg->SV_3DOF_B_BASIC.fLPq;
158 qAeqAxB(&(sfg->SV_3DOF_B_BASIC.fLPq), &ftmpq);
159 break;
160 #endif
161 #if F_3DOF_Y_BASIC
162 case (Q3G):
163 CurrentQ = sfg->SV_3DOF_Y_BASIC.fq;
164 qAeqAxB(&(sfg->SV_3DOF_Y_BASIC.fq), &ftmpq);
165 break;
166 #endif
167 #if F_6DOF_GB_BASIC
168 case (Q6MA):
169 CurrentQ = sfg->SV_6DOF_GB_BASIC.fLPq;
170 qAeqAxB(&(sfg->SV_6DOF_GB_BASIC.fLPq), &ftmpq);
171 break;
172 #endif
173 #if F_6DOF_GY_KALMAN
174 case (Q6AG):
175 CurrentQ = sfg->SV_6DOF_GY_KALMAN.fqPl;
176 qAeqAxB(&(sfg->SV_6DOF_GY_KALMAN.fqPl), &ftmpq);
177 break;
178 #endif
179 #if F_9DOF_GBY_KALMAN
180 case (Q9):
181 CurrentQ = sfg->SV_9DOF_GBY_KALMAN.fqPl;
182 qAeqAxB(&(sfg->SV_9DOF_GBY_KALMAN.fqPl), &ftmpq);
183 break;
184 #endif
185 default:
186 CurrentQ.q0 = 1.0;
187 CurrentQ.q1 = 0.0;
188 CurrentQ.q2 = 0.0;
189 CurrentQ.q3 = 0.0;
190 break;
191 }
192
193 // Begin of code for white-box testing - requires IAR debugger
194 switch (iTestProgress) {
195 case 0: // no test in progress, check to see if we should start one
196 if (sfg->iPerturbation>0) {
197 // Start Test
198 iTestProgress = 1;
199 sfg->iPerturbation = 0;
200 iTestDelay = 0;
201 //iTestAngle = 0;
202 // We'll need the complex conjugate of the starting quaternion
203 StartingQ.q0 = CurrentQ.q0;
204 StartingQ.q1 = -1 * CurrentQ.q1;
205 StartingQ.q2 = -1 * CurrentQ.q2;
206 StartingQ.q3 = -1 * CurrentQ.q3;
207 }
208 break;
209 default: // Test in progress, check to see if trigger reached
210 iTestDelay += 1;
211 qAeqAxB(&CurrentQ, &StartingQ);
212 angle = 2 * F180OVERPI * acos(CurrentQ.q0);
213 angle = fmod(fabs(angle), 180.0);
214 //iTestAngle = (uint16_t) (10 * angle);
215 // In IAR, you can use a Log breakpoint to monitor "return to stationary pose".
216 // Use the following expression in the Message field and check the
217 // checkbox for C-Spy macro. Then Click any of the "Test" buttons
218 // in the Sensor Fusion Toolbox and monitor the results in the Messages window.
219 //"Delay=", iTestDelay:%d, " Angle=",iTestAngle:%d
220 if (angle<threshold) iTestProgress=2; // triggered
221 if (angle < (0.2 * threshold)) iTestProgress=0; // test is done
222 if (iTestDelay>100) iTestProgress=0; // abort test
223 break;
224 }
225 // End of code for white-box testing
226 #endif
227 }
228
