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