Revision 0:f677e13975d0, committed 2018-12-23

Comitter:

MultipleMonomials

Date:

Sun Dec 23 05:23:21 2018 +0000

Child:

1:aac28ffd63ed

Commit message:

Initial commit

Changed in this revision

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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BNO080.h	Sun Dec 23 05:23:21 2018 +0000
@@ -0,0 +1,453 @@
+/*
+ * This is USC RPL's ARM MBed BNO080 IMU driver, by Jamie Smith.
+ *
+ * It is based on SparkFun and Nathan Seidle's Arduino driver for this chip, but is substantially rewritten and adapted.
+ * It also supports some extra features, such as setting the mounting orientation and
+ * enabling some additional data reports.
+ *
+ * This driver uses no dynamic allocation, but does allocate a couple hundred bytes of class variables as buffers.
+ * This should allow you to monitor its memory usage using MBed's size printout.
+ *
+ * The BNO080 is a very complex chip; it's capable of monitoring and controlling other sensors and making
+ * intelligent decisions and calculations using its data.  Accordingly, the protocol for communicating with it
+ * is quite complex, and it took me quite a while to wrap my head around it.  If you need to modify or debug
+ * this driver, look at the CPP file for an overview of the chip's communication protocol.
+ *
+ * Note: this driver only supports I2C.  I attempted to create an SPI version, but as far as I can tell,
+ * the BNO's SPI interface has a bug that causes you to be unable to wake the chip from sleep in some conditions.
+ * Until this is fixed, SPI on it is virtually unusable.
+ */
+
+#ifndef HAMSTER_BNO080_H
+#define HAMSTER_BNO080_H
+
+#include <mbed.h>
+#include <quaternion.h>
+
+#include "BNO080Constants.h"
+
+class BNO080
+{
+    /**
+     * Serial stream to print debug info to.  Used for errors, and debugging output if debugging is enabled.
+     */
+    Serial * _debugPort;
+
+    /**
+     * I2C port object
+     */
+    I2C _i2cPort;
+
+    /// user defined port speed
+    int  _i2cPortSpeed;
+
+    /// i2c address of IMU (7 bits)
+    uint8_t _i2cAddress;
+
+    DigitalIn _int;
+    DigitalOut _rst;
+
+    DigitalOut _scope;
+
+    // packet storage
+    //-----------------------------------------------------------------------------------------------------------------
+
+#define SHTP_HEADER_SIZE 4
+#define STORED_PACKET_SIZE 128
+
+    /// Each packet has a header of 4 uint8_ts
+    uint8_t shtpHeader[SHTP_HEADER_SIZE];
+
+    /// Stores data contained in each packet.  Packets can contain an arbitrary amount of data, but we shouldn't need to read more than a few hundred bytes of them.
+    /// The only long packets we actually care about are batched sensor data packets, and with how this driver handles batching, we *should* only have to deal
+    /// with at most 9 reports at a time = ~90 bytes + a few bytes of padding
+    uint8_t shtpData[STORED_PACKET_SIZE];
+
+    /// Length of packet that was received into buffer.  Does NOT include header bytes.
+    uint16_t packetLength;
+
+    /// There are 6 com channels. Each channel has its own seqnum
+    uint8_t sequenceNumber[6] = {0, 0, 0, 0, 0, 0};
+
+    /// Commands have a seqNum as well. These are inside command packet, the header uses its own seqNum per channel
+    uint8_t commandSequenceNumber = 0;
+
+    // data storage
+    //-----------------------------------------------------------------------------------------------------------------
+
+    // 1 larger than the largest sensor report ID
+#define STATUS_ARRAY_LEN 0x1A
+
+    /// stores status of each sensor, indexed by report ID
+    uint8_t reportStatus[STATUS_ARRAY_LEN] = {};
+
+public:
+
+    /// List of all sensor reports that the IMU supports.
+    enum class Report : uint8_t
+    {
+        /**
+         * Total acceleration of the IMU in world space.
+         * See BNO datasheet section 2.1.1
+         */
+        TOTAL_ACCELERATION = SENSOR_REPORTID_ACCELEROMETER,
+
+        /**
+         * Acceleration of the IMU not including the acceleration of gravity.
+         * See BNO datasheet section 2.1.1
+         */
+        LINEAR_ACCELERATION = SENSOR_REPORTID_LINEAR_ACCELERATION,
+
+        /**
+         * Acceleration of gravity felt by the IMU.
+         * See BNO datasheet section 2.1.1
+         */
+        GRAVITY_ACCELERATION = SENSOR_REPORTID_GRAVITY,
+
+        /**
+         * (calibrated) gyroscope reading of the rotational speed of the IMU.
+         * See BNO datasheet section 2.1.2
+         */
+        GYROSCOPE = SENSOR_REPORTID_GYROSCOPE_CALIBRATED,
+
+        /**
+         * (calibrated) reading of magnetic field levels.
+         * See BNO datasheet section 2.1.3
+         */
+        MAG_FIELD = SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED,
+
+        /**
+         * Fused reading of the IMU's rotation in space using all three sensors.  This is the most accurate reading
+         * of absolute orientation that the IMU can provide.
+         * See BNO datasheet section 2.2.4
+         */
+        ROTATION = SENSOR_REPORTID_ROTATION_VECTOR,
+
+        /**
+         * Fused reading of rotation from accelerometer and magnetometer readings.  This report is designed to decrease
+         * power consumption (by turning off the gyroscope) in exchange for reduced responsiveness.
+         */
+        GEOMAGNETIC_ROTATION = SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR,
+
+        /**
+         * Fused reading of the IMU's rotation in space.  Unlike the regular rotation vector, the Game Rotation Vector
+         * is not referenced against the magnetic field and the "zero yaw" point is arbitrary.
+         * See BNO datasheet section 2.2.2
+         */
+        GAME_ROTATION = SENSOR_REPORTID_GAME_ROTATION_VECTOR,
+
+        /**
+         * Detects a user tapping on the device containing the IMU.
+         * See BNO datasheet section 2.4.2
+         */
+        TAP_DETECTOR = SENSOR_REPORTID_TAP_DETECTOR,
+
+        /**
+         * Detects whether the device is on a table, being held stably, or being moved.
+         * See BNO datasheet section 2.4.1
+         */
+        STABILITY_CLASSIFIER = SENSOR_REPORTID_STABILITY_CLASSIFIER,
+
+        /**
+         * Detects a user taking a step with the IMU worn on their person.
+         * See BNO datasheet section 2.4.3
+         */
+        STEP_DETECTOR = SENSOR_REPORTID_STEP_DETECTOR,
+
+        /**
+         * Detects how many steps a user has taken.
+         * See BNO datasheet section 2.4.4
+         */
+        STEP_COUNTER = SENSOR_REPORTID_STEP_COUNTER,
+
+        /**
+         * Detects when the IMU has made a "significant" motion, defined as moving a few steps and/or accelerating significantly.
+         * See BNO datasheet section 2.4.6
+         */
+        SIGNIFICANT_MOTION = SENSOR_REPORTID_SIGNIFICANT_MOTION,
+
+        /**
+         * Detects when the IMU is being shaken.
+         * See BNO datasheet section 2.4.7
+         */
+        SHAKE_DETECTOR = SENSOR_REPORTID_SHAKE_DETECTOR
+    };
+
+    // data variables to read reports from
+    //-----------------------------------------------------------------------------------------------------------------
+
+    // @{
+    /// Version info read from the IMU when it starts up
+    uint8_t majorSoftwareVersion;
+    uint8_t minorSoftwareVersion;
+    uint16_t patchSoftwareVersion;
+    uint32_t partNumber;
+    uint32_t buildNumber;
+    // @}
+
+
+    /**
+     * Readout from Accleration report.
+     * Represents total acceleration in m/s^2 felt by the BNO's accelerometer.
+     */
+    TVector3 totalAcceleration;
+
+    /**
+     * Readout from Linear Acceleration report.
+     * Represents acceleration felt in m/s^2 by the BNO's accelerometer not including the force of gravity.
+     */
+    TVector3 linearAcceleration;
+
+    /**
+     * Readout from Gravity report.
+     * Represents the force of gravity in m/s^2 felt by the BNO's accelerometer.
+     */
+    TVector3 gravityAcceleration;
+
+    /**
+     * Readout from Calibrated Gyroscope report
+     * Represents the angular velocities of the chip in rad/s in the X, Y, and Z axes
+     */
+    TVector3 gyroRotation;
+
+    /**
+     * Readout from the Magnetic Field Calibrated report.
+     * Represents the magnetic field read by the chip in uT in the X, Y, and Z axes
+     */
+    TVector3 magField;
+
+    /**
+     * Readout from the Rotation Vector report.
+     * Represents the rotation of the IMU (relative to magnetic north) in radians.
+     */
+    Quaternion rotationVector;
+
+    /**
+     * Auxillary accuracy readout from the Rotation Vector report.
+     * Represents the estimated accuracy of the rotation vector in radians.
+     */
+    float rotationAccuracy;
+
+    /**
+     * Readout from the Game Rotation Vector report.
+     * Represents the rotation of the IMU in radians.  Unlike the regular rotation vector, the Game Rotation Vector
+     * is not referenced against the magnetic field and the "zero yaw" point is arbitrary.
+     */
+    Quaternion gameRotationVector;
+
+    /**
+     * Readout from the Geomagnetic Rotation Vector report.
+     * Represents the geomagnetic rotation of the IMU (relative to magnetic north) in radians.
+     */
+    Quaternion geomagneticRotationVector;
+
+    /**
+     * Auxillary accuracy readout from the Geomagnetic Rotation Vector report.
+     * Represents the estimated accuracy of the rotation vector in radians.
+     */
+    float geomagneticRotationAccuracy;
+
+    /**
+     * Tap readout from the Tap Detector report.  This flag is set to true whenever a tap is detected, and you should
+     * manually clear it when you have processed the tap.
+     */
+    bool tapDetected;
+
+    /**
+     * Whether the last tap detected was a single or double tap.
+     */
+    bool doubleTap;
+
+
+    // Management functions
+    //-----------------------------------------------------------------------------------------------------------------
+
+    /**
+     * Construct a BNO080, providing pins and parameters.
+     *
+     * NOTE: while some schematics tell you to connect the BOOTN and WAKEN pins to the processor, this driver does not use or require them.
+     * Just tie them both to VCC per the datasheet.
+     *
+     * @param debugPort Serial port to write output to.  Cannot be nullptr.
+     * @param user_SDApin Hardware SPI MOSI pin
+     * @param user_SCLpin Hardware SPI MISO pin
+     * @param user_SCLKPin Hardware SPI SCLK pin
+     * @param user_CSPin SPI CS pin.  Can be any IO pin, no restrictions.
+     * @param user_INTPin Input pin connected to HINTN
+     * @param user_RSTPin Output pin connected to NRST
+     * @param i2cPortSpeed SPI frequency.  The BNO's max is 3Mhz, we default to 300Khz for safety.
+     */
+    BNO080(Serial *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin,
+           uint8_t i2cAddress=0x4a, int i2cPortSpeed=400000);
+
+    /**
+     * Resets and connects to the IMU.
+     *
+     * If this function is failing, it would be a good idea to turn on BNO_DEBUG in the cpp file to get detailed output
+     *
+     * @return whether or not initialization was successful
+     */
+    bool begin();
+
+    /**
+     * Tells the IMU to use its current rotation vector as the "zero" rotation vector and to reorient
+     * all outputs accordingly.
+     *
+     * @param zOnly If true, only the rotation about the Z axis (the heading) will be tared.
+     */
+    void tare(bool zOnly = false);
+
+    /**
+     * Tells the IMU to begin a dynamic sensor calibration.  To calibrate the IMU, call this function and move
+     * the IMU according to the instructions in the "BNO080 Sensor Calibration Procedure" app note
+     * (http://www.hillcrestlabs.com/download/59de9014566d0727bd002ae7).
+     *
+     * To tell when the calibration is complete, look at the status bits for Game Rotation Vector (for accel and gyro)
+     * and Magnetic Field (for the magnetometer).
+     *
+     * The gyro and accelerometer should only need to be calibrated once, but the magnetometer will need to be recalibrated
+     * every time the orientation of ferrous metals around the IMU changes (e.g. when it is put into a new enclosure).
+     *
+     * The new calibration will not be saved in flash until you call saveCalibration().
+     *
+     * @param calibrateAccel Whether to calibrate the accelerometer.
+     * @param calibrateGyro Whether to calibrate the gyro.
+     * @param calibrateMag Whether to calibrate the magnetometer.
+     */
+    void startCalibration(bool calibrateAccel, bool calibrateGyro, bool calibrateMag);
+
+    /**
+     * Saves the calibration started with startCalibration() and ends the calibration.
+     * You will want to call this once the status bits read as "accuracy high".
+     *
+     * WARNING: if you paid for a factory calibrated IMU, then this WILL OVERWRITE THE FACTORY CALIBRATION in whatever sensors
+     * are being calibrated.  Use with caution!
+     */
+    void saveCalibration();
+
+    // Report functions
+    //-----------------------------------------------------------------------------------------------------------------
+
+    /**
+     * Checks for new data packets queued on the IMU.
+     * If there are packets queued, receives all of them and updates
+     * the class variables with the results.
+     *
+     * @return true iff new data was received
+     */
+    bool updateData();
+
+
+    /**
+     * Gets the status of a report as a 2 bit number.
+     * per SH-2 section 6.5.1, this is interpreted as: <br>
+     * 0 - unreliable <br>
+     * 1 - accuracy low <br>
+     * 2 - accuracy medium <br>
+     * 3 - accuracy high <br>
+     * of course, these are only updated if a given report is enabled.
+     * @param report
+     * @return
+     */
+    uint8_t getReportStatus(Report report);
+
+
+    /**
+     * Enable a data report from the IMU.  Look at the comments above to see what the reports do.
+     *
+     * @param timeBetweenReports time in milliseconds between data updates.
+     */
+    void enableReport(Report report, uint16_t timeBetweenReports);
+
+private:
+
+    // internal utility functions
+    //-----------------------------------------------------------------------------------------------------------------
+
+    /**
+     * Processes the packet currently stored in the buffer, and updates class variables to reflect the data it contains
+     */
+    void processPacket();
+
+    /**
+     * Processes the sensor data packet currently stored in the buffer.
+     * Only called from processPacket()
+     */
+    void parseSensorDataPacket();
+
+    /**
+     * Call to wait for a packet with the given parameters to come in.
+     *
+     * @param channel Channel of the packet
+     * @param reportID Report ID (first data byte) of the packet
+     * @param timeout how long to wait for the packet
+     * @return true if the packet has been received, false if it timed out
+     */
+    bool waitForPacket(int channel, uint8_t reportID, float timeout = .125f);
+
+    /**
+     * Given a Q value, converts fixed point floating to regular floating point number.
+     * @param fixedPointValue
+     * @param qPoint
+     * @return
+     */
+    float qToFloat(int16_t fixedPointValue, uint8_t qPoint);
+
+    /**
+     * Given a floating point value and a Q point, convert to Q
+     * See https://en.wikipedia.org/wiki/Q_(number_format)
+     * @param qFloat
+     * @param qPoint
+     * @return
+     */
+    int16_t floatToQ(float qFloat, uint8_t qPoint);
+
+    /**
+     *  Tell the sensor to do a command.
+     *  See SH-2 Reference Manual section 6.3.8 page 42, Command request
+     *  The caller is expected to set shtpData 3 though 11 prior to calling
+     */
+    void sendCommand(uint8_t command);
+
+    /**
+     * Given a sensor's report ID, this tells the BNO080 to begin reporting the values.
+     *
+     * @param reportID
+     * @param timeBetweenReports
+     * @param specificConfig the specific config word. Useful for personal activity classifier.
+     */
+    void setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig = 0);
+
+    /**
+     * Reads a packet from the IMU and stores it in the class variables.
+     *
+     * @param timeout how long to wait for there to be a packet
+     *
+     * @return whether a packet was recieved.
+     */
+    bool receivePacket(float timeout=.2f);
+
+    /**
+     * Sends the current shtpData contents to the BNO.  It's a good idea to disable interrupts before you call this.
+     *
+     * @param channelNumber the channel to send on
+     * @param dataLength How many bits of shtpData to send
+     * @return
+     */
+    bool sendPacket(uint8_t channelNumber, uint8_t dataLength);
+
+    /**
+     * Prints the current shtp packet stored in the buffer.
+     * @param length
+     */
+    void printPacket();
+
+    /**
+     * Erases the current SHTP packet buffer so new data can be written
+     */
+     void zeroBuffer();
+
+};
+
+
+#endif //HAMSTER_BNO080_H