File content as of revision 0:f677e13975d0:

//
// 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;
}