BNO080 Driver

Diff: BNO080.cpp

Revision:

1:aac28ffd63ed

Parent:

0:f677e13975d0

Child:

2:2269b723d16a

--- a/BNO080.cpp	Sun Dec 23 05:23:21 2018 +0000
+++ b/BNO080.cpp	Sat Dec 29 03:31:00 2018 -0800
@@ -1,743 +1,1275 @@
-//
-// 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;
-}
+//
+// 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)
+{
+	//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;
+	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::enableCalibration(bool calibrateAccel, bool calibrateGyro, bool calibrateMag)
+{
+	// send the Configure ME Calibration command
+	zeroBuffer();
+
+	shtpData[3] = static_cast<uint8_t>(calibrateAccel ? 1 : 0);
+	shtpData[4] = static_cast<uint8_t>(calibrateGyro ? 1 : 0);
+	shtpData[5] = static_cast<uint8_t>(calibrateMag ? 1 : 0);
+
+	shtpData[6] = 0; // Configure ME Calibration command
+
+	shtpData[7] = 0; // planar accelerometer calibration always disabled
+
+	sendCommand(COMMAND_ME_CALIBRATE);
+
+	// now, wait for the response
+	if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE))
+	{
+#if BNO_DEBUG
+		_debugPort->printf("Timeout waiting for calibration response!\n");
+#endif
+		return false;
+	}
+
+	if(shtpData[2] != COMMAND_ME_CALIBRATE)
+	{
+#if BNO_DEBUG
+		_debugPort->printf("Received wrong response to calibration command!\n");
+#endif
+		return false;
+	}
+
+	if(shtpData[5] != 0)
+	{
+#if BNO_DEBUG
+		_debugPort->printf("IMU reports calibrate command failed!\n");
+#endif
+		return false;
+	}
+
+	// acknowledge checks out!
+	return true;
+}
+
+bool BNO080::saveCalibration()
+{
+	zeroBuffer();
+
+	// no arguments
+	sendCommand(COMMAND_SAVE_DCD);
+
+	// now, wait for the response
+	if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE))
+	{
+#if BNO_DEBUG
+		_debugPort->printf("Timeout waiting for calibration response!\n");
+#endif
+		return false;
+	}
+
+	if(shtpData[2] != COMMAND_SAVE_DCD)
+	{
+#if BNO_DEBUG
+		_debugPort->printf("Received wrong response to calibration command!\n");
+#endif
+		return false;
+	}
+
+	if(shtpData[5] != 0)
+	{
+#if BNO_DEBUG
+		_debugPort->printf("IMU reports calibrate command failed!\n");
+#endif
+		return false;
+	}
+
+	// acknowledge checks out!
+	return true;
+}
+
+void BNO080::setSensorOrientation(Quaternion orientation)
+{
+	zeroBuffer();
+
+	_debugPort->printf("y: %f", orientation.y());
+
+	// convert floats to Q
+	int16_t Q_x = floatToQ(orientation.x(), ORIENTATION_QUAT_Q_POINT);
+	int16_t Q_y = floatToQ(orientation.y(), ORIENTATION_QUAT_Q_POINT);
+	int16_t Q_z = floatToQ(orientation.z(), ORIENTATION_QUAT_Q_POINT);
+	int16_t Q_w = floatToQ(orientation.w(), ORIENTATION_QUAT_Q_POINT);
+
+	_debugPort->printf("Q_y: %hd", Q_y);
+
+	shtpData[3] = 2; // set reorientation
+
+	shtpData[4] = static_cast<uint8_t>(Q_x & 0xFF); //P1 - X component LSB
+	shtpData[5] = static_cast<uint8_t>(Q_x >> 8); //P2 - X component MSB
+
+	shtpData[6] = static_cast<uint8_t>(Q_y & 0xFF); //P3 - Y component LSB
+	shtpData[7] = static_cast<uint8_t>(Q_y >> 8); //P4 - Y component MSB
+
+	shtpData[8] = static_cast<uint8_t>(Q_z & 0xFF); //P5 - Z component LSB
+	shtpData[9] = static_cast<uint8_t>(Q_z >> 8); //P6 - Z component MSB
+
+	shtpData[10] = static_cast<uint8_t>(Q_w & 0xFF); //P7 - W component LSB
+	shtpData[11] = static_cast<uint8_t>(Q_w >> 8); //P8 - W component MSB
+
+	//Using this shtpData packet, send a command
+	sendCommand(COMMAND_TARE); // Send tare command
+
+	// NOTE: unlike literally every other command, a sensor orientation command is never acknowledged in any way.
+}
+
+
+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];
+}
+
+const char* BNO080::getReportStatusString(Report report)
+{
+	switch(getReportStatus(report))
+	{
+		case 0:
+			return "Unreliable";
+		case 1:
+			return "Accuracy Low";
+		case 2:
+			return "Accuracy Medium";
+		case 3:
+			return "Accuracy High";
+		default:
+			return "Error";
+	}
+}
+
+bool BNO080::hasNewData(Report report)
+{
+	uint8_t reportNum = static_cast<uint8_t>(report);
+	if(reportNum > STATUS_ARRAY_LEN)
+	{
+		return false;
+	}
+
+	bool newData = reportHasBeenUpdated[reportNum];
+	reportHasBeenUpdated[reportNum] = false; // clear flag
+	return newData;
+}
+
+//Sends the packet to enable the rotation vector
+void BNO080::enableReport(Report report, uint16_t timeBetweenReports)
+{
+	// check time
+	float periodSeconds = timeBetweenReports / 1000.0;
+
+	if(periodSeconds < getMinPeriod(report))
+	{
+		_debugPort->printf("Error: attempt made to set report 0x%02hhx to period of %.06f s, which is smaller than its min period of %.06f s.\n",
+						   static_cast<uint8_t>(report), periodSeconds, getMinPeriod(report));
+		return;
+	}
+	/*
+	else if(getMaxPeriod(report) > 0 && periodSeconds > getMaxPeriod(report))
+	{
+		_debugPort->printf("Error: attempt made to set report 0x%02hhx to period of %.06f s, which is larger than its max period of %.06f s.\n",
+						   static_cast<uint8_t>(report), periodSeconds, getMaxPeriod(report));
+		return;
+	}
+	*/
+	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::disableReport(Report report)
+{
+	// set the report's polling period to zero to disable it
+	setFeatureCommand(static_cast<uint8_t>(report), 0);
+}
+
+uint32_t BNO080::getSerialNumber()
+{
+	uint32_t serNoBuffer;
+
+	if(!readFRSRecord(FRS_RECORDID_SERIAL_NUMBER, &serNoBuffer, 1))
+	{
+		return 0;
+	}
+
+	return serNoBuffer;
+}
+
+float BNO080::getRange(Report report)
+{
+	loadReportMetadata(report);
+
+	return qToFloat_dword(metadataRecord[1], getQ1(report));
+}
+
+
+float BNO080::getResolution(Report report)
+{
+	loadReportMetadata(report);
+
+	return qToFloat_dword(metadataRecord[2], getQ1(report));
+}
+
+float BNO080::getPower(Report report)
+{
+	loadReportMetadata(report);
+
+	uint16_t powerQ = static_cast<uint16_t>(metadataRecord[3] & 0xFFFF);
+
+	return qToFloat_dword(powerQ, POWER_Q_POINT);
+}
+
+float BNO080::getMinPeriod(Report report)
+{
+	loadReportMetadata(report);
+
+	return metadataRecord[4] / 1e6f; // convert from microseconds to seconds
+}
+
+float BNO080::getMaxPeriod(Report report)
+{
+	loadReportMetadata(report);
+
+	if(getMetaVersion() == 3)
+	{
+		// no max period entry in this record format
+		return -1.0f;
+	}
+
+	return metadataRecord[9] / 1e6f; // convert from microseconds to seconds
+}
+
+void BNO080::printMetadataSummary(Report report)
+{
+#if BNO_DEBUG
+	if(!loadReportMetadata(report))
+	{
+		_debugPort->printf("Failed to load report metadata!\n");
+	}
+
+	_debugPort->printf("======= Metadata for report 0x%02hhx =======\n", static_cast<uint8_t>(report));
+
+	_debugPort->printf("Range: +- %.04f units\n", getRange(report));
+	_debugPort->printf("Resolution: %.04f units\n", getResolution(report));
+	_debugPort->printf("Power Used: %.03f mA\n", getPower(report));
+	_debugPort->printf("Min Period: %.06f s\n", getMinPeriod(report));
+	_debugPort->printf("Max Period: %.06f s\n\n", getMaxPeriod(report));
+
+#endif
+}
+
+int16_t BNO080::getQ1(Report report)
+{
+	loadReportMetadata(report);
+
+	return static_cast<int16_t>(metadataRecord[7] & 0xFFFF);
+}
+
+int16_t BNO080::getQ2(Report report)
+{
+	loadReportMetadata(report);
+
+	return static_cast<int16_t>(metadataRecord[7] >> 16);
+}
+
+int16_t BNO080::getQ3(Report report)
+{
+	loadReportMetadata(report);
+
+	return static_cast<int16_t>(metadataRecord[8] >> 16);
+}
+
+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_MAGNETIC_FIELD_UNCALIBRATED 16
+#define SIZEOF_ROTATION_VECTOR 14
+#define SIZEOF_GAME_ROTATION_VECTOR 12
+#define SIZEOF_GEOMAGNETIC_ROTATION_VECTOR 14
+#define SIZEOF_TAP_DETECTOR 5
+#define SIZEOF_STABILITY_REPORT 6
+#define SIZEOF_STEP_DETECTOR 8
+#define SIZEOF_STEP_COUNTER 12
+#define SIZEOF_SIGNIFICANT_MOTION 6
+#define SIZEOF_SHAKE_DETECTOR 6
+
+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)
+	{
+		if(currReportOffset >= STORED_PACKET_SIZE)
+		{
+			_debugPort->printf("Error: sensor report longer than packet buffer!\n");
+			return;
+		}
+
+		// 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);
+
+			// set updated flag
+			reportHasBeenUpdated[reportNum] = true;
+		}
+
+		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_MAGNETIC_FIELD_UNCALIBRATED:
+			{
+				magFieldUncalibrated = TVector3(
+						qToFloat(data1, MAGNETOMETER_Q_POINT),
+						qToFloat(data2, MAGNETOMETER_Q_POINT),
+						qToFloat(data3, MAGNETOMETER_Q_POINT));
+
+				uint16_t ironOffsetXQ = shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
+				uint16_t ironOffsetYQ = shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12];
+				uint16_t ironOffsetZQ = shtpData[currReportOffset + 15] << 8 | shtpData[currReportOffset + 14];
+
+				hardIronOffset = TVector3(
+						qToFloat(ironOffsetXQ, MAGNETOMETER_Q_POINT),
+					  	qToFloat(ironOffsetYQ, MAGNETOMETER_Q_POINT),
+	  					qToFloat(ironOffsetZQ, MAGNETOMETER_Q_POINT));
+
+				currReportOffset += SIZEOF_MAGNETIC_FIELD_UNCALIBRATED;
+			}
+				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;
+
+			case SENSOR_REPORTID_STABILITY_CLASSIFIER:
+			{
+				uint8_t classificationNumber = shtpData[currReportOffset + 4];
+
+				if(classificationNumber > 4)
+				{
+					classificationNumber = 0;
+				}
+
+				stability = static_cast<Stability>(classificationNumber);
+
+				currReportOffset += SIZEOF_STABILITY_REPORT;
+			}
+				break;
+
+			case SENSOR_REPORTID_STEP_DETECTOR:
+
+				// the fact that we got the report means that a step was detected
+				stepDetected = true;
+
+				currReportOffset += SIZEOF_STEP_DETECTOR;
+
+				break;
+
+			case SENSOR_REPORTID_STEP_COUNTER:
+
+				stepCount = shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8];
+
+				currReportOffset += SIZEOF_STEP_COUNTER;
+
+				break;
+
+			case SENSOR_REPORTID_SIGNIFICANT_MOTION:
+
+				// the fact that we got the report means that significant motion was detected
+				significantMotionDetected = true;
+
+				currReportOffset += SIZEOF_SIGNIFICANT_MOTION;
+
+			case SENSOR_REPORTID_SHAKE_DETECTOR:
+
+				shakeDetected = true;
+
+				xAxisShake = (shtpData[currReportOffset + 4] & 1) != 0;
+				yAxisShake = (shtpData[currReportOffset + 4] & (1 << 1)) != 0;
+				zAxisShake = (shtpData[currReportOffset + 4] & (1 << 2)) != 0;
+
+				currReportOffset += SIZEOF_SHAKE_DETECTOR;
+
+			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);
+}
+
+float BNO080::qToFloat_dword(uint32_t fixedPointValue, int16_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);
+}
+
+bool BNO080::readFRSRecord(uint16_t recordID, uint32_t* readBuffer, uint16_t readLength)
+{
+	// send initial read request
+	zeroBuffer();
+
+	shtpData[0] = SHTP_REPORT_FRS_READ_REQUEST;
+	// read offset of 0 -> start at the start of the record
+	shtpData[2] = 0;
+	shtpData[3] = 0;
+	// record ID
+	shtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
+	shtpData[5] = static_cast<uint8_t>(recordID >> 8);
+	// block size
+	shtpData[6] = static_cast<uint8_t>(readLength & 0xFF);
+	shtpData[7] = static_cast<uint8_t>(readLength >> 8);
+
+	sendPacket(CHANNEL_CONTROL, 8);
+
+	// now, read back the responses
+	size_t readOffset = 0;
+	while(readOffset < readLength)
+	{
+		if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE))
+		{
+#if BNO_DEBUG
+			_debugPort->printf("Error: did not receive FRS read response after sending read request!\n");
+#endif
+			return false;
+		}
+
+		uint8_t status = static_cast<uint8_t>(shtpData[1] & 0b1111);
+		uint8_t dataLength = shtpData[1] >> 4;
+
+		// check status
+		if(status == 1)
+		{
+#if BNO_DEBUG
+			_debugPort->printf("Error: FRS reports invalid record ID!\n");
+#endif
+			return false;
+		}
+		else if(status == 2)
+		{
+#if BNO_DEBUG
+			_debugPort->printf("Error: FRS is busy!\n");
+#endif
+			return false;
+		}
+		else if(status == 4)
+		{
+#if BNO_DEBUG
+			_debugPort->printf("Error: FRS reports offset is out of range!\n");
+#endif
+			return false;
+		}
+		else if(status == 5)
+		{
+#if BNO_DEBUG
+			_debugPort->printf("Error: FRS reports record %hx is empty!\n", recordID);
+#endif
+			return false;
+		}
+		else if(status == 8)
+		{
+#if BNO_DEBUG
+			_debugPort->printf("Error: FRS reports flash memory device unavailable!\n");
+#endif
+			return false;
+		}
+
+		// check data length
+		if(dataLength == 0)
+		{
+#if BNO_DEBUG
+			_debugPort->printf("Error: Received FRS packet with 0 data length!\n");
+#endif
+			return false;
+		}
+		else if(dataLength == 1)
+		{
+			if(readOffset + 1 != readLength)
+			{
+#if BNO_DEBUG
+				_debugPort->printf("Error: Received 1 length packet but more than 1 byte remains to be be read!\n");
+#endif
+				return false;
+			}
+		}
+
+		// now, _finally_, read the dang words
+		readBuffer[readOffset] = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | (shtpData[4]);
+
+		// check if we only wanted the first word
+		++readOffset;
+		if(readOffset == readLength)
+		{
+			break;
+		}
+
+		readBuffer[readOffset] = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | (shtpData[8]);
+		readOffset++;
+	}
+
+	// read successful
+	return true;
+
+}
+
+//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");
+		_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;
+}
+
+bool BNO080::loadReportMetadata(BNO080::Report report)
+{
+	uint16_t reportMetaRecord;
+
+	// first, convert the report into the correct FRS record ID for that report's metadata
+	// data from SH-2 section 5.1
+	switch(report)
+	{
+		case Report::TOTAL_ACCELERATION:
+			reportMetaRecord = 0xE301;
+			break;
+		case Report::LINEAR_ACCELERATION:
+			reportMetaRecord = 0xE303;
+			break;
+		case Report::GRAVITY_ACCELERATION:
+			reportMetaRecord = 0xE304;
+			break;
+		case Report::GYROSCOPE:
+			reportMetaRecord = 0xE306;
+			break;
+		case Report::MAG_FIELD:
+			reportMetaRecord = 0xE309;
+			break;
+		case Report::MAG_FIELD_UNCALIBRATED:
+			reportMetaRecord = 0xE30A;
+			break;
+		case Report::ROTATION:
+			reportMetaRecord = 0xE30B;
+			break;
+		case Report::GEOMAGNETIC_ROTATION:
+			reportMetaRecord = 0xE30D;
+			break;
+		case Report::GAME_ROTATION:
+			reportMetaRecord = 0xE30C;
+			break;
+		case Report::TAP_DETECTOR:
+			reportMetaRecord = 0xE313;
+			break;
+		case Report::STABILITY_CLASSIFIER:
+			reportMetaRecord = 0xE317;
+			break;
+		case Report::STEP_DETECTOR:
+			reportMetaRecord = 0xE314;
+			break;
+		case Report::STEP_COUNTER:
+			reportMetaRecord = 0xE315;
+			break;
+		case Report::SIGNIFICANT_MOTION:
+			reportMetaRecord = 0xE316;
+			break;
+		case Report::SHAKE_DETECTOR:
+			reportMetaRecord = 0xE318;
+			break;
+	}
+
+	// if we already have that data stored, everything's OK
+	if(bufferMetadataRecord == reportMetaRecord)
+	{
+		return true;
+	}
+
+	// now, load the metadata into the buffer
+	if(!readFRSRecord(reportMetaRecord, metadataRecord, METADATA_BUFFER_LEN))
+	{
+		// clear this so future calls won't try to use the cached version
+		bufferMetadataRecord = 0;
+
+		return false;
+	}
+
+	bufferMetadataRecord = reportMetaRecord;
+
+	return true;
+}