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