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 DecodeCommandBytes.c
10 \brief Command interpreter which interfaces to the Sensor Fusion Toolbox
11 */
12
13 #include "sensor_fusion.h"
14 #include "control.h"
15 #include "fusion.h"
16 #include "calibration_storage.h"
17 // All commands for the command interpreter are exactly 4 characters long.
18 // The command interpeter converts the incoming packet to a 32-bit integer, which is then
19 // the index into a large switch statement for processing commands.
20 // The following block of #define statements are responsible for the conversion from 4-characters
21 // into an easier to use integer format.
22 #define cmd_VGplus (((((('V' << 8) | 'G') << 8) | '+') << 8) | ' ') // "VG+ " = enable angular velocity packet transmission
23 #define cmd_VGminus (((((('V' << 8) | 'G') << 8) | '-') << 8) | ' ') // "VG- " = disable angular velocity packet transmission
24 #define cmd_DBplus (((((('D' << 8) | 'B') << 8) | '+') << 8) | ' ') // "DB+ " = enable debug packet transmission
25 #define cmd_DBminus (((((('D' << 8) | 'B') << 8) | '-') << 8) | ' ') // "DB- " = disable debug packet transmission
26 #define cmd_Q3 (((((('Q' << 8) | '3') << 8) | ' ') << 8) | ' ') // "Q3 " = transmit 3-axis accelerometer quaternion in standard packet
27 #define cmd_Q3M (((((('Q' << 8) | '3') << 8) | 'M') << 8) | ' ') // "Q3M " = transmit 3-axis magnetic quaternion in standard packet
28 #define cmd_Q3G (((((('Q' << 8) | '3') << 8) | 'G') << 8) | ' ') // "Q3G " = transmit 3-axis gyro quaternion in standard packet
29 #define cmd_Q6MA (((((('Q' << 8) | '6') << 8) | 'M') << 8) | 'A') // "Q6MA" = transmit 6-axis mag/accel quaternion in standard packet
30 #define cmd_Q6AG (((((('Q' << 8) | '6') << 8) | 'A') << 8) | 'G') // "Q6AG" = transmit 6-axis accel/gyro quaternion in standard packet
31 #define cmd_Q9 (((((('Q' << 8) | '9') << 8) | ' ') << 8) | ' ') // "Q9 " = transmit 9-axis quaternion in standard packet (default)
32 #define cmd_RPCplus (((((('R' << 8) | 'P') << 8) | 'C') << 8) | '+') // "RPC+" = Roll/Pitch/Compass on
33 #define cmd_RPCminus (((((('R' << 8) | 'P') << 8) | 'C') << 8) | '-') // "RPC-" = Roll/Pitch/Compass off
34 #define cmd_ALTplus (((((('A' << 8) | 'L') << 8) | 'T') << 8) | '+') // "ALT+" = Altitude packet on
35 #define cmd_ALTminus (((((('A' << 8) | 'L') << 8) | 'T') << 8) | '-') // "ALT-" = Altitude packet off
36 #define cmd_RST (((((('R' << 8) | 'S') << 8) | 'T') << 8) | ' ') // "RST " = Soft reset
37 #define cmd_RINS (((((('R' << 8) | 'I') << 8) | 'N') << 8) | 'S') // "RINS" = Reset INS inertial navigation velocity and position
38 #define cmd_SVAC (((((('S' << 8) | 'V') << 8) | 'A') << 8) | 'C') // "SVAC" = save all calibrations to Kinetis flash
39 #define cmd_SVMC (((((('S' << 8) | 'V') << 8) | 'M') << 8) | 'C') // "SVMC" = save magnetic calibration to Kinetis flash
40 #define cmd_SVYC (((((('S' << 8) | 'V') << 8) | 'Y') << 8) | 'C') // "SVYC" = save gyroscope calibration to Kinetis flash
41 #define cmd_SVGC (((((('S' << 8) | 'V') << 8) | 'G') << 8) | 'C') // "SVGC" = save precision accelerometer calibration to Kinetis flash
42 #define cmd_ERAC (((((('E' << 8) | 'R') << 8) | 'A') << 8) | 'C') // "ERAC" = erase all calibrations from Kinetis flash
43 #define cmd_ERMC (((((('E' << 8) | 'R') << 8) | 'M') << 8) | 'C') // "ERMC" = erase magnetic calibration from Kinetis flash
44 #define cmd_ERYC (((((('E' << 8) | 'R') << 8) | 'Y') << 8) | 'C') // "ERYC" = erase gyro offset calibration from Kinetis flash
45 #define cmd_ERGC (((((('E' << 8) | 'R') << 8) | 'G') << 8) | 'C') // "ERGC" = erase precision accelerometer calibration from Kinetis flash
46 #define cmd_180X (((((('1' << 8) | '8') << 8) | '0') << 8) | 'X') // "180X" perturbation
47 #define cmd_180Y (((((('1' << 8) | '8') << 8) | '0') << 8) | 'Y') // "180Y" perturbation
48 #define cmd_180Z (((((('1' << 8) | '8') << 8) | '0') << 8) | 'Z') // "180Z" perturbation
49 #define cmd_M90X (((((('M' << 8) | '9') << 8) | '0') << 8) | 'X') // "M90X" perturbation
50 #define cmd_P90X (((((('P' << 8) | '9') << 8) | '0') << 8) | 'X') // "P90X" perturbation
51 #define cmd_M90Y (((((('M' << 8) | '9') << 8) | '0') << 8) | 'Y') // "M90Y" perturbation
52 #define cmd_P90Y (((((('P' << 8) | '9') << 8) | '0') << 8) | 'Y') // "P90Y" perturbation
53 #define cmd_M90Z (((((('M' << 8) | '9') << 8) | '0') << 8) | 'Z') // "M90Z" perturbation
54 #define cmd_P90Z (((((('P' << 8) | '9') << 8) | '0') << 8) | 'Z') // "P90Z" perturbation
55 #define cmd_PA00 (((((('P' << 8) | 'A') << 8) | '0') << 8) | '0') // "PA00" average precision accelerometer location 0
56 #define cmd_PA01 (((((('P' << 8) | 'A') << 8) | '0') << 8) | '1') // "PA01" average precision accelerometer location 1
57 #define cmd_PA02 (((((('P' << 8) | 'A') << 8) | '0') << 8) | '2') // "PA02" average precision accelerometer location 2
58 #define cmd_PA03 (((((('P' << 8) | 'A') << 8) | '0') << 8) | '3') // "PA03" average precision accelerometer location 3
59 #define cmd_PA04 (((((('P' << 8) | 'A') << 8) | '0') << 8) | '4') // "PA04" average precision accelerometer location 4
60 #define cmd_PA05 (((((('P' << 8) | 'A') << 8) | '0') << 8) | '5') // "PA05" average precision accelerometer location 5
61 #define cmd_PA06 (((((('P' << 8) | 'A') << 8) | '0') << 8) | '6') // "PA06" average precision accelerometer location 6
62 #define cmd_PA07 (((((('P' << 8) | 'A') << 8) | '0') << 8) | '7') // "PA07" average precision accelerometer location 7
63 #define cmd_PA08 (((((('P' << 8) | 'A') << 8) | '0') << 8) | '8') // "PA08" average precision accelerometer location 8
64 #define cmd_PA09 (((((('P' << 8) | 'A') << 8) | '0') << 8) | '9') // "PA09" average precision accelerometer location 9
65 #define cmd_PA10 (((((('P' << 8) | 'A') << 8) | '1') << 8) | '0') // "PA10" average precision accelerometer location 10
66 #define cmd_PA11 (((((('P' << 8) | 'A') << 8) | '1') << 8) | '1') // "PA11" average precision accelerometer location 11
67
DecodeCommandBytes(SensorFusionGlobals * sfg,char iCommandBuffer[],uint8 sUART_InputBuffer[],uint16 nbytes)68 void DecodeCommandBytes(SensorFusionGlobals *sfg, char iCommandBuffer[], uint8 sUART_InputBuffer[], uint16 nbytes)
69 {
70 int32 isum; // 32 bit command identifier
71 int16 i, j; // loop counters
72
73 // parse all received bytes in sUARTInputBuf into the iCommandBuffer delay line
74 for (i = 0; i < nbytes; i++) {
75 // shuffle the iCommandBuffer delay line and add the new command byte
76 for (j = 0; j < 3; j++)
77 iCommandBuffer[j] = iCommandBuffer[j + 1];
78 iCommandBuffer[3] = sUART_InputBuffer[i];
79
80 // check if we have a valid command yet
81 isum = ((((((int32)iCommandBuffer[0] << 8) | iCommandBuffer[1]) << 8) | iCommandBuffer[2]) << 8) | iCommandBuffer[3];
82 switch (isum) {
83 case cmd_VGplus: // "VG+ " = enable angular velocity packet transmission
84 sfg->pControlSubsystem->AngularVelocityPacketOn = true;
85 iCommandBuffer[3] = '~';
86 break;
87
88 case cmd_VGminus: // "VG- " = disable angular velocity packet transmission
89 sfg->pControlSubsystem->AngularVelocityPacketOn = false;
90 iCommandBuffer[3] = '~';
91 break;
92
93 case cmd_DBplus: // "DB+ " = enable debug packet transmission
94 sfg->pControlSubsystem->DebugPacketOn = true;
95 iCommandBuffer[3] = '~';
96 break;
97
98 case cmd_DBminus: // "DB- " = disable debug packet transmission
99 sfg->pControlSubsystem->DebugPacketOn = false;
100 iCommandBuffer[3] = '~';
101 break;
102
103 case cmd_Q3: // "Q3 " = transmit 3-axis accelerometer quaternion in standard packet
104 #if F_3DOF_G_BASIC
105 sfg->pControlSubsystem->QuaternionPacketType = Q3;
106 #endif
107 iCommandBuffer[3] = '~';
108 break;
109
110 case cmd_Q3M: // "Q3M " = transmit 3-axis magnetometer quaternion in standard packet
111 #if F_3DOF_B_BASIC
112 sfg->pControlSubsystem->QuaternionPacketType = Q3M;
113 #endif
114 iCommandBuffer[3] = '~';
115 break;
116
117 case cmd_Q3G: // "Q3G " = transmit 3-axis gyro quaternion in standard packet
118 #if F_3DOF_Y_BASIC
119 sfg->pControlSubsystem->QuaternionPacketType = Q3G;
120 #endif
121 iCommandBuffer[3] = '~';
122 break;
123
124 case cmd_Q6MA: // "Q6MA" = transmit 6-axis mag/accel quaternion in standard packet
125 #if F_6DOF_GB_BASIC
126 sfg->pControlSubsystem->QuaternionPacketType = Q6MA;
127 #endif
128 iCommandBuffer[3] = '~';
129 break;
130
131 case cmd_Q6AG: // "Q6AG" = transmit 6-axis accel/gyro quaternion in standard packet
132 #if F_6DOF_GY_KALMAN
133 sfg->pControlSubsystem->QuaternionPacketType = Q6AG;
134 #endif
135 iCommandBuffer[3] = '~';
136 break;
137
138 case cmd_Q9: // "Q9 " = transmit 9-axis quaternion in standard packet (default)
139 #if F_9DOF_GBY_KALMAN
140 sfg->pControlSubsystem->QuaternionPacketType = Q9;
141 #endif
142 iCommandBuffer[3] = '~';
143 break;
144
145 case cmd_RPCplus: // "RPC+" = Roll/Pitch/Compass on
146 sfg->pControlSubsystem->RPCPacketOn = true;
147 iCommandBuffer[3] = '~';
148 break;
149
150 case cmd_RPCminus: // "RPC-" = Roll/Pitch/Compass off
151 sfg->pControlSubsystem->RPCPacketOn = false;
152 iCommandBuffer[3] = '~';
153 break;
154
155 case cmd_ALTplus: // "ALT+" = Altitude packet on
156 sfg->pControlSubsystem->AltPacketOn = true;
157 iCommandBuffer[3] = '~';
158 break;
159
160 case cmd_ALTminus: // "ALT-" = Altitude packet off
161 sfg->pControlSubsystem->AltPacketOn = false;
162 iCommandBuffer[3] = '~';
163 break;
164
165 case cmd_RST: // "RST " = Soft reset
166 // reset sensor fusion
167 fInitializeFusion(sfg);
168
169 // reset magnetic calibration and magnetometer data buffer
170 #if F_USING_MAG
171 fInitializeMagCalibration(&sfg->MagCal, &sfg->MagBuffer);
172 #endif
173 // reset precision accelerometer calibration and accelerometer measurements
174 #if F_USING_ACCEL
175 fInitializeAccelCalibration(&sfg->AccelCal, &sfg->AccelBuffer, &(sfg->pControlSubsystem->AccelCalPacketOn)) ;
176 #endif
177 iCommandBuffer[3] = '~';
178 break;
179
180 case cmd_RINS: // "RINS" = Reset INS inertial navigation velocity and position
181 #if F_9DOF_GBY_KALMAN
182 for (i = CHX; i <= CHZ; i++) {
183 sfg->SV_9DOF_GBY_KALMAN.fVelGl[i] = 0.0F;
184 sfg->SV_9DOF_GBY_KALMAN.fDisGl[i] = 0.0F;
185 }
186 #endif
187 iCommandBuffer[3] = '~';
188 break;
189
190 case cmd_SVAC: // "SVAC" = save all calibrations to Kinetis flash
191 SaveMagCalibrationToNVM(sfg);
192 SaveGyroCalibrationToNVM(sfg);
193 SaveAccelCalibrationToNVM(sfg);
194 iCommandBuffer[3] = '~';
195 break;
196
197 case cmd_SVMC: // "SVMC" = save magnetic calibration to Kinetis flash
198 SaveMagCalibrationToNVM(sfg);
199 iCommandBuffer[3] = '~';
200 break;
201
202 case cmd_SVYC: // "SVYC" = save gyroscope calibration to Kinetis flash
203 SaveGyroCalibrationToNVM(sfg);
204 iCommandBuffer[3] = '~';
205 break;
206
207 case cmd_SVGC: // "SVGC" = save precision accelerometer calibration to Kinetis flash
208 SaveAccelCalibrationToNVM(sfg);
209 iCommandBuffer[3] = '~';
210 break;
211
212 case cmd_ERAC: // "ERAC" = erase all calibrations from Kinetis flash
213 EraseMagCalibrationFromNVM();
214 EraseGyroCalibrationFromNVM();
215 EraseAccelCalibrationFromNVM();
216 iCommandBuffer[3] = '~';
217 break;
218
219 case cmd_ERMC: // "ERMC" = erase magnetic calibration offset 0 bytes from Kinetis flash
220 EraseMagCalibrationFromNVM();
221 iCommandBuffer[3] = '~';
222 break;
223
224 case cmd_ERYC: // "ERYC" = erase gyro offset calibrationoffset 128 bytes from Kinetis flash
225 EraseGyroCalibrationFromNVM();
226 iCommandBuffer[3] = '~';
227 break;
228
229 case cmd_ERGC: // "ERGC" = erase precision accelerometer calibration offset 192 bytesfrom Kinetis flash
230 EraseAccelCalibrationFromNVM();
231 iCommandBuffer[3] = '~';
232 break;
233
234 case cmd_180X: // "180X" perturbation
235 sfg->iPerturbation = 1;
236 iCommandBuffer[3] = '~';
237 break;
238
239 case cmd_180Y: // "180Y" perturbation
240 sfg->iPerturbation = 2;
241 iCommandBuffer[3] = '~';
242 break;
243
244 case cmd_180Z: // "180Z" perturbation
245 sfg->iPerturbation = 3;
246 iCommandBuffer[3] = '~';
247 break;
248
249 case cmd_M90X: // "M90X" perturbation
250 sfg->iPerturbation = 4;
251 iCommandBuffer[3] = '~';
252 break;
253
254 case cmd_P90X: // "P90X" perturbation
255 sfg->iPerturbation = 5;
256 iCommandBuffer[3] = '~';
257 break;
258
259 case cmd_M90Y: // "M90Y" perturbation
260 sfg->iPerturbation = 6;
261 iCommandBuffer[3] = '~';
262 break;
263
264 case cmd_P90Y: // "P90Y" perturbation
265 sfg->iPerturbation = 7;
266 iCommandBuffer[3] = '~';
267 break;
268
269 case cmd_M90Z: // "M90Z" perturbation
270 sfg->iPerturbation = 8;
271 iCommandBuffer[3] = '~';
272 break;
273
274 case cmd_P90Z: // "P90Z" perturbation
275 sfg->iPerturbation = 9;
276 iCommandBuffer[3] = '~';
277 break;
278
279 #if F_USING_ACCEL
280 case cmd_PA00: // "PA00" average precision accelerometer location 0
281 sfg->AccelBuffer.iStoreLocation = 0;
282 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
283 iCommandBuffer[3] = '~';
284 break;
285
286 case cmd_PA01: // "PA01" average precision accelerometer location 1
287 sfg->AccelBuffer.iStoreLocation = 1;
288 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
289 iCommandBuffer[3] = '~';
290 break;
291
292 case cmd_PA02: // "PA02" average precision accelerometer location 2
293 sfg->AccelBuffer.iStoreLocation = 2;
294 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
295 iCommandBuffer[3] = '~';
296 break;
297
298 case cmd_PA03: // "PA03" average precision accelerometer location 3
299 sfg->AccelBuffer.iStoreLocation = 3;
300 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
301 iCommandBuffer[3] = '~';
302 break;
303
304 case cmd_PA04: // "PA04" average precision accelerometer location 4
305 sfg->AccelBuffer.iStoreLocation = 4;
306 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
307 iCommandBuffer[3] = '~';
308 break;
309
310 case cmd_PA05: // "PA05" average precision accelerometer location 5
311 sfg->AccelBuffer.iStoreLocation = 5;
312 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
313 iCommandBuffer[3] = '~';
314 break;
315
316 case cmd_PA06: // "PA06" average precision accelerometer location 6
317 sfg->AccelBuffer.iStoreLocation = 6;
318 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
319 iCommandBuffer[3] = '~';
320 break;
321
322 case cmd_PA07: // "PA07" average precision accelerometer location 7
323 sfg->AccelBuffer.iStoreLocation = 7;
324 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
325 iCommandBuffer[3] = '~';
326 break;
327
328 case cmd_PA08: // "PA08" average precision accelerometer location 8
329 sfg->AccelBuffer.iStoreLocation = 8;
330 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
331 iCommandBuffer[3] = '~';
332 break;
333
334 case cmd_PA09: // "PA09" average precision accelerometer location 9
335 sfg->AccelBuffer.iStoreLocation = 9;
336 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
337 iCommandBuffer[3] = '~';
338 break;
339
340 case cmd_PA10: // "PA10" average precision accelerometer location 10
341 sfg->AccelBuffer.iStoreLocation = 10;
342 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
343 iCommandBuffer[3] = '~';
344 break;
345
346 case cmd_PA11: // "PA11" average precision accelerometer location 11
347 sfg->AccelBuffer.iStoreLocation = 11;
348 sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
349 iCommandBuffer[3] = '~';
350 break;
351 #endif // precision accelerometer calibration
352
353 default:
354 // no action
355 break;
356 }
357 } // end of loop over received characters
358
359 return;
360 }
361
