Sensor Fusion Library 0.6.1
Orientation sensing for Espressif (ESP32, ESP8266) processors
Loading...
Searching...
No Matches
control_output.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
15#include "sensor_fusion.h" // top level sensor fusion interfaces
16#include "board.h"
17#include "build.h"
18#include "control.h" // Command/Streaming interface - application specific
19#include "fusion_testing.h" // will include SensorPerturbations for test purposes
20
21// OutputBufAppendItem() appends a variable number of source bytes to a destination buffer
22// for transmission as the output packet.
23// Output packets are delimited by inserting the special byte 0x7E at the start
24// and end of packets. this function must therefore handle the case of 0x7E appearing
25// as general data. this is done here with the substitutions:
26// a) replace 0x7E by 0x7D and 0x5E (one byte becomes two)
27// b) replace 0x7D by 0x7D and 0x5D (one byte becomes two)
28// the inverse mapping must be performed at the application receiving the output stream: ie:
29// replace 0x7D and 0x5E with 0x7E
30// replace 0x7D and 0x5D with 0x7D
31// NOTE: do not use this function to append the start and end bytes 0x7E to the output
32// buffer. instead add the start and end bytes 0x7E explicitly as in:
33// output_buf[iByteCount++] = 0x7E;
34void OutputBufAppendItem(uint8_t *pDest, uint16_t *pIndex, uint8_t *pSource,
35 uint16_t iBytesToCopy)
36{
37 uint16_t i; // loop counter
38
39 // loop over number of bytes to add to the destination buffer
40 for (i = 0; i < iBytesToCopy; i++)
41 {
42 switch (pSource[i])
43 {
44 case 0x7E:
45 // special case 1: replace 0x7E (start and end byte) with 0x7D and 0x5E
46 pDest[(*pIndex)++] = 0x7D;
47 pDest[(*pIndex)++] = 0x5E;
48 break;
49
50 case 0x7D:
51 // special case 2: replace 0x7D with 0x7D and 0x5D
52 pDest[(*pIndex)++] = 0x7D;
53 pDest[(*pIndex)++] = 0x5D;
54 break;
55
56 default:
57 // general case, simply add this byte without change
58 pDest[(*pIndex)++] = pSource[i];
59 break;
60 }
61 }
62
63 return;
64}//end OutputBufAppendItem()
65
66// OutputBufAppendZeros() is a utility function for appending zeros
67void OutputBufAppendZeros(uint8_t *pDest, uint16_t *pIndex, uint16_t numZeros) {
68 int16_t scratch16 = 0;
69 uint16_t i;
70 for (i=0; i<numZeros; i++) {
71 OutputBufAppendItem(pDest, pIndex, (uint8_t *) &scratch16, 2);
72 }
73}//end OutputBufAppendZeros()
74
75// utility function for reading common algorithm parameters
76void ReadCommonParams( SV_ptr data,
77 Quaternion *fq,
78 int16_t *iPhi,
79 int16_t *iThe,
80 int16_t *iRho,
81 int16_t iOmega[],
82 uint16_t *isystick) {
83 *fq = data->fq;
84 iOmega[CHX] = (int16_t) (data->fOmega[CHX] * 20.0F);
85 iOmega[CHY] = (int16_t) (data->fOmega[CHY] * 20.0F);
86 iOmega[CHZ] = (int16_t) (data->fOmega[CHZ] * 20.0F);
87 *iPhi = (int16_t) (10.0F * data->fPhi);
88 *iThe = (int16_t) (10.0F * data->fThe);
89 *iRho = (int16_t) (10.0F * data->fRho);
90 *isystick = (uint16_t) (data->systick / 20);
91}//end ReadCommonParams()
92
93// Throttle back output stream by fractional multiplier
94bool Throttle()
95{
96 static int32_t iThrottle = 0;
97 bool skip;
98 // The UART (serial over USB and over WiFi / Bluetooth)
99 // is limited to 115kbps which is more than adequate for the 31kbps
100 // needed at the default 25Hz output rate but insufficient for 100Hz or
101 // 200Hz output rates. There is little point is providing output data
102 // faster than 25Hz video rates but since the UARTs can
103 // support a higher rate, the limit is set to MAXPACKETRATEHZ=40Hz.
104
105 // the increment applied to iThrottle is in the range 0 to (RATERESOLUTION - 1)
106 iThrottle += ((int32_t) MAXPACKETRATEHZ * (int32_t) RATERESOLUTION) / (int32_t) FUSION_HZ;
107 if (iThrottle >= RATERESOLUTION) {
108 // update the throttle counter and transmit the packets over UART (USB and Bluetooth)
109 iThrottle -= RATERESOLUTION;
110 skip = false;
111 } else {
112 skip = true;
113 }
114 return(skip);
115}//end Throttle()
116
117// prepare packets to send, e.g. via Bluetooth, or UART to OpenSDA / USB
118void CreateOutgoingPackets(SensorFusionGlobals *sfg)
119{
120 uint8_t *output_buf = sfg->pControlSubsystem->serial_out_buf;
121 Quaternion fq; // quaternion to be transmitted
122 float ftmp; // scratch
123 static uint32_t iTimeStamp = 0; // 1MHz time stamp
124 uint16_t iIndex; // output buffer counter
125 int32_t scratch32; // scratch int32_t
126 int16_t scratch16; // scratch int16_t
127 int16_t iPhi,
128 iThe,
129 iRho; // integer angles to be transmitted
130 int16_t iDelta; // magnetic inclination angle if available
131 int16_t iOmega[3]; // scaled angular velocity vector
132 uint16_t isystick; // algorithm systick time
133 int16_t i, j, k; // general purpose
134 uint8_t tmpuint8_t; // scratch uint8_t
135 uint8_t flags; // byte of flags
136 uint8_t AngularVelocityPacketOn,
137 DebugPacketOn,
138 RPCPacketOn;
139 int8_t AccelCalPacketOn;
140 static uint8_t iPacketNumber = 0; // packet number
141
142 // update the 1MHz time stamp counter expected by the PC GUI (independent of project clock rates)
143 iTimeStamp += 1000000 / FUSION_HZ;
144
145#if (MAXPACKETRATEHZ < FUSION_HZ)
146 if (Throttle()) return; // need to skip packet transmission to avoid UART overrun
147#endif
148
149 // cache local copies of control flags so we don't have to keep dereferencing pointers below
150 quaternion_type quaternionPacketType;
151 quaternionPacketType = sfg->pControlSubsystem->QuaternionPacketType;
152 AngularVelocityPacketOn = sfg->pControlSubsystem->AngularVelocityPacketOn;
153 DebugPacketOn = sfg->pControlSubsystem->DebugPacketOn;
154 RPCPacketOn = sfg->pControlSubsystem->RPCPacketOn;
155 AccelCalPacketOn = sfg->pControlSubsystem->AccelCalPacketOn;
156
157 // zero the counter for bytes accumulated into the transmit buffer
158 sfg->pControlSubsystem->bytes_to_send = 0;
159 iIndex = 0;
160
161 // ************************************************************************
162 // Main type 1: range 0 to 35 = 36 bytes
163 // Debug type 2: range 0 to 7 = 8 bytes
164 // Angular velocity type 3: range 0 to 13 = 14 bytes
165 // Euler angles type 4: range 0 to 13 = 14 bytes
166 // Altitude/Temp type 5: range 0 to 13 = 14 bytes
167 // Magnetic type 6: range 0 to 16 = 18 bytes
168 // Kalman packet 7: range 0 to 47 = 48 bytes
169 // Precision Accelerometer packet 8: range 0 to 46 = 47 bytes
170 //
171 // Total excluding intermittent packet 8 is:
172 // 152 bytes vs 256 bytes size of output_buf
173 // at 25Hz, data rate is 25*152 = 3800 bytes/sec = 38.0kbaud = 33% of 115.2kbaud
174 // at 40Hz, data rate is 40*152 = 6080 bytes/sec = 60.8kbaud = 53% of 115.2kbaud
175 // at 50Hz, data rate is 50*152 = 7600 bytes/sec = 76.0kbaud = 66% of 115.2kbaud
176 // ************************************************************************
177 // ************************************************************************
178 // fixed length packet type 1
179 // this packet type is always transmitted
180 // total size is 0 to 35 equals 36 bytes
181 // ************************************************************************
182 // [0]: packet start byte (need a iIndex++ here since not using OutputBufAppendItem)
183 output_buf[iIndex++] = 0x7E;
184
185 // [1]: packet type 1 byte (iIndex is automatically updated in OutputBufAppendItem)
186 tmpuint8_t = 0x01;
187 OutputBufAppendItem(output_buf, &iIndex, &tmpuint8_t, 1);
188
189 // [2]: packet number byte
190 OutputBufAppendItem(output_buf, &iIndex, &iPacketNumber, 1);
191 iPacketNumber++;
192
193 // [6-3]: 1MHz time stamp (4 bytes)
194 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &iTimeStamp, 4);
195
196 // [12-7]: integer accelerometer data words (scaled to 8192 counts per g for PC GUI)
197 // send non-zero data only if the accelerometer sensor is enabled and used by the selected quaternion
198 if (sfg->iFlags & F_USING_ACCEL) {
199 switch (quaternionPacketType)
200 {
201 case Q3:
202 case Q6MA:
203 case Q6AG:
204 case Q9:
205#if F_USING_ACCEL
206 // accelerometer data is used for the selected quaternion so transmit but clip at 4g
207 if( sfg->Accel.iCountsPerg != 0 ) {
208 scratch32 = (sfg->Accel.iGc[CHX] * 8192) / sfg->Accel.iCountsPerg;
209 if (scratch32 > 32767) scratch32 = 32767;
210 if (scratch32 < -32768) scratch32 = -32768;
211 scratch16 = (int16_t) (scratch32);
212 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2); //crashes here
213 scratch32 = (sfg->Accel.iGc[CHY] * 8192) / sfg->Accel.iCountsPerg;
214 if (scratch32 > 32767) scratch32 = 32767;
215 if (scratch32 < -32768) scratch32 = -32768;
216 scratch16 = (int16_t) (scratch32);
217 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
218 scratch32 = (sfg->Accel.iGc[CHZ] * 8192) / sfg->Accel.iCountsPerg;
219 if (scratch32 > 32767) scratch32 = 32767;
220 if (scratch32 < -32768) scratch32 = -32768;
221 scratch16 = (int16_t) (scratch32);
222 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
223 }else { //avoid divide-by-zero
224 scratch16 = 32767;
225 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
226 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
227 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
228 }
229 break;
230#endif // F_USING_ACCEL
231 case Q3M:
232 case Q3G:
233 default:
234 // accelerometer data is not used in currently selected algorithm so transmit zero
235 OutputBufAppendZeros(output_buf, &iIndex, 3);
236 break;
237 }
238 } else {
239 // accelerometer structure is not defined so transmit zero
240 OutputBufAppendZeros(output_buf, &iIndex, 3);
241 }
242 // [18-13]: integer calibrated magnetometer data words (already scaled to 10 count per uT for PC GUI)
243 // send non-zero data only if the magnetometer sensor is enabled and used by the selected quaternion
244 if (sfg->iFlags & F_USING_MAG)
245 switch (quaternionPacketType)
246 {
247 case Q3M:
248 case Q6MA:
249 case Q9:
250#if F_USING_MAG
251 // magnetometer data is used for the selected quaternion so transmit
252 if( sfg->Mag.iCountsPeruT != 0 ) {
253 scratch16 = (int16_t) (sfg->Mag.iBc[CHX] * 10) / (sfg->Mag.iCountsPeruT);
254 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
255 scratch16 = (int16_t) ((sfg->Mag.iBc[CHY] * 10) / sfg->Mag.iCountsPeruT);
256 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
257 scratch16 = (int16_t) ((sfg->Mag.iBc[CHZ] * 10) / sfg->Mag.iCountsPeruT);
258 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
259 }else { //prevent divide-by-zero
260 scratch16 = 32767;
261 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
262 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
263 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
264 }
265 break;
266#endif
267 // magnetometer data is not used in currently selected algorithm so transmit zero
268 case Q3:
269 case Q3G:
270 case Q6AG:
271 default:
272 OutputBufAppendZeros(output_buf, &iIndex, 3);
273 break;
274 }
275 else
276 {
277 // magnetometer structure is not defined so transmit zero
278 OutputBufAppendZeros(output_buf, &iIndex, 3);
279 }
280
281 // [24-19]: uncalibrated gyro data words (scaled to 20 counts per deg/s for PC GUI)
282 // send non-zero data only if the gyro sensor is enabled and used by the selected quaternion
283 if (sfg->iFlags & F_USING_GYRO)
284 {
285 switch (quaternionPacketType)
286 {
287 case Q3G:
288 case Q6AG:
289#if F_USING_GYRO
290 case Q9:
291
292 // gyro data is used for the selected quaternion so transmit
293 if( sfg->Gyro.iCountsPerDegPerSec != 0 ) {
294 scratch16 = (int16_t) ((sfg->Gyro.iYs[CHX] * 20) / sfg->Gyro.iCountsPerDegPerSec);
295 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
296 scratch16 = (int16_t) ((sfg->Gyro.iYs[CHY] * 20) / sfg->Gyro.iCountsPerDegPerSec);
297 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
298 scratch16 = (int16_t) ((sfg->Gyro.iYs[CHZ] * 20) / sfg->Gyro.iCountsPerDegPerSec);
299 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
300 }else { //prevent divide-by-zero
301 scratch16 = 32767;
302 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
303 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
304 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
305 }
306 break;
307#endif
308 case Q3:
309 case Q3M:
310 case Q6MA:
311 default:
312 // gyro data is not used in currently selected algorithm so transmit zero
313 OutputBufAppendZeros(output_buf, &iIndex, 3);
314 break;
315 }
316 }
317 else
318 {
319 // gyro structure is not defined so transmit zero
320 OutputBufAppendZeros(output_buf, &iIndex, 3);
321 }
322
323 // initialize default quaternion, flags byte, angular velocity and orientation
324 fq.q0 = 1.0F;
325 fq.q1 = fq.q2 = fq.q3 = 0.0F;
326 flags = 0x00;
327 iOmega[CHX] = iOmega[CHY] = iOmega[CHZ] = 0;
328 iPhi = iThe = iRho = iDelta = 0;
329 isystick = 0;
330
331 // flags byte 33: quaternion type in least significant nibble
332 // Q3: coordinate nibble, 1
333 // Q3M: coordinate nibble, 6
334 // Q3G: coordinate nibble, 3
335 // Q6MA: coordinate nibble, 2
336 // Q6AG: coordinate nibble, 4
337 // Q9: coordinate nibble, 8
338 // flags byte 33: coordinate in most significant nibble
339 // Aerospace/NED: 0, quaternion nibble
340 // Android: 1, quaternion nibble
341 // Windows 8: 2, quaternion nibble
342 // set the quaternion, flags, angular velocity and Euler angles
343 switch (quaternionPacketType)
344 {
345#if F_3DOF_G_BASIC
346 case Q3:
347 if (sfg->iFlags & F_3DOF_G_BASIC)
348 {
349 flags |= 0x01;
350 ReadCommonParams((SV_ptr)&sfg->SV_3DOF_G_BASIC, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
351 }
352 break;
353#endif
354#if F_3DOF_B_BASIC
355 case Q3M:
356 if (sfg->iFlags & F_3DOF_B_BASIC)
357 {
358 flags |= 0x06;
359 ReadCommonParams((SV_ptr)&sfg->SV_3DOF_B_BASIC, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
360 }
361 break;
362#endif
363#if F_3DOF_Y_BASIC
364 case Q3G:
365 if (sfg->iFlags & F_3DOF_Y_BASIC)
366 {
367 flags |= 0x03;
368 ReadCommonParams((SV_ptr)&sfg->SV_3DOF_Y_BASIC, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
369 }
370 break;
371#endif
372#if F_6DOF_GB_BASIC
373 case Q6MA:
374 if (sfg->iFlags & F_6DOF_GB_BASIC)
375 {
376 flags |= 0x02;
377 iDelta = (int16_t) (10.0F * sfg->SV_6DOF_GB_BASIC.fLPDelta);
378 ReadCommonParams((SV_ptr)&sfg->SV_6DOF_GB_BASIC, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
379 }
380 break;
381#endif
382#if F_6DOF_GY_KALMAN
383 case Q6AG:
384 if (sfg->iFlags & F_6DOF_GY_KALMAN)
385 {
386 flags |= 0x04;
387 ReadCommonParams((SV_ptr)&sfg->SV_6DOF_GY_KALMAN, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
388 }
389 break;
390#endif
391#if F_9DOF_GBY_KALMAN
392 case Q9:
393 if (sfg->iFlags & F_9DOF_GBY_KALMAN)
394 {
395 flags |= 0x08;
396 iDelta = (int16_t) (10.0F * sfg->SV_9DOF_GBY_KALMAN.fDeltaPl);
397 ReadCommonParams((SV_ptr)&sfg->SV_9DOF_GBY_KALMAN, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
398 }
399 break;
400#endif
401 default:
402 // use the default data already initialized
403 break;
404 }
405
406 // [32-25]: scale the quaternion (30K = 1.0F) and add to the buffer
407 scratch16 = (int16_t) (fq.q0 * 30000.0F);
408 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
409 scratch16 = (int16_t) (fq.q1 * 30000.0F);
410 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
411 scratch16 = (int16_t) (fq.q2 * 30000.0F);
412 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
413 scratch16 = (int16_t) (fq.q3 * 30000.0F);
414 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
415
416 // set the coordinate system bits in flags from default NED (00)
417#if THISCOORDSYSTEM == ANDROID
418 // set the Android flag bits
419 flags |= 0x10;
420#elif THISCOORDSYSTEM == WIN8
421 // set the Win8 flag bits
422 flags |= 0x20;
423#endif // THISCOORDSYSTEM
424
425 // [33]: add the flags byte to the buffer
426 OutputBufAppendItem(output_buf, &iIndex, &flags, 1);
427
428 // [34]: add the shield (bits 7-5) and Kinetis (bits 4-0) byte
429 tmpuint8_t = ((THIS_SHIELD & 0x07) << 5) | (THIS_BOARD & 0x1F);
430 OutputBufAppendItem(output_buf, &iIndex, &tmpuint8_t, 1);
431
432 // [35]: add the tail byte for the standard packet type 1
433 output_buf[iIndex++] = 0x7E;
434
435 // ************************************************************************
436 // Variable length debug packet type 2
437 // total size is 0 to 7 equals 8 bytes
438 // ************************************************************************
439 if (DebugPacketOn)
440 {
441 // [0]: packet start byte
442 output_buf[iIndex++] = 0x7E;
443
444 // [1]: packet type 2 byte
445 tmpuint8_t = 0x02;
446 OutputBufAppendItem(output_buf, &iIndex, &tmpuint8_t, 1);
447
448 // [2]: packet number byte
449 OutputBufAppendItem(output_buf, &iIndex, &iPacketNumber, 1);
450 iPacketNumber++;
451
452 // [4-3] software version number
453 scratch16 = THISBUILD;
454 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
455
456 // [6-5] systick count / 20
457 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &isystick, 2);
458
459 // [7 in practice but can be variable]: add the tail byte for the debug packet type 2
460 output_buf[iIndex++] = 0x7E;
461 }
462
463 // ************************************************************************
464 // Angular Velocity packet type 3
465 // total bytes for packet type 2 is range 0 to 13 = 14 bytes
466 // ************************************************************************
467 if (AngularVelocityPacketOn)
468 {
469 // [0]: packet start byte
470 output_buf[iIndex++] = 0x7E;
471
472 // [1]: packet type 3 byte (angular velocity)
473 tmpuint8_t = 0x03;
474 OutputBufAppendItem(output_buf, &iIndex, &tmpuint8_t, 1);
475
476 // [2]: packet number byte
477 OutputBufAppendItem(output_buf, &iIndex, &iPacketNumber, 1);
478 iPacketNumber++;
479
480 // [6-3]: time stamp (4 bytes)
481 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &iTimeStamp, 4);
482
483 // [12-7]: add the scaled angular velocity vector to the output buffer
484 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &iOmega[CHX], 2);
485 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &iOmega[CHY], 2);
486 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &iOmega[CHZ], 2);
487
488 // [13]: add the tail byte for the angular velocity packet type 3
489 output_buf[iIndex++] = 0x7E;
490 }
491
492 // ************************************************************************
493 // Roll, Pitch, Compass Euler angles packet type 4
494 // total bytes for packet type 4 is range 0 to 13 = 14 bytes
495 // ************************************************************************
496 if (RPCPacketOn)
497 {
498 // [0]: packet start byte
499 output_buf[iIndex++] = 0x7E;
500
501 // [1]: packet type 4 byte (Euler angles)
502 tmpuint8_t = 0x04;
503 OutputBufAppendItem(output_buf, &iIndex, &tmpuint8_t, 1);
504
505 // [2]: packet number byte
506 OutputBufAppendItem(output_buf, &iIndex, &iPacketNumber, 1);
507 iPacketNumber++;
508
509 // [6-3]: time stamp (4 bytes)
510 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &iTimeStamp, 4);
511
512 // [12-7]: add the angles (resolution 0.1 deg per count) to the transmit buffer
513 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &iPhi, 2);
514 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &iThe, 2);
515 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &iRho, 2);
516
517 // [13]: add the tail byte for the roll, pitch, compass angle packet type 4
518 output_buf[iIndex++] = 0x7E;
519 }
520
521 // ************************************************************************
522 // Altitude / Temperature packet type 5
523 // total bytes for packet type 5 is range 0 to 13 = 14 bytes
524 // ************************************************************************
525#if F_USING_PRESSURE
526 if (sfg->iFlags & F_1DOF_P_BASIC)
527 {
528 if (sfg->pControlSubsystem->AltPacketOn && sfg->Pressure.iWhoAmI)
529 {
530 // [0]: packet start byte
531 output_buf[iIndex++] = 0x7E;
532
533 // [1]: packet type 5 byte
534 tmpuint8_t = 0x05;
535 OutputBufAppendItem(output_buf, &iIndex, &tmpuint8_t, 1);
536
537 // [2]: packet number byte
538 OutputBufAppendItem(output_buf, &iIndex, &iPacketNumber, 1);
539 iPacketNumber++;
540
541 // [6-3]: time stamp (4 bytes)
542 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &iTimeStamp,
543 4);
544
545 // [10-7]: altitude (4 bytes, metres times 1000)
546 scratch32 = (int32_t) (sfg->SV_1DOF_P_BASIC.fLPH * 1000.0F);
547 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch32, 4);
548
549 // [12-11]: temperature (2 bytes, deg C times 100)
550 scratch16 = (int16_t) (sfg->SV_1DOF_P_BASIC.fLPT * 100.0F);
551 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
552
553 // [13]: add the tail byte for the altitude / temperature packet type 5
554 output_buf[iIndex++] = 0x7E;
555 }
556 }
557#endif
558
559 // ************************************************************************
560 // magnetic buffer packet type 6
561 // currently total size is 0 to 17 equals 18 bytes
562 // this packet is only transmitted if a magnetic algorithm is computed
563 // ************************************************************************
564#if F_USING_MAG
565 static int16_t MagneticPacketID = 0; // magnetic packet number
566 if (sfg->iFlags & F_USING_MAG)
567 {
568 // [0]: packet start byte
569 output_buf[iIndex++] = 0x7E;
570
571 // [1]: packet type 6 byte
572 tmpuint8_t = 0x06;
573 OutputBufAppendItem(output_buf, &iIndex, &tmpuint8_t, 1);
574
575 // [2]: packet number byte
576 OutputBufAppendItem(output_buf, &iIndex, &iPacketNumber, 1);
577 iPacketNumber++;
578
579 // [4-3]: number of active measurements in the magnetic buffer
580 OutputBufAppendItem(output_buf, &iIndex,
581 (uint8_t *) &(sfg->MagBuffer.iMagBufferCount), 2);
582
583 // [6-5]: fit error (%) with resolution 0.01%
584 if (sfg->MagCal.fFitErrorpc > 327.67F)
585 scratch16 = 32767;
586 else
587 scratch16 = (int16_t) (sfg->MagCal.fFitErrorpc * 100.0F);
588 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
589
590 // [8-7]: geomagnetic field strength with resolution 0.1uT
591 scratch16 = (int16_t) (sfg->MagCal.fB * 10.0F);
592 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
593
594 // always calculate magnetic buffer row and column (low overhead and saves warnings)
595 k = MagneticPacketID - 10;
596 j = k / MAGBUFFSIZEX;
597 i = k - j * MAGBUFFSIZEX;
598
599 // [10-9]: int16_t: ID of magnetic variable to be transmitted
600 // ID 0 to 4 inclusive are magnetic calibration coefficients
601 // ID 5 to 9 inclusive are for future expansion
602 // ID 10 to (MAGBUFFSIZEX=12) * (MAGBUFFSIZEY=24)-1 or 10 to 10+288-1 are magnetic buffer elements
603 // where the convention is used that a negative value indicates empty buffer element (index=-1)
604 if ((MagneticPacketID >= 10) && (sfg->MagBuffer.index[i][j] == -1))
605 {
606 // use negative ID to indicate inactive magnetic buffer element
607 scratch16 = -MagneticPacketID;
608 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
609 }
610 else
611 {
612 // use positive ID unchanged for variable or active magnetic buffer entry
613 scratch16 = MagneticPacketID;
614 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
615 }
616
617 // [12-11]: int16_t: variable 1 to be transmitted this iteration
618 // [14-13]: int16_t: variable 2 to be transmitted this iteration
619 // [16-15]: int16_t: variable 3 to be transmitted this iteration
620 switch (MagneticPacketID)
621 {
622 case 0:
623 // item 1: currently unused
624 scratch16 = 0;
625 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
626
627 // item 2: currently unused
628 scratch16 = 0;
629 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
630
631 // item 3: magnetic inclination angle with resolution 0.1 deg
632 scratch16 = iDelta;
633 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
634 break;
635
636 case 1:
637 // items 1 to 3: hard iron components range -3276uT to +3276uT encoded with 0.1uT resolution
638 scratch16 = (int16_t) (sfg->MagCal.fV[CHX] * 10.0F);
639 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
640 scratch16 = (int16_t) (sfg->MagCal.fV[CHY] * 10.0F);
641 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
642 scratch16 = (int16_t) (sfg->MagCal.fV[CHZ] * 10.0F);
643 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
644 break;
645
646 case 2:
647 // items 1 to 3: diagonal soft iron range -32. to +32. encoded with 0.001 resolution
648 scratch16 = (int16_t) (sfg->MagCal.finvW[CHX][CHX] * 1000.0F);
649 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
650 scratch16 = (int16_t) (sfg->MagCal.finvW[CHY][CHY] * 1000.0F);
651 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
652 scratch16 = (int16_t) (sfg->MagCal.finvW[CHZ][CHZ] * 1000.0F);
653 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
654 break;
655
656 case 3:
657 // items 1 to 3: off-diagonal soft iron range -32. to +32. encoded with 0.001 resolution
658 scratch16 = (int16_t) (sfg->MagCal.finvW[CHX][CHY] * 1000.0F);
659 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
660 scratch16 = (int16_t) (sfg->MagCal.finvW[CHX][CHZ] * 1000.0F);
661 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
662 scratch16 = (int16_t) (sfg->MagCal.finvW[CHY][CHZ] * 1000.0F);
663 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
664 break;
665
666 case 4:
667 case 5:
668 case 6:
669 case 7:
670 case 8:
671 case 9:
672 // cases 4 to 9 inclusive are for future expansion so transmit zeroes for now
673 OutputBufAppendZeros(output_buf, &iIndex, 3);
674 break;
675
676 default:
677 // 10 and upwards: this handles the magnetic buffer elements
678 OutputBufAppendItem(output_buf, &iIndex,
679 (uint8_t *) &(sfg->MagBuffer.iBs[CHX][i][j]), 2);
680 OutputBufAppendItem(output_buf, &iIndex,
681 (uint8_t *) &(sfg->MagBuffer.iBs[CHY][i][j]), 2);
682 OutputBufAppendItem(output_buf, &iIndex,
683 (uint8_t *) &(sfg->MagBuffer.iBs[CHZ][i][j]), 2);
684 break;
685 }
686
687 // wrap the variable ID back to zero if necessary
688 MagneticPacketID++;
689 if (MagneticPacketID >= (10 + MAGBUFFSIZEX * MAGBUFFSIZEY))
690 MagneticPacketID = 0;
691
692 // [17]: add the tail byte for the magnetic packet type 6
693 output_buf[iIndex++] = 0x7E;
694 }
695#endif
696
697 // *******************************************************************************
698 // Kalman filter packet type 7
699 // total bytes for packet type 7 is range 0 to 41 inclusive = 42 bytes
700 // this packet is only transmitted when a Kalman algorithm is computed
701 // and then non-zero data is transmitted only when a Kalman quaternion is selected
702 // *******************************************************************************
703 bool kalman = false;
704#if F_6DOF_GY_KALMAN
705 uint8_t six_axis_kalman = (sfg->iFlags & F_6DOF_GY_KALMAN) && (quaternionPacketType == Q6AG);
706 kalman = six_axis_kalman;
707#endif
708#if F_9DOF_GBY_KALMAN
709 uint8_t nine_axis_kalman = (sfg->iFlags & F_9DOF_GBY_KALMAN) && (quaternionPacketType == Q9);
710 kalman = kalman | nine_axis_kalman;
711#endif
712#if F_6DOF_GY_KALMAN || F_9DOF_GBY_KALMAN
713 if (kalman)
714 {
715 if ((quaternionPacketType == Q6AG) || (quaternionPacketType == Q9))
716 {
717 // [0]: packet start byte
718 output_buf[iIndex++] = 0x7E;
719
720 // [1]: packet type 7 byte
721 tmpuint8_t = 0x07;
722 OutputBufAppendItem(output_buf, &iIndex, &tmpuint8_t, 1);
723
724 // [2]: packet number byte
725 OutputBufAppendItem(output_buf, &iIndex, &iPacketNumber, 1);
726 iPacketNumber++;
727
728 // [4-3]: fzgErr[CHX] resolution scaled by 30000
729 // [6-5]: fzgErr[CHY] resolution scaled by 30000
730 // [8-7]: fzgErr[CHZ] resolution scaled by 30000
731 for (i = CHX; i <= CHZ; i++)
732 {
733#if F_6DOF_GY_KALMAN
734 if (six_axis_kalman) scratch16 = (int16_t) (sfg->SV_6DOF_GY_KALMAN.fZErr[i] * 30000.0F);
735#endif
736#if F_9DOF_GBY_KALMAN
737 if (nine_axis_kalman) scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fZErr[i] * 30000.0F);
738#endif
739 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16,2);
740 }
741
742 // [10-9]: fgErrPl[CHX] resolution scaled by 30000
743 // [12-11]: fgErrPl[CHY] resolution scaled by 30000
744 // [14-13]: fgErrPl[CHZ] resolution scaled by 30000
745 for (i = CHX; i <= CHZ; i++)
746 {
747#if F_6DOF_GY_KALMAN
748 if (six_axis_kalman) scratch16 = (int16_t) (sfg->SV_6DOF_GY_KALMAN.fqgErrPl[i] * 30000.0F);
749#endif
750#if F_9DOF_GBY_KALMAN
751 if (nine_axis_kalman) scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fqgErrPl[i] * 30000.0F);
752#endif
753 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16,2);
754 }
755
756 // [16-15]: fzmErr[CHX] resolution scaled by 30000
757 // [18-17]: fzmErr[CHY] resolution scaled by 30000
758 // [20-19]: fzmErr[CHZ] resolution scaled by 30000
759 for (i = CHX; i <= CHZ; i++)
760 {
761#if F_6DOF_GY_KALMAN
762 if (six_axis_kalman) scratch16 = 0;
763#endif
764#if F_9DOF_GBY_KALMAN
765 if (nine_axis_kalman) scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fZErr[i + 3] * 30000.0F);
766#endif
767 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
768 }
769
770 // [22-21]: fmErrPl[CHX] resolution scaled by 30000
771 // [24-23]: fmErrPl[CHY] resolution scaled by 30000
772 // [26-25]: fmErrPl[CHZ] resolution scaled by 30000
773 for (i = CHX; i <= CHZ; i++)
774 {
775#if F_6DOF_GY_KALMAN
776 if (six_axis_kalman) scratch16 = 0;
777#endif
778#if F_9DOF_GBY_KALMAN
779 if (nine_axis_kalman) scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fqmErrPl[i] * 30000.0F);
780#endif
781 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
782 }
783
784 // [28-27]: fbPl[CHX] resolution 0.001 deg/sec
785 // [30-29]: fbPl[CHY] resolution 0.001 deg/sec
786 // [32-31]: fbPl[CHZ] resolution 0.001 deg/sec
787 for (i = CHX; i <= CHZ; i++)
788 {
789#if F_6DOF_GY_KALMAN
790 if (six_axis_kalman) scratch16 = (int16_t) (sfg->SV_6DOF_GY_KALMAN.fbPl[i] * 1000.0F);
791#endif
792#if F_9DOF_GBY_KALMAN
793 if (nine_axis_kalman) scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fbPl[i] * 1000.0F);
794#endif
795 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
796 }
797
798 // [34-33]: fDeltaPl resolution 0.01deg
799 scratch16 = 0;
800#if F_9DOF_GBY_KALMAN
801 if (nine_axis_kalman) scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fDeltaPl * 100.0F);
802#endif
803 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
804
805 // [36-35]: fAccGl[CHX] resolution 1/8192 g
806 // [38-37]: fAccGl[CHY] resolution 1/8192 g
807 // [40-39]: fAccGl[CHZ] resolution 1/8192 g
808 for (i = CHX; i <= CHZ; i++)
809 {
810 // default to zero data
811 ftmp = 0.0F;
812#if F_6DOF_GY_KALMAN
813 if (six_axis_kalman) ftmp = sfg->SV_6DOF_GY_KALMAN.fAccGl[i] * 8192.0F;
814#endif
815#if F_9DOF_GBY_KALMAN
816 if (nine_axis_kalman) ftmp = sfg->SV_9DOF_GBY_KALMAN.fAccGl[i] * 8192.0F;
817#endif
818
819 // check for clipping
820 if (ftmp > 32767.0F) scratch16 = 32767;
821 else if (ftmp < -32768.0F) scratch16 = -32768;
822 else scratch16 = (int16_t) ftmp;
823 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
824 }
825
826 // [42-41]: fDisGl[CHX] resolution 0.01m
827 // [44-43]: fDisGl[CHY] resolution 0.01m
828 // [46-45]: fDisGl[CHZ] resolution 0.01m
829 for (i = CHX; i <= CHZ; i++)
830 {
831 // default to zero data
832 ftmp = 0.0F;
833#if F_9DOF_GBY_KALMAN
834 if (nine_axis_kalman) ftmp = sfg->SV_9DOF_GBY_KALMAN.fDisGl[i] * 100.0F;
835#endif
836
837 // check for clipping
838 if (ftmp > 32767.0F) scratch16 = 32767;
839 else if (ftmp < -32768.0F) scratch16 = -32768;
840 else scratch16 = (int16_t) ftmp;
841 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
842 }
843
844 // [47]: add the tail byte for the Kalman packet type 7
845 output_buf[iIndex++] = 0x7E;
846 }
847 } // end of check for Kalman packet
848#endif
849#if F_USING_ACCEL
850 // *************************************************************************
851 // fixed length packet type 8 transmitted whenever a precision accelerometer
852 // measurement has been stored.
853 // total size is 0 to 40 equals 41 bytes
854 // *************************************************************************
855 // check to see which packet (if any) is to be transmitted
856 if (AccelCalPacketOn != -1)
857 {
858 // [0]: packet start byte (need a iIndex++ here since not using OutputBufAppendItem)
859 output_buf[iIndex++] = 0x7E;
860
861 // [1]: packet type 8 byte (iIndex is automatically updated in OutputBufAppendItem)
862 tmpuint8_t = 0x08;
863 OutputBufAppendItem(output_buf, &iIndex, &tmpuint8_t, 1);
864
865 // [2]: packet number byte
866 OutputBufAppendItem(output_buf, &iIndex, &iPacketNumber, 1);
867 iPacketNumber++;
868
869 // [3]: AccelCalPacketOn in range 0-11 denotes stored location and MAXORIENTATIONS denotes transmit
870 // precision accelerometer calibration on power on before any measurements have been obtained.
871 OutputBufAppendItem(output_buf, &iIndex,
872 (uint8_t *) &(AccelCalPacketOn), 1);
873
874 // [9-4]: stored accelerometer measurement fGs (scaled to 8192 counts per g)
875 if ((AccelCalPacketOn >= 0) &&
876 (AccelCalPacketOn < MAX_ACCEL_CAL_ORIENTATIONS))
877 {
878 scratch16 = (int16_t) (sfg->AccelBuffer.fGsStored[AccelCalPacketOn][CHX] * 8192.0F);
879 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
880 scratch16 = (int16_t) (sfg->AccelBuffer.fGsStored[AccelCalPacketOn][CHY] * 8192.0F);
881 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
882 scratch16 = (int16_t) (sfg->AccelBuffer.fGsStored[AccelCalPacketOn][CHZ] * 8192.0F);
883 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
884 }
885 else
886 {
887 // transmit zero bytes since this is the power on or reset transmission of the precision calibration
888 OutputBufAppendZeros(output_buf, &iIndex, 3);
889 }
890
891 // [15-10]: precision accelerometer offset vector fV (g scaled by 32768.0)
892 scratch16 = (int16_t) (sfg->AccelCal.fV[CHX] * 32768.0F);
893 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
894 scratch16 = (int16_t) (sfg->AccelCal.fV[CHY] * 32768.0F);
895 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
896 scratch16 = (int16_t) (sfg->AccelCal.fV[CHZ] * 32768.0F);
897 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
898
899 // [21-16]: precision accelerometer inverse gain matrix diagonal finvW - 1.0 (scaled by 10000.0)
900 scratch16 = (int16_t) ((sfg->AccelCal.finvW[CHX][CHX] - 1.0F) * 10000.0F);
901 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
902 scratch16 = (int16_t) ((sfg->AccelCal.finvW[CHY][CHY] - 1.0F) * 10000.0F);
903 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
904 scratch16 = (int16_t) ((sfg->AccelCal.finvW[CHZ][CHZ] - 1.0F) * 10000.0F);
905 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
906
907 // [27-22]: precision accelerometer inverse gain matrix off-diagonal finvW (scaled by 10000)
908 scratch16 = (int16_t) (sfg->AccelCal.finvW[CHX][CHY] * 10000.0F);
909 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
910 scratch16 = (int16_t) (sfg->AccelCal.finvW[CHX][CHZ] * 10000.0F);
911 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
912 scratch16 = (int16_t) (sfg->AccelCal.finvW[CHY][CHZ] * 10000.0F);
913 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
914
915 // [33-28]: precision accelerometer rotation matrix diagonal fR0 (scaled by 10000)
916 scratch16 = (int16_t) (sfg->AccelCal.fR0[CHX][CHX] * 10000.0F);
917 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
918 scratch16 = (int16_t) (sfg->AccelCal.fR0[CHY][CHY] * 10000.0F);
919 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
920 scratch16 = (int16_t) (sfg->AccelCal.fR0[CHZ][CHZ] * 10000.0F);
921 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
922
923 // [45-34]: precision accelerometer inverse rotation matrix off-diagonal fR0 (scaled by 10000)
924 scratch16 = (int16_t) (sfg->AccelCal.fR0[CHX][CHY] * 10000.0F);
925 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
926 scratch16 = (int16_t) (sfg->AccelCal.fR0[CHX][CHZ] * 10000.0F);
927 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
928 scratch16 = (int16_t) (sfg->AccelCal.fR0[CHY][CHX] * 10000.0F);
929 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
930 scratch16 = (int16_t) (sfg->AccelCal.fR0[CHY][CHZ] * 10000.0F);
931 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
932 scratch16 = (int16_t) (sfg->AccelCal.fR0[CHZ][CHX] * 10000.0F);
933 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
934 scratch16 = (int16_t) (sfg->AccelCal.fR0[CHZ][CHY] * 10000.0F);
935 OutputBufAppendItem(output_buf, &iIndex, (uint8_t *) &scratch16, 2);
936
937 // [46]: add the tail byte for the packet type 8
938 output_buf[iIndex++] = 0x7E;
939
940 // disable future packets of this type until a new measurement has been obtained
941 sfg->pControlSubsystem->AccelCalPacketOn = -1;
942 }
943#endif // F_USING_ACCEL
944 // ********************************************************************************
945 // all packets have now been constructed in the output buffer.
946 // The final iIndex++ gives the number of bytes to transmit which is one more than
947 // the last index in the buffer.
948 // ********************************************************************************
949 sfg->pControlSubsystem->bytes_to_send = iIndex;
950
951 return;
952}
board.h
Board configuration file.
build.h
Build configuration file.
F_9DOF_GBY_KALMAN
#define F_9DOF_GBY_KALMAN
9DOF accel, mag and gyro algorithm selector - 0x4000 to include, 0x0000 otherwise/*#end#*‍/
Definition build.h:72
THISBUILD
#define THISBUILD
define build number sent in debug packet for display purposes only
Definition build.h:24
F_6DOF_GY_KALMAN
#define F_6DOF_GY_KALMAN
6DOF accel and gyro (Kalman) algorithm selector - 0x2000 to include, 0x0000 otherwise/*#end#*‍/
Definition build.h:70
FUSION_HZ
#define FUSION_HZ
(int) rate of fusion algorithm execution
Definition build.h:85
F_1DOF_P_BASIC
#define F_1DOF_P_BASIC
1DOF pressure (altitude) and temperature algorithm selector - 0x0100 to include, 0x0000 otherwise/*#e...
Definition build.h:60
F_USING_MAG
#define F_USING_MAG
nominally 0x0002 if an magnetometer is to be used, 0x0000 otherwise
Definition build.h:49
F_USING_ACCEL
#define F_USING_ACCEL
nominally 0x0001 if an accelerometer is to be used, 0x0000 otherwise
Definition build.h:48
F_3DOF_Y_BASIC
#define F_3DOF_Y_BASIC
3DOF gyro integration algorithm selector - 0x0800 to include, 0x0000 otherwise/*#end#*‍/
Definition build.h:66
F_6DOF_GB_BASIC
#define F_6DOF_GB_BASIC
6DOF accel and mag eCompass algorithm selector - 0x1000 to include, 0x0000 otherwise/*#end#*‍/
Definition build.h:68
F_3DOF_G_BASIC
#define F_3DOF_G_BASIC
3DOF accel tilt (accel) algorithm selector - 0x0200 to include, 0x0000 otherwise/*#end#*‍/
Definition build.h:62
F_USING_GYRO
#define F_USING_GYRO
nominally 0x0004 if a gyro is to be used, 0x0000 otherwise
Definition build.h:50
F_3DOF_B_BASIC
#define F_3DOF_B_BASIC
3DOF mag eCompass (vehicle/mag) algorithm selector - 0x0400 to include, 0x0000 otherwise/*#end#*‍/
Definition build.h:64
control.h
Defines control sub-system.
CreateOutgoingPackets
void CreateOutgoingPackets(SensorFusionGlobals *sfg)
Definition control_output.c:118
OutputBufAppendItem
void OutputBufAppendItem(uint8_t *pDest, uint16_t *pIndex, uint8_t *pSource, uint16_t iBytesToCopy)
Utility function used to place data in output buffer about to be transmitted via UART.
Definition control_output.c:34
fusion_testing.h
ApplyPerturbation function used to analyze dynamic performance.
MAGBUFFSIZEX
#define MAGBUFFSIZEX
x dimension in magnetometer buffer (14x28 equals 392 elements)
Definition magnetic.h:32
MAGBUFFSIZEY
#define MAGBUFFSIZEY
y dimension in magnetometer buffer (14x28 equals 392 elements)
Definition magnetic.h:33
MAX_ACCEL_CAL_ORIENTATIONS
#define MAX_ACCEL_CAL_ORIENTATIONS
number of stored precision accelerometer measurements
Definition precisionAccelerometer.h:24
sensor_fusion.h
The sensor_fusion.h file implements the top level programming interface.
quaternion_type
enum quaternion quaternion_type
the quaternion type to be transmitted
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
CHY
#define CHY
Used to access Y-channel entries in various data data structures.
Definition sensor_fusion.h:55
CHZ
#define CHZ
Used to access Z-channel entries in various data data structures.
Definition sensor_fusion.h:56
MagBuffer::index
int32_t index[MAGBUFFSIZEX][MAGBUFFSIZEY]
array of time indices
Definition magnetic.h:54
MagBuffer::iMagBufferCount
int16_t iMagBufferCount
number of magnetometer readings
Definition magnetic.h:56
MagBuffer::iBs
int16_t iBs[3][MAGBUFFSIZEX][MAGBUFFSIZEY]
uncalibrated magnetometer readings
Definition magnetic.h:53
MagCalibration::fB
float fB
current geomagnetic field magnitude (uT)
Definition magnetic.h:65
MagCalibration::fFitErrorpc
float fFitErrorpc
current fit error %
Definition magnetic.h:67
MagCalibration::fV
float fV[3]
current hard iron offset x, y, z, (uT)
Definition magnetic.h:63
MagCalibration::finvW
float finvW[3][3]
current inverse soft iron matrix
Definition magnetic.h:64
MagSensor::iBc
int16_t iBc[3]
averaged calibrated measurement (counts)
Definition sensor_fusion.h:224
MagSensor::iCountsPeruT
int16_t iCountsPeruT
counts per uT
Definition sensor_fusion.h:225
Quaternion
quaternion structure definition
Definition orientation.h:25
Quaternion::q3
float q3
z vector component
Definition orientation.h:29
Quaternion::q0
float q0
scalar component
Definition orientation.h:26
Quaternion::q1
float q1
x vector component
Definition orientation.h:27
Quaternion::q2
float q2
y vector component
Definition orientation.h:28
SV_COMMON
Definition sensor_fusion.h:440
SV_COMMON::systick
int32_t systick
systick timer;
Definition sensor_fusion.h:450
SV_COMMON::fq
Quaternion fq
orientation quaternion
Definition sensor_fusion.h:447
SV_COMMON::fOmega
float fOmega[3]
average angular velocity (deg/s)
Definition sensor_fusion.h:449
SV_COMMON::fPhi
float fPhi
roll (deg)
Definition sensor_fusion.h:441
SV_COMMON::fThe
float fThe
pitch (deg)
Definition sensor_fusion.h:442
SV_COMMON::fRho
float fRho
compass (deg)
Definition sensor_fusion.h:444
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::Mag
struct MagSensor Mag
magnetometer storage
Definition sensor_fusion.h:495
SensorFusionGlobals::iFlags
uint32_t iFlags
a bit-field of sensors and algorithms used
Definition sensor_fusion.h:473
Generated on Wed Feb 18 2026 09:42:29 for Sensor Fusion Library by doxygen 1.9.8