@@ -149,7 +149,7 @@ bool BNO080::dataAvailable(void)
149
149
if (digitalRead (_int) == HIGH)
150
150
return (false );
151
151
}
152
-
152
+
153
153
if (receivePacket () == true )
154
154
{
155
155
// Check to see if this packet is a sensor reporting its data to us
@@ -163,11 +163,11 @@ bool BNO080::dataAvailable(void)
163
163
parseCommandReport (); // This will update responses to commands, calibrationStatus, etc.
164
164
return (true );
165
165
}
166
- else if (shtpHeader[2 ] == CHANNEL_GYRO)
167
- {
168
- parseInputReport (); // This will update the rawAccelX, etc variables depending on which feature report is found
169
- return (true );
170
- }
166
+ else if (shtpHeader[2 ] == CHANNEL_GYRO)
167
+ {
168
+ parseInputReport (); // This will update the rawAccelX, etc variables depending on which feature report is found
169
+ return (true );
170
+ }
171
171
}
172
172
return (false );
173
173
}
@@ -232,12 +232,12 @@ void BNO080::parseInputReport(void)
232
232
int16_t dataLength = ((uint16_t )shtpHeader[1 ] << 8 | shtpHeader[0 ]);
233
233
dataLength &= ~(1 << 15 ); // Clear the MSbit. This bit indicates if this package is a continuation of the last.
234
234
// Ignore it for now. TODO catch this as an error and exit
235
-
235
+
236
236
dataLength -= 4 ; // Remove the header bytes from the data count
237
237
238
238
timeStamp = ((uint32_t )shtpData[4 ] << (8 * 3 )) | ((uint32_t )shtpData[3 ] << (8 * 2 )) | ((uint32_t )shtpData[2 ] << (8 * 1 )) | ((uint32_t )shtpData[1 ] << (8 * 0 ));
239
239
240
- // The gyro-integrated input reports are sent via the special gyro channel and do no include the usual ID, sequence, and status fields
240
+ // The gyro-integrated input reports are sent via the special gyro channel and do no include the usual ID, sequence, and status fields
241
241
if (shtpHeader[2 ] == CHANNEL_GYRO) {
242
242
rawQuatI = (uint16_t )shtpData[1 ] << 8 | shtpData[0 ];
243
243
rawQuatJ = (uint16_t )shtpData[3 ] << 8 | shtpData[2 ];
@@ -246,7 +246,7 @@ void BNO080::parseInputReport(void)
246
246
rawFastGyroX = (uint16_t )shtpData[9 ] << 8 | shtpData[8 ];
247
247
rawFastGyroY = (uint16_t )shtpData[11 ] << 8 | shtpData[10 ];
248
248
rawFastGyroZ = (uint16_t )shtpData[13 ] << 8 | shtpData[12 ];
249
-
249
+
250
250
return ;
251
251
}
252
252
@@ -295,8 +295,10 @@ void BNO080::parseInputReport(void)
295
295
rawMagY = data2;
296
296
rawMagZ = data3;
297
297
}
298
- else if (shtpData[5 ] == SENSOR_REPORTID_ROTATION_VECTOR || shtpData[5 ] == SENSOR_REPORTID_GAME_ROTATION_VECTOR ||
299
- shtpData[5 ] == SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR || shtpData[5 ] == SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR)
298
+ else if (shtpData[5 ] == SENSOR_REPORTID_ROTATION_VECTOR ||
299
+ shtpData[5 ] == SENSOR_REPORTID_GAME_ROTATION_VECTOR ||
300
+ shtpData[5 ] == SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR ||
301
+ shtpData[5 ] == SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR)
300
302
{
301
303
quatAccuracy = status;
302
304
rawQuatI = data1;
@@ -305,7 +307,7 @@ void BNO080::parseInputReport(void)
305
307
rawQuatReal = data4;
306
308
307
309
// Only available on rotation vector and ar/vr stabilized rotation vector,
308
- // not game rot vector and not ar/vr stabilized rotation vector
310
+ // not game rot vector and not ar/vr stabilized rotation vector
309
311
rawQuatRadianAccuracy = data5;
310
312
}
311
313
else if (shtpData[5 ] == SENSOR_REPORTID_STEP_COUNTER)
@@ -1214,13 +1216,13 @@ boolean BNO080::receivePacket(void)
1214
1216
uint8_t packetMSB = _spiPort->transfer (0 );
1215
1217
uint8_t channelNumber = _spiPort->transfer (0 );
1216
1218
uint8_t sequenceNumber = _spiPort->transfer (0 ); // Not sure if we need to store this or not
1217
-
1219
+
1218
1220
// Store the header info
1219
1221
shtpHeader[0 ] = packetLSB;
1220
1222
shtpHeader[1 ] = packetMSB;
1221
1223
shtpHeader[2 ] = channelNumber;
1222
1224
shtpHeader[3 ] = sequenceNumber;
1223
-
1225
+
1224
1226
// Calculate the number of data bytes in this packet
1225
1227
uint16_t dataLength = ((uint16_t )packetMSB << 8 | packetLSB);
1226
1228
dataLength &= ~(1 << 15 ); // Clear the MSbit.
@@ -1242,7 +1244,7 @@ boolean BNO080::receivePacket(void)
1242
1244
}
1243
1245
1244
1246
digitalWrite (_cs, HIGH); // Release BNO080
1245
-
1247
+
1246
1248
_spiPort->endTransaction ();
1247
1249
// printPacket();
1248
1250
}
0 commit comments