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;
sBufAppendItem(uint8_t * pDest,uint16_t * pIndex,uint8_t * pSource,uint16_t iBytesToCopy)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
sBufAppendZeros(uint8_t * pDest,uint16_t * pIndex,uint16_t numZeros)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
readCommon(SV_ptr data,Quaternion * fq,int16_t * iPhi,int16_t * iThe,int16_t * iRho,int16_t iOmega[],uint16_t * isystick)78 void readCommon( SV_ptr data,
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
throttle()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"
CreateAndSendPackets(SensorFusionGlobals * sfg,uint8_t * sUARTOutputBuffer)122 void CreateAndSendPackets(SensorFusionGlobals *sfg, uint8_t *sUARTOutputBuffer)
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
925 sfg->pControlSubsystem->AccelCalPacketOn = -1;
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 }
936
