The library needs to be tested once we get the IMU
Diff: BNO080.cpp
- Revision:
- 0:f677e13975d0
- Child:
- 1:aac28ffd63ed
diff -r 000000000000 -r f677e13975d0 BNO080.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BNO080.cpp Sun Dec 23 05:23:21 2018 +0000 @@ -0,0 +1,743 @@ +// +// USC RPL BNO080 driver. +// + +/* + * Overview of BNO080 Communications + * =============================================== + * + * Hilcrest has developed a protocol called SHTP (Sensor Hub Transport Protocol) for binary communications with + * the BNO080 and the other IMUs it sells. Over this protocol, SH-2 (Sensor Hub 2) messages are sent to configure + * the chip and read data back. + * + * SHTP messages are divided at two hierarchical levels: first the channel, then the report ID. Each category + * of messages (system commands, sensor data reports, etc.) has its own channel, and the individual messages + * in each channel are identified by their report id, which is the first byte of the message payload (note that the + * datasheets don't *always* call the first byte the report ID, but that byte does identify the report, so I'm going + * with it). + * + * =============================================== + * + * Information about the BNO080 is split into three datasheets. Here's the download links and what they cover: + * + * - the BNO080 datasheet: http://www.hillcrestlabs.com/download/5a05f340566d07c196001ec1 + * -- Chip pinouts + * -- Example circuits + * -- Physical specifications + * -- Supported reports and configuration settings (at a high level) + * -- List of packets on the SHTP executable channel + * + * - the SHTP protocol: http://www.hillcrestlabs.com/download/59de8f99cd829e94dc0029d7 + * -- SHTP transmit and receive protcols (for SPI, I2C, and UART) + * -- SHTP binary format + * -- packet types on the SHTP command channel + * + * - the SH-2 reference: http://www.hillcrestlabs.com/download/59de8f398934bf6faa00293f + * -- list of packets and their formats for all channels other than command and executable + * -- list of FRS (Flash Record System) entries and their formats + * + * =============================================== + * + * Overview of SHTP channels: + * + * 0 -> Command + * -- Used for protocol-global packets, currently only the advertisement packet (which lists all the channels) and error reports + * + * 1 -> Executable + * -- Used for things that control the software on the chip: commands to reset and sleep + * -- Also used by the chip to report when it's done booting up + * + * 2 -> Control + * -- Used to send configuration commands to the IMU and for it to send back responses. + * -- Common report IDs: Command Request (0xF2), Set Feature (0xFD) + * + * 3 -> Sensor Reports + * -- Used for sensors to send back data reports. + * -- AFAIK the only report ID on this channel will be 0xFB (Report Base Timestamp); sensor data is send in a series of structures + * following an 0xFB + * + * 4 -> Wake Sensor Reports + * -- same as above, but for sensors configured to wake the device + * + * 5 -> Gyro Rotation Vector + * -- a dedicated channel for the Gyro Rotation Vector sensor report + * -- Why does this get its own channel? I don't know!!! + */ + +#include "BNO080.h" +#include "BNO080Constants.h" + +/// Set to 1 to enable debug printouts. Should be very useful if the chip is giving you trouble. +/// When debugging, it is recommended to use the highest possible serial baudrate so as not to interrupt the timing of operations. +#define BNO_DEBUG 1 + +BNO080::BNO080(Serial *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin, + uint8_t i2cAddress, int i2cPortSpeed) : + _debugPort(debugPort), + _i2cPort(user_SDApin, user_SCLpin), + _i2cAddress(i2cAddress), + _int(user_INTPin), + _rst(user_RSTPin, 1), + _scope(p21, 1) +{ + //Get user settings + _i2cPortSpeed = i2cPortSpeed; + if(_i2cPortSpeed > 4000000) + { + _i2cPortSpeed = 4000000; //BNO080 max is 400Khz + } + _i2cPort.frequency(_i2cPortSpeed); + +} + +bool BNO080::begin() +{ + //Configure the BNO080 for SPI communication + + _rst = 0; // Reset BNO080 + wait(.002f); // Min length not specified in datasheet? + _rst = 1; // Bring out of reset + + // wait for a falling edge (NOT just a low) on the INT pin to denote startup + Timer timeoutTimer; + + bool highDetected = false; + bool lowDetected = false; + + while(true) + { + if(timeoutTimer.read() > BNO080_RESET_TIMEOUT) + { + _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n"); + return false; + } + + // simple edge detector + if(!highDetected) + { + if(_int == 1) + { + highDetected = true; + } + } + else if(!lowDetected) + { + if(_int == 0) + { + lowDetected = true; + } + } + else + { + // high and low detected + break; + } + } + + _debugPort->printf("BNO080 detected!\n"); + + // At system startup, the hub must send its full advertisement message (see SHTP 5.2 and 5.3) to the + // host. It must not send any other data until this step is complete. + // We don't actually care what's in it, we're just using it as a signal to indicate that the reset is complete. + receivePacket(); + + // now, after startup, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command + waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET); + + // Next, officially tell it to initialize, and wait for a successful Initialize Response + zeroBuffer(); + shtpData[3] = 0; + _scope = 0; + sendCommand(COMMAND_INITIALIZE); + + + if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE) || shtpData[2] != COMMAND_INITIALIZE || shtpData[5] != 0) + { + _debugPort->printf("BNO080 reports initialization failed.\n"); + __enable_irq(); + return false; + } + else + { +#if BNO_DEBUG + _debugPort->printf("BNO080 reports initialization successful!\n"); +#endif + } + + + // Finally, we want to interrogate the device about its model and version. + zeroBuffer(); + shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info + shtpData[1] = 0; //Reserved + sendPacket(CHANNEL_CONTROL, 2); + + waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5); + + if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) + { + majorSoftwareVersion = shtpData[2]; + minorSoftwareVersion = shtpData[3]; + patchSoftwareVersion = (shtpData[13] << 8) | shtpData[12]; + partNumber = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | shtpData[4]; + buildNumber = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | shtpData[8]; + +#if BNO_DEBUG + _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n", + majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion, + buildNumber, partNumber); +#endif + + } + else + { + _debugPort->printf("Bad response from product ID command.\n"); + return false; + } + + // successful init + return true; + +} + +void BNO080::tare(bool zOnly) +{ + zeroBuffer(); + + // from SH-2 section 6.4.4.1 + shtpData[3] = 0; // perform tare now + + if(zOnly) + { + shtpData[4] = 0b100; // tare Z axis + } + else + { + shtpData[4] = 0b111; // tare X, Y, and Z axes + } + + shtpData[5] = 0; // reorient all motion outputs + + sendCommand(COMMAND_TARE); +} + +bool BNO080::updateData() +{ + if(_int.read() != 0) + { + // no waiting packets + return false; + } + + while(_int.read() == 0) + { + if(!receivePacket()) + { + // comms error + return false; + } + + processPacket(); + } + + // packets were received, so data may have changed + return true; +} + +uint8_t BNO080::getReportStatus(Report report) +{ + uint8_t reportNum = static_cast<uint8_t>(report); + if(reportNum > STATUS_ARRAY_LEN) + { + return 0; + } + + return reportStatus[reportNum]; +} + +//Sends the packet to enable the rotation vector +void BNO080::enableReport(Report report, uint16_t timeBetweenReports) +{ + setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports); + + // note: we don't wait for ACKs on these packets because they can take quite a while, like half a second, to come in +} + + +void BNO080::processPacket() +{ + if(shtpHeader[2] == CHANNEL_CONTROL) + { + // currently no command reports are read + } + else if(shtpHeader[2] == CHANNEL_EXECUTABLE) + { + // currently no executable reports are read + } + else if(shtpHeader[2] == CHANNEL_COMMAND) + { + + } + else if(shtpHeader[2] == CHANNEL_REPORTS || shtpHeader[2] == CHANNEL_WAKE_REPORTS) + { + if(shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP) + { + // sensor data packet + parseSensorDataPacket(); + } + } +} + +// sizes of various sensor data packet elements +#define SIZEOF_BASE_TIMESTAMP 5 +#define SIZEOF_TIMESTAMP_REBASE 5 +#define SIZEOF_ACCELEROMETER 10 +#define SIZEOF_LINEAR_ACCELERATION 10 +#define SIZEOF_GYROSCOPE_CALIBRATED 10 +#define SIZEOF_MAGNETIC_FIELD_CALIBRATED 10 +#define SIZEOF_ROTATION_VECTOR 14 +#define SIZEOF_GAME_ROTATION_VECTOR 12 +#define SIZEOF_GEOMAGNETIC_ROTATION_VECTOR 14 +#define SIZEOF_TAP_DETECTOR 5 + + +void BNO080::parseSensorDataPacket() +{ + size_t currReportOffset = 0; + + // every sensor data report first contains a timestamp offset to show how long it has been between when + // the host interrupt was sent and when the packet was transmitted. + // We don't use interrupts and don't care about times, so we can throw this out. + currReportOffset += SIZEOF_BASE_TIMESTAMP; + + while(currReportOffset < packetLength) + { + // lots of sensor reports use 3 16-bit numbers stored in bytes 4 through 9 + // we can save some time by parsing those out here. + uint16_t data1 = (uint16_t)shtpData[currReportOffset + 5] << 8 | shtpData[currReportOffset + 4]; + uint16_t data2 = (uint16_t)shtpData[currReportOffset + 7] << 8 | shtpData[currReportOffset + 6]; + uint16_t data3 = (uint16_t)shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8]; + + uint8_t reportNum = shtpData[currReportOffset]; + + if(reportNum != SENSOR_REPORTID_TIMESTAMP_REBASE) + { + // set status from byte 2 + reportStatus[reportNum] = static_cast<uint8_t>(shtpData[currReportOffset + 2] & 0b11); + } + + switch(shtpData[currReportOffset]) + { + case SENSOR_REPORTID_TIMESTAMP_REBASE: + currReportOffset += SIZEOF_TIMESTAMP_REBASE; + break; + + case SENSOR_REPORTID_ACCELEROMETER: + + totalAcceleration = TVector3( + qToFloat(data1, ACCELEROMETER_Q_POINT), + qToFloat(data2, ACCELEROMETER_Q_POINT), + qToFloat(data3, ACCELEROMETER_Q_POINT)); + + currReportOffset += SIZEOF_ACCELEROMETER; + break; + + case SENSOR_REPORTID_LINEAR_ACCELERATION: + + linearAcceleration = TVector3( + qToFloat(data1, ACCELEROMETER_Q_POINT), + qToFloat(data2, ACCELEROMETER_Q_POINT), + qToFloat(data3, ACCELEROMETER_Q_POINT)); + + currReportOffset += SIZEOF_LINEAR_ACCELERATION; + break; + + case SENSOR_REPORTID_GRAVITY: + + gravityAcceleration = TVector3( + qToFloat(data1, ACCELEROMETER_Q_POINT), + qToFloat(data2, ACCELEROMETER_Q_POINT), + qToFloat(data3, ACCELEROMETER_Q_POINT)); + + currReportOffset += SIZEOF_LINEAR_ACCELERATION; + break; + + case SENSOR_REPORTID_GYROSCOPE_CALIBRATED: + + gyroRotation = TVector3( + qToFloat(data1, GYRO_Q_POINT), + qToFloat(data2, GYRO_Q_POINT), + qToFloat(data3, GYRO_Q_POINT)); + + currReportOffset += SIZEOF_GYROSCOPE_CALIBRATED; + break; + + case SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED: + + magField = TVector3( + qToFloat(data1, MAGNETOMETER_Q_POINT), + qToFloat(data2, MAGNETOMETER_Q_POINT), + qToFloat(data3, MAGNETOMETER_Q_POINT)); + + currReportOffset += SIZEOF_MAGNETIC_FIELD_CALIBRATED; + break; + + case SENSOR_REPORTID_ROTATION_VECTOR: + { + uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; + uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12]; + + rotationVector = TVector4( + qToFloat(data1, ROTATION_Q_POINT), + qToFloat(data2, ROTATION_Q_POINT), + qToFloat(data3, ROTATION_Q_POINT), + qToFloat(realPartQ, ROTATION_Q_POINT)); + + rotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT); + + currReportOffset += SIZEOF_ROTATION_VECTOR; + } + break; + + case SENSOR_REPORTID_GAME_ROTATION_VECTOR: + { + uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; + + gameRotationVector = TVector4( + qToFloat(data1, ROTATION_Q_POINT), + qToFloat(data2, ROTATION_Q_POINT), + qToFloat(data3, ROTATION_Q_POINT), + qToFloat(realPartQ, ROTATION_Q_POINT)); + + currReportOffset += SIZEOF_GAME_ROTATION_VECTOR; + } + break; + + case SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR: + { + uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; + uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12]; + + geomagneticRotationVector = TVector4( + qToFloat(data1, ROTATION_Q_POINT), + qToFloat(data2, ROTATION_Q_POINT), + qToFloat(data3, ROTATION_Q_POINT), + qToFloat(realPartQ, ROTATION_Q_POINT)); + + geomagneticRotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT); + + currReportOffset += SIZEOF_GEOMAGNETIC_ROTATION_VECTOR; + } + break; + + case SENSOR_REPORTID_TAP_DETECTOR: + + // since we got the report, a tap was detected + tapDetected = true; + + doubleTap = (shtpData[currReportOffset + 4] & (1 << 6)) != 0; + + currReportOffset += SIZEOF_TAP_DETECTOR; + break; + + default: + _debugPort->printf("Error: unrecognized report ID in sensor report: %hhx. Byte %u, length %hu\n", shtpData[currReportOffset], currReportOffset, packetLength); + return; + } + } + +} + +bool BNO080::waitForPacket(int channel, uint8_t reportID, float timeout) +{ + Timer timeoutTimer; + timeoutTimer.start(); + + while(timeoutTimer.read() <= timeout) + { + if(_int.read() == 0) + { + if(!receivePacket(timeout)) + { + return false; + } + + if(channel == shtpHeader[2] && reportID == shtpData[0]) + { + // found correct packet! + return true; + } + else + { + // other data packet, send to proper channels + processPacket(); + } + } + } + + _debugPort->printf("Packet wait timeout.\n"); + return false; +} + +//Given a register value and a Q point, convert to float +//See https://en.wikipedia.org/wiki/Q_(number_format) +float BNO080::qToFloat(int16_t fixedPointValue, uint8_t qPoint) +{ + float qFloat = fixedPointValue; + qFloat *= pow(2, qPoint * -1); + return (qFloat); +} + +//Given a floating point value and a Q point, convert to Q +//See https://en.wikipedia.org/wiki/Q_(number_format) +int16_t BNO080::floatToQ(float qFloat, uint8_t qPoint) +{ + int16_t qVal = static_cast<int16_t>(qFloat * pow(2, qPoint)); + return qVal; +} + +//Tell the sensor to do a command +//See 6.3.8 page 41, Command request +//The caller is expected to set P0 through P8 prior to calling +void BNO080::sendCommand(uint8_t command) +{ + shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request + shtpData[1] = commandSequenceNumber++; //Increments automatically each function call + shtpData[2] = command; //Command + + //Caller must set these + /*shtpData[3] = 0; //P0 + shtpData[4] = 0; //P1 + shtpData[5] = 0; //P2 + shtpData[6] = 0; + shtpData[7] = 0; + shtpData[8] = 0; + shtpData[9] = 0; + shtpData[10] = 0; + shtpData[11] = 0;*/ + + //Transmit packet on channel 2, 12 bytes + sendPacket(CHANNEL_CONTROL, 12); +} + +//Given a sensor's report ID, this tells the BNO080 to begin reporting the values +//Also sets the specific config word. Useful for personal activity classifier +void BNO080::setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig) +{ + uint32_t microsBetweenReports = static_cast<uint32_t>(timeBetweenReports * 1000); + + const uint32_t batchMicros = 0; + + shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55 + shtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector + shtpData[2] = 0; //Feature flags + shtpData[3] = 0; //Change sensitivity (LSB) + shtpData[4] = 0; //Change sensitivity (MSB) + shtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms + shtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval + shtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval + shtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB) + shtpData[9] = (batchMicros >> 0) & 0xFF; //Batch Interval (LSB) + shtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval + shtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval + shtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB) + shtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB) + shtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config + shtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config + shtpData[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB) + + //Transmit packet on channel 2, 17 bytes + sendPacket(CHANNEL_CONTROL, 17); +} + + +//Given the data packet, send the header then the data +//Returns false if sensor does not ACK +bool BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength) +{ + // start the transaction and contact the IMU + _i2cPort.start(); + + // to indicate an i2c read, shift the 7 bit address up 1 bit and keep bit 0 as a 0 + int writeResult = _i2cPort.write(_i2cAddress << 1); + + if(writeResult != 1) + { + _debugPort->printf("BNO I2C write failed!\n"); + _scope = 0; + _i2cPort.stop(); + return false; + } + + + uint16_t totalLength = dataLength + 4; //Add four bytes for the header + packetLength = dataLength; + +#if BNO_DEBUG + shtpHeader[0] = totalLength & 0xFF; + shtpHeader[1] = totalLength >> 8; + shtpHeader[2] = channelNumber; + shtpHeader[3] = sequenceNumber[channelNumber]; + + _debugPort->printf("Transmitting packet: ----------------\n"); + printPacket(); +#endif + + //Send the 4 byte packet header + _i2cPort.write(totalLength & 0xFF); //Packet length LSB + _i2cPort.write(totalLength >> 8); //Packet length MSB + _i2cPort.write(channelNumber); //Channel number + _i2cPort.write(sequenceNumber[channelNumber]++); //Send the sequence number, increments with each packet sent, different counter for each channel + + //Send the user's data packet + for (uint8_t i = 0 ; i < dataLength ; i++) + { + _i2cPort.write(shtpData[i]); + } + _i2cPort.stop(); + + return (true); +} + +//Check to see if there is any new data available +//Read the contents of the incoming packet into the shtpData array +bool BNO080::receivePacket(float timeout) +{ + Timer waitStartTime; + waitStartTime.start(); + + while(_int.read() != 0) + { + if(waitStartTime.read() > timeout) + { + _debugPort->printf("BNO I2C wait timeout\n"); + return false; + } + + } + + // start the transaction and contact the IMU + _i2cPort.start(); + + // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1 + int writeResult = _i2cPort.write((_i2cAddress << 1) | 0x1); + + if(writeResult != 1) + { + _debugPort->printf("BNO I2C read failed!\n"); + return false; + } + + //Get the first four bytes, aka the packet header + uint8_t packetLSB = static_cast<uint8_t>(_i2cPort.read(true)); + uint8_t packetMSB = static_cast<uint8_t>(_i2cPort.read(true)); + uint8_t channelNumber = static_cast<uint8_t>(_i2cPort.read(true)); + uint8_t sequenceNum = static_cast<uint8_t>(_i2cPort.read(true)); //Not sure if we need to store this or not + + //Store the header info + shtpHeader[0] = packetLSB; + shtpHeader[1] = packetMSB; + shtpHeader[2] = channelNumber; + shtpHeader[3] = sequenceNum; + + if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF) + { + // invalid according to BNO080 datasheet section 1.4.1 + +#if BNO_DEBUG + _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n"); +#endif + return false; + } + + //Calculate the number of data bytes in this packet + packetLength = (static_cast<uint16_t>(packetMSB) << 8 | packetLSB); + + // Clear the MSbit. + // This bit indicates if this package is a continuation of the last. TBH, I don't really know what this means (it's not really explained in the datasheet) + // but we don't actually care about any of the advertisement packets + // that use this, so we can just cut off the rest of the packet by releasing chip select. + packetLength &= ~(1 << 15); + + if (packetLength == 0) + { + // Packet is empty + return (false); //All done + } + + packetLength -= 4; //Remove the header bytes from the data count + + //Read incoming data into the shtpData array + for (uint16_t dataSpot = 0 ; dataSpot < packetLength ; dataSpot++) + { + bool sendACK = dataSpot < packetLength - 1; + + // per the datasheet, 0xFF is used as filler for the receiver to transmit back + uint8_t incoming = static_cast<uint8_t>(_i2cPort.read(sendACK)); + if (dataSpot < STORED_PACKET_SIZE) //BNO080 can respond with upto 270 bytes, avoid overflow + shtpData[dataSpot] = incoming; //Store data into the shtpData array + } + + _i2cPort.stop(); + +#if BNO_DEBUG + _debugPort->printf("Recieved packet: ----------------\n"); + printPacket(); // note: add 4 for the header length +#endif + + return (true); //We're done! +} + +//Pretty prints the contents of the current shtp header and data packets +void BNO080::printPacket() +{ +#if BNO_DEBUG + //Print the four byte header + _debugPort->printf("Header:"); + for (uint8_t x = 0 ; x < 4 ; x++) + { + _debugPort->printf(" "); + if (shtpHeader[x] < 0x10) _debugPort->printf("0"); + _debugPort->printf("%hhx", shtpHeader[x]); + } + + uint16_t printLength = packetLength; + if (printLength > 40) printLength = 40; //Artificial limit. We don't want the phone book. + + _debugPort->printf(" Body:"); + for (uint16_t x = 0 ; x < printLength ; x++) + { + _debugPort->printf(" "); + if (shtpData[x] < 0x10) _debugPort->printf("0"); + _debugPort->printf("%hhx", shtpData[x]); + } + + _debugPort->printf(", Length:"); + _debugPort->printf("%hhu", packetLength + SHTP_HEADER_SIZE); + + if(shtpHeader[1] >> 7) + { + _debugPort->printf("[C]"); + } + + _debugPort->printf(", SeqNum: %hhu", shtpHeader[3]); + + _debugPort->printf(", Channel:"); + if (shtpHeader[2] == 0) _debugPort->printf("Command"); + else if (shtpHeader[2] == 1) _debugPort->printf("Executable"); + else if (shtpHeader[2] == 2) _debugPort->printf("Control"); + else if (shtpHeader[2] == 3) _debugPort->printf("Sensor-report"); + else if (shtpHeader[2] == 4) _debugPort->printf("Wake-report"); + else if (shtpHeader[2] == 5) _debugPort->printf("Gyro-vector"); + else _debugPort->printf("%hhu", shtpHeader[2]); + + _debugPort->printf("\n"); +#endif +} + + +void BNO080::zeroBuffer() +{ + memset(shtpHeader, 0, SHTP_HEADER_SIZE); + memset(shtpData, 0, STORED_PACKET_SIZE); + packetLength = 0; +}