Jamie Smith / BNO080

Fully featured I2C and SPI driver for CEVA (Hilcrest)'s BNO080 and FSM300 Inertial Measurement Units.

Dependents:   BNO080-Examples BNO080-Examples

BNO080 Driver

by Jamie Smith / USC Rocket Propulsion Lab

After lots of development, we are proud to present our driver for the Hilcrest BNO080 IMU! This driver is inspired by SparkFun and Nathan Seidle's Arduino driver for this chip, but has been substantially rewritten and adapted.

It supports the main features of the chip, such as reading rotation and acceleration data, as well as some of its more esoteric functionality, such as counting steps and detecting whether the device is being hand-held.

Features

  • Support for 15 different data reports from the IMU, from acceleration to rotation to tap detection
  • Support for reading of sensor data, and automatic checking of update rate against allowed values in metadata
  • BNO_DEBUG switch enabling verbose, detailed output about communications with the chip for ease of debugging
  • Ability to tare sensor rotation and set mounting orientation
  • Can operate in several execution modes: polling I2C, polling SPI, and threaded SPI (which handles timing-critical functions in a dedicated thread, and automatically activates when the IMU has data available)
    • Also has experimental support for using asynchronous SPI transactions, allowing other threads to execute while communication with the BNO is occurring. Note that this functionality requires a patch to Mbed OS source code due to Mbed bug #13941
  • Calibration function
  • Reasonable code size for what you get: the library uses about 4K of flash and one instance of the object uses about 1700 bytes of RAM.

Documentation

Full Doxygen documentation is available online here

Example Code

Here's a simple example:

BNO080 Rotation Vector and Acceleration

#include <mbed.h>
#include <BNO080.h>

int main()
{
	Serial pc(USBTX, USBRX);

	// Create IMU, passing in output stream, pins, I2C address, and I2C frequency
	// These pin assignments are specific to my dev setup -- you'll need to change them
	BNO080I2C imu(&pc, p28, p27, p16, p30, 0x4a, 100000); 

	pc.baud(115200);
	pc.printf("============================================================\n");

	// Tell the IMU to report rotation every 100ms and acceleration every 200ms
	imu.enableReport(BNO080::ROTATION, 100);
	imu.enableReport(BNO080::TOTAL_ACCELERATION, 200);

	while (true)
	{
		wait(.001f);
		
		// poll the IMU for new data -- this returns true if any packets were received
		if(imu.updateData())
		{
			// now check for the specific type of data that was received (can be multiple at once)
			if (imu.hasNewData(BNO080::ROTATION))
			{
				// convert quaternion to Euler degrees and print
				pc.printf("IMU Rotation Euler: ");
				TVector3 eulerRadians = imu.rotationVector.euler();
				TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI);
				eulerDegrees.print(pc, true);
				pc.printf("\n");
			}
			if (imu.hasNewData(BNO080::TOTAL_ACCELERATION))
			{
				// print the acceleration vector using its builtin print() method
				pc.printf("IMU Total Acceleration: ");
				imu.totalAcceleration.print(pc, true);
				pc.printf("\n");
			}
		}
	}

}

If you want more, a comprehensive, ready-to-run set of examples is available on my BNO080-Examples repository.

Credits

This driver makes use of a lightweight, public-domain library for vectors and quaternions available here.

Changelog

Version 2.1 (Nov 24 2020)

  • Added BNO080Async, which provides a threaded implementation of the SPI driver. This should help get the best performance and remove annoying timing requirements on the code calling the driver
  • Added experimental USE_ASYNC_SPI option
  • Fixed bug in v2.0 causing calibrations to fail

Version 2.0 (Nov 18 2020)

  • Added SPI support
  • Refactored buffer system so that SPI could be implemented as a subclass. Unfortunately this does substantially increase the memory usage of the driver, but I believe that the benefits are worth it.

Version 1.3 (Jul 21 2020)

  • Fix deprecation warnings and compile errors in Mbed 6
  • Fix compile errors in Arm Compiler (why doesn't it have M_PI????)

Version 1.2 (Jan 30 2020)

  • Removed accidental IRQ change
  • Fixed hard iron offset reading incorrectly due to missing cast

Version 1.1 (Jun 14 2019)

  • Added support for changing permanent orientation
  • Add FRS writing functions
  • Removed some errant printfs

Version 1.0 (Dec 29 2018)

  • Initial Mbed OS release

Diff: BNO080.cpp

Revision:
8:199c7fad233d
Parent:
7:a121b6c8d662
Child:
9:430f5302f9e1
--- a/BNO080.cpp	Tue Jul 21 21:43:47 2020 -0700
+++ b/BNO080.cpp	Wed Nov 18 18:07:27 2020 -0800
@@ -66,16 +66,15 @@
 
 #include "BNO080.h"
 #include "BNO080Constants.h"
+#include <cinttypes>
+#include <algorithm>
 
 /// 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 0
 
-BNO080::BNO080(Stream *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin,
-			   uint8_t i2cAddress, int i2cPortSpeed) :
+BNO080Base::BNO080Base(Stream *debugPort, PinName user_INTPin, PinName user_RSTPin) :
 		_debugPort(debugPort),
-		_i2cPort(user_SDApin, user_SCLpin),
-		_i2cAddress(i2cAddress),
 		_int(user_INTPin),
 		_rst(user_RSTPin, 1),
 		commandSequenceNumber(0),
@@ -90,18 +89,9 @@
 {
 	// zero sequence numbers
 	memset(sequenceNumber, 0, sizeof(sequenceNumber));
-
-	//Get user settings
-	_i2cPortSpeed = i2cPortSpeed;
-	if(_i2cPortSpeed > 4000000)
-	{
-		_i2cPortSpeed = 4000000; //BNO080 max is 400Khz
-	}
-	_i2cPort.frequency(_i2cPortSpeed);
-
 }
 
-bool BNO080::begin()
+bool BNO080Base::begin()
 {
 	//Configure the BNO080 for I2C communication
 
@@ -118,7 +108,7 @@
 
 	while(true)
 	{
-		if(timeoutTimer.read() > BNO080_RESET_TIMEOUT)
+		if(timeoutTimer.elapsed_time() > BNO080_RESET_TIMEOUT)
 		{
 			_debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n");
 			return false;
@@ -156,17 +146,9 @@
 	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)
+	if(!waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET))
 	{
-		_debugPort->printf("BNO080 reports initialization failed.\n");
+		_debugPort->printf("No initialization report from BNO080.\n");
 		return false;
 	}
 	else
@@ -176,22 +158,21 @@
 #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
+	txShtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info
+	txShtpData[1] = 0; //Reserved
 	sendPacket(CHANNEL_CONTROL, 2);
 
-	waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5);
+	waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5ms);
 
-	if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE)
+	if (rxShtpData[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];
+		majorSoftwareVersion = rxShtpData[2];
+		minorSoftwareVersion = rxShtpData[3];
+		patchSoftwareVersion = (rxShtpData[13] << 8) | rxShtpData[12];
+		partNumber = (rxShtpData[7] << 24) | (rxShtpData[6] << 16) | (rxShtpData[5] << 8) | rxShtpData[4];
+		buildNumber = (rxShtpData[11] << 24) | (rxShtpData[10] << 16) | (rxShtpData[9] << 8) | rxShtpData[8];
 
 #if BNO_DEBUG
 		_debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n",
@@ -211,39 +192,39 @@
 
 }
 
-void BNO080::tare(bool zOnly)
+void BNO080Base::tare(bool zOnly)
 {
 	zeroBuffer();
 
  	// from SH-2 section 6.4.4.1
-	shtpData[3] = 0; // perform tare now
+	txShtpData[3] = 0; // perform tare now
 
 	if(zOnly)
 	{
-		shtpData[4] = 0b100; // tare Z axis
+		txShtpData[4] = 0b100; // tare Z axis
 	}
 	else
 	{
-		shtpData[4] = 0b111; // tare X, Y, and Z axes
+		txShtpData[4] = 0b111; // tare X, Y, and Z axes
 	}
 
-	shtpData[5] = 0; // reorient all motion outputs
+	txShtpData[5] = 0; // reorient all motion outputs
 
 	sendCommand(COMMAND_TARE);
 }
 
-bool BNO080::enableCalibration(bool calibrateAccel, bool calibrateGyro, bool calibrateMag)
+bool BNO080Base::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);
+	txShtpData[3] = static_cast<uint8_t>(calibrateAccel ? 1 : 0);
+	txShtpData[4] = static_cast<uint8_t>(calibrateGyro ? 1 : 0);
+	txShtpData[5] = static_cast<uint8_t>(calibrateMag ? 1 : 0);
 
-	shtpData[6] = 0; // Configure ME Calibration command
+	txShtpData[6] = 0; // Configure ME Calibration command
 
-	shtpData[7] = 0; // planar accelerometer calibration always disabled
+	txShtpData[7] = 0; // planar accelerometer calibration always disabled
 
 	sendCommand(COMMAND_ME_CALIBRATE);
 
@@ -256,7 +237,7 @@
 		return false;
 	}
 
-	if(shtpData[2] != COMMAND_ME_CALIBRATE)
+	if(rxShtpData[2] != COMMAND_ME_CALIBRATE)
 	{
 #if BNO_DEBUG
 		_debugPort->printf("Received wrong response to calibration command!\n");
@@ -264,7 +245,7 @@
 		return false;
 	}
 
-	if(shtpData[5] != 0)
+	if(rxShtpData[5] != 0)
 	{
 #if BNO_DEBUG
 		_debugPort->printf("IMU reports calibrate command failed!\n");
@@ -276,7 +257,7 @@
 	return true;
 }
 
-bool BNO080::saveCalibration()
+bool BNO080Base::saveCalibration()
 {
 	zeroBuffer();
 
@@ -292,7 +273,7 @@
 		return false;
 	}
 
-	if(shtpData[2] != COMMAND_SAVE_DCD)
+	if(rxShtpData[2] != COMMAND_SAVE_DCD)
 	{
 #if BNO_DEBUG
 		_debugPort->printf("Received wrong response to calibration command!\n");
@@ -300,7 +281,7 @@
 		return false;
 	}
 
-	if(shtpData[5] != 0)
+	if(rxShtpData[5] != 0)
 	{
 #if BNO_DEBUG
 		_debugPort->printf("IMU reports calibrate command failed!\n");
@@ -312,7 +293,7 @@
 	return true;
 }
 
-void BNO080::setSensorOrientation(Quaternion orientation)
+void BNO080Base::setSensorOrientation(Quaternion orientation)
 {
 	zeroBuffer();
 
@@ -322,19 +303,19 @@
 	int16_t Q_z = floatToQ(orientation.z(), ORIENTATION_QUAT_Q_POINT);
 	int16_t Q_w = floatToQ(orientation.w(), ORIENTATION_QUAT_Q_POINT);
 
-	shtpData[3] = 2; // set reorientation
+	txShtpData[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
+	txShtpData[4] = static_cast<uint8_t>(Q_x & 0xFF); //P1 - X component LSB
+	txShtpData[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
+	txShtpData[6] = static_cast<uint8_t>(Q_y & 0xFF); //P3 - Y component LSB
+	txShtpData[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
+	txShtpData[8] = static_cast<uint8_t>(Q_z & 0xFF); //P5 - Z component LSB
+	txShtpData[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
+	txShtpData[10] = static_cast<uint8_t>(Q_w & 0xFF); //P7 - W component LSB
+	txShtpData[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
@@ -344,7 +325,7 @@
 
 #define ORIENTATION_RECORD_LEN 4
 
-bool BNO080::setPermanentOrientation(Quaternion orientation)
+bool BNO080Base::setPermanentOrientation(Quaternion orientation)
 {
 	uint32_t orientationRecord[ORIENTATION_RECORD_LEN];
 
@@ -357,7 +338,7 @@
 	return writeFRSRecord(FRS_RECORDID_SYSTEM_ORIENTATION, orientationRecord, ORIENTATION_RECORD_LEN);
 }                                                                                                       
 
-bool BNO080::updateData()
+bool BNO080Base::updateData()
 {
 	if(_int.read() != 0)
 	{
@@ -374,13 +355,16 @@
 		}
 
 		processPacket();
+
+		// Allow time for the IMU to ready another packet if it has one
+		wait_us(150); // time between packets measured with a logic analyzer
 	}
 
 	// packets were received, so data may have changed
 	return true;
 }
 
-uint8_t BNO080::getReportStatus(Report report)
+uint8_t BNO080Base::getReportStatus(Report report)
 {
 	uint8_t reportNum = static_cast<uint8_t>(report);
 	if(reportNum > STATUS_ARRAY_LEN)
@@ -391,7 +375,7 @@
 	return reportStatus[reportNum];
 }
 
-const char* BNO080::getReportStatusString(Report report)
+const char* BNO080Base::getReportStatusString(Report report)
 {
 	switch(getReportStatus(report))
 	{
@@ -408,7 +392,7 @@
 	}
 }
 
-bool BNO080::hasNewData(Report report)
+bool BNO080Base::hasNewData(Report report)
 {
 	uint8_t reportNum = static_cast<uint8_t>(report);
 	if(reportNum > STATUS_ARRAY_LEN)
@@ -422,7 +406,7 @@
 }
 
 //Sends the packet to enable the rotation vector
-void BNO080::enableReport(Report report, uint16_t timeBetweenReports)
+void BNO080Base::enableReport(Report report, uint16_t timeBetweenReports)
 {
 #if BNO_DEBUG
 	// check time is valid
@@ -434,26 +418,20 @@
 						   static_cast<uint8_t>(report), periodSeconds, getMinPeriod(report));
 		return;
 	}
-   
-                                                                          
-  
-                                                                                                                                          
-                                                                            
-         
-  
+
 #endif
 	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)
+void BNO080Base::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 BNO080Base::getSerialNumber()
 {
 	uint32_t serNoBuffer;
 
@@ -465,7 +443,7 @@
 	return serNoBuffer;
 }
 
-float BNO080::getRange(Report report)
+float BNO080Base::getRange(Report report)
 {
 	loadReportMetadata(report);
 
@@ -473,14 +451,14 @@
 }
 
 
-float BNO080::getResolution(Report report)
+float BNO080Base::getResolution(Report report)
 {
 	loadReportMetadata(report);
 
 	return qToFloat_dword(metadataRecord[2], getQ1(report));
 }
 
-float BNO080::getPower(Report report)
+float BNO080Base::getPower(Report report)
 {
 	loadReportMetadata(report);
 
@@ -489,14 +467,14 @@
 	return qToFloat_dword(powerQ, POWER_Q_POINT);
 }
 
-float BNO080::getMinPeriod(Report report)
+float BNO080Base::getMinPeriod(Report report)
 {
 	loadReportMetadata(report);
 
 	return metadataRecord[4] / 1e6f; // convert from microseconds to seconds
 }
 
-float BNO080::getMaxPeriod(Report report)
+float BNO080Base::getMaxPeriod(Report report)
 {
 	loadReportMetadata(report);
 
@@ -509,7 +487,7 @@
 	return metadataRecord[9] / 1e6f; // convert from microseconds to seconds
 }
 
-void BNO080::printMetadataSummary(Report report)
+void BNO080Base::printMetadataSummary(Report report)
 {
 #if BNO_DEBUG
 	if(!loadReportMetadata(report))
@@ -528,44 +506,44 @@
 #endif
 }
 
-int16_t BNO080::getQ1(Report report)
+int16_t BNO080Base::getQ1(Report report)
 {
 	loadReportMetadata(report);
 
 	return static_cast<int16_t>(metadataRecord[7] & 0xFFFF);
 }
 
-int16_t BNO080::getQ2(Report report)
+int16_t BNO080Base::getQ2(Report report)
 {
 	loadReportMetadata(report);
 
 	return static_cast<int16_t>(metadataRecord[7] >> 16);
 }
 
-int16_t BNO080::getQ3(Report report)
+int16_t BNO080Base::getQ3(Report report)
 {
 	loadReportMetadata(report);
 
 	return static_cast<int16_t>(metadataRecord[8] >> 16);
 }
 
-void BNO080::processPacket()
+void BNO080Base::processPacket()
 {
-	if(shtpHeader[2] == CHANNEL_CONTROL)
+	if(rxShtpHeader[2] == CHANNEL_CONTROL)
 	{
 		// currently no command reports are read
 	}
-	else if(shtpHeader[2] == CHANNEL_EXECUTABLE)
+	else if(rxShtpHeader[2] == CHANNEL_EXECUTABLE)
 	{
 		// currently no executable reports are read
 	}
-	else if(shtpHeader[2] == CHANNEL_COMMAND)
+	else if(rxShtpHeader[2] == CHANNEL_COMMAND)
 	{
 
 	}
-	else if(shtpHeader[2] == CHANNEL_REPORTS || shtpHeader[2] == CHANNEL_WAKE_REPORTS)
+	else if(rxShtpHeader[2] == CHANNEL_REPORTS || rxShtpHeader[2] == CHANNEL_WAKE_REPORTS)
 	{
-		if(shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP)
+		if(rxShtpData[0] == SHTP_REPORT_BASE_TIMESTAMP)
 		{
 			// sensor data packet
 			parseSensorDataPacket();
@@ -591,7 +569,7 @@
 #define SIZEOF_SIGNIFICANT_MOTION 6
 #define SIZEOF_SHAKE_DETECTOR 6
 
-void BNO080::parseSensorDataPacket()
+void BNO080Base::parseSensorDataPacket()
 {
 	size_t currReportOffset = 0;
 
@@ -600,32 +578,26 @@
 	// We don't use interrupts and don't care about times, so we can throw this out.
 	currReportOffset += SIZEOF_BASE_TIMESTAMP;
 
-	while(currReportOffset < packetLength)
+	while(currReportOffset < rxPacketLength)
 	{
-		if(currReportOffset >= STORED_PACKET_SIZE)
-		{
-			_debugPort->printf("Error: sensor report longer than packet buffer! Some data was not read! Increase buffer size or decrease number of reports!\r\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];
+		uint16_t data1 = (uint16_t)rxShtpData[currReportOffset + 5] << 8 | rxShtpData[currReportOffset + 4];
+		uint16_t data2 = (uint16_t)rxShtpData[currReportOffset + 7] << 8 | rxShtpData[currReportOffset + 6];
+		uint16_t data3 = (uint16_t)rxShtpData[currReportOffset + 9] << 8 | rxShtpData[currReportOffset + 8];
 
-		uint8_t reportNum = shtpData[currReportOffset];
+		uint8_t reportNum = rxShtpData[currReportOffset];
 
 		if(reportNum != SENSOR_REPORTID_TIMESTAMP_REBASE)
 		{
 			// set status from byte 2
-			reportStatus[reportNum] = static_cast<uint8_t>(shtpData[currReportOffset + 2] & 0b11);
+			reportStatus[reportNum] = static_cast<uint8_t>(rxShtpData[currReportOffset + 2] & 0b11);
 
 			// set updated flag
 			reportHasBeenUpdated[reportNum] = true;
 		}
 
-		switch(shtpData[currReportOffset])
+		switch(rxShtpData[currReportOffset])
 		{
 			case SENSOR_REPORTID_TIMESTAMP_REBASE:
 				currReportOffset += SIZEOF_TIMESTAMP_REBASE;
@@ -688,9 +660,9 @@
 						qToFloat(data2, MAGNETOMETER_Q_POINT),
 						qToFloat(data3, MAGNETOMETER_Q_POINT));
 
-				uint16_t ironOffsetXQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
-				uint16_t ironOffsetYQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12];
-				uint16_t ironOffsetZQ = (uint16_t) shtpData[currReportOffset + 15] << 8 | shtpData[currReportOffset + 14];
+				uint16_t ironOffsetXQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
+				uint16_t ironOffsetYQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12];
+				uint16_t ironOffsetZQ = (uint16_t) rxShtpData[currReportOffset + 15] << 8 | rxShtpData[currReportOffset + 14];
 
 				hardIronOffset = TVector3(
 						qToFloat(ironOffsetXQ, MAGNETOMETER_Q_POINT),
@@ -703,8 +675,8 @@
 
 			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];
+				uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
+				uint16_t accuracyQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12];
 
 				rotationVector = TVector4(
 						qToFloat(data1, ROTATION_Q_POINT),
@@ -720,7 +692,7 @@
 
 			case SENSOR_REPORTID_GAME_ROTATION_VECTOR:
 			{
-				uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
+				uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
 
 				gameRotationVector = TVector4(
 						qToFloat(data1, ROTATION_Q_POINT),
@@ -734,8 +706,8 @@
 
 			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];
+				uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
+				uint16_t accuracyQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12];
 
 				geomagneticRotationVector = TVector4(
 						qToFloat(data1, ROTATION_Q_POINT),
@@ -754,14 +726,14 @@
 				// since we got the report, a tap was detected
 				tapDetected = true;
 
-				doubleTap = (shtpData[currReportOffset + 4] & (1 << 6)) != 0;
+				doubleTap = (rxShtpData[currReportOffset + 4] & (1 << 6)) != 0;
 
 				currReportOffset += SIZEOF_TAP_DETECTOR;
 				break;
 
 			case SENSOR_REPORTID_STABILITY_CLASSIFIER:
 			{
-				uint8_t classificationNumber = shtpData[currReportOffset + 4];
+				uint8_t classificationNumber = rxShtpData[currReportOffset + 4];
 
 				if(classificationNumber > 4)
 				{
@@ -785,7 +757,7 @@
 
 			case SENSOR_REPORTID_STEP_COUNTER:
 
-				stepCount = shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8];
+				stepCount = rxShtpData[currReportOffset + 9] << 8 | rxShtpData[currReportOffset + 8];
 
 				currReportOffset += SIZEOF_STEP_COUNTER;
 
@@ -804,28 +776,34 @@
 
 				shakeDetected = true;
 
-				xAxisShake = (shtpData[currReportOffset + 4] & 1) != 0;
-				yAxisShake = (shtpData[currReportOffset + 4] & (1 << 1)) != 0;
-				zAxisShake = (shtpData[currReportOffset + 4] & (1 << 2)) != 0;
+				xAxisShake = (rxShtpData[currReportOffset + 4] & 1) != 0;
+				yAxisShake = (rxShtpData[currReportOffset + 4] & (1 << 1)) != 0;
+				zAxisShake = (rxShtpData[currReportOffset + 4] & (1 << 2)) != 0;
 
 				currReportOffset += SIZEOF_SHAKE_DETECTOR;
 
 				break;
 				
 			default:
-				_debugPort->printf("Error: unrecognized report ID in sensor report: %hhx.  Byte %u, length %hu\n", shtpData[currReportOffset], currReportOffset, packetLength);
+				_debugPort->printf("Error: unrecognized report ID in sensor report: %hhx.  Byte %u, length %hu\n", rxShtpData[currReportOffset], currReportOffset, rxPacketLength);
 				return;
 		}
+
+		if(currReportOffset >= SHTP_RX_PACKET_SIZE)
+		{
+			_debugPort->printf("Error: sensor report longer than packet buffer! Some data was not read! Increase buffer size or decrease number of reports!\r\n");
+			return;
+		}
 	}
 
 }
 
-bool BNO080::waitForPacket(int channel, uint8_t reportID, float timeout)
+bool BNO080Base::waitForPacket(int channel, uint8_t reportID, std::chrono::milliseconds timeout)
 {
 	Timer timeoutTimer;
 	timeoutTimer.start();
 
-	while(timeoutTimer.read() <= timeout)
+	while(timeoutTimer.elapsed_time() <= timeout)
 	{
 		if(_int.read() == 0)
 		{
@@ -834,7 +812,7 @@
 				return false;
 			}
 
-			if(channel == shtpHeader[2] && reportID == shtpData[0])
+			if(channel == rxShtpHeader[2] && reportID == rxShtpData[0])
 			{
 				// found correct packet!
 				return true;
@@ -853,14 +831,14 @@
 
 //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 BNO080Base::qToFloat(int16_t fixedPointValue, uint8_t qPoint)
 {
 	float qFloat = fixedPointValue;
 	qFloat *= pow(2.0f, qPoint * -1);
 	return (qFloat);
 }
 
-float BNO080::qToFloat_dword(uint32_t fixedPointValue, int16_t qPoint)
+float BNO080Base::qToFloat_dword(uint32_t fixedPointValue, int16_t qPoint)
 {
 	float qFloat = fixedPointValue;
 	qFloat *= pow(2.0f, qPoint * -1);
@@ -869,13 +847,13 @@
 
 //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 BNO080Base::floatToQ(float qFloat, uint8_t qPoint)
 {
 	int16_t qVal = static_cast<int16_t>(qFloat * pow(2.0f, qPoint));
 	return qVal;
 }
 
-int32_t BNO080::floatToQ_dword(float qFloat, uint16_t qPoint)
+int32_t BNO080Base::floatToQ_dword(float qFloat, uint16_t qPoint)
 {
 	int32_t qVal = static_cast<int32_t>(qFloat * pow(2.0f, qPoint));
 	return qVal;
@@ -883,11 +861,11 @@
 //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)
+void BNO080Base::sendCommand(uint8_t command)
 {
-	shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request
-	shtpData[1] = commandSequenceNumber++; //Increments automatically each function call
-	shtpData[2] = command; //Command
+	txShtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request
+	txShtpData[1] = commandSequenceNumber++; //Increments automatically each function call
+	txShtpData[2] = command; //Command
 
 	//Caller must set these
 	/*shtpData[3] = 0; //P0
@@ -906,49 +884,49 @@
 
 //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)
+void BNO080Base::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)
+	txShtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55
+	txShtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
+	txShtpData[2] = 0; //Feature flags
+	txShtpData[3] = 0; //Change sensitivity (LSB)
+	txShtpData[4] = 0; //Change sensitivity (MSB)
+	txShtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms
+	txShtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval
+	txShtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval
+	txShtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
+	txShtpData[9] = (batchMicros >> 0) & 0xFF;  //Batch Interval (LSB)
+	txShtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval
+	txShtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval
+	txShtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB)
+	txShtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB)
+	txShtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config
+	txShtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config
+	txShtpData[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)
+bool BNO080Base::readFRSRecord(uint16_t recordID, uint32_t* readBuffer, uint16_t readLength)
 {
 	// send initial read request
 	zeroBuffer();
 
-	shtpData[0] = SHTP_REPORT_FRS_READ_REQUEST;
+	txShtpData[0] = SHTP_REPORT_FRS_READ_REQUEST;
 	// read offset of 0 -> start at the start of the record
-	shtpData[2] = 0;
-	shtpData[3] = 0;
+	txShtpData[2] = 0;
+	txShtpData[3] = 0;
 	// record ID
-	shtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
-	shtpData[5] = static_cast<uint8_t>(recordID >> 8);
+	txShtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
+	txShtpData[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);
+	txShtpData[6] = static_cast<uint8_t>(readLength & 0xFF);
+	txShtpData[7] = static_cast<uint8_t>(readLength >> 8);
 
 	sendPacket(CHANNEL_CONTROL, 8);
 
@@ -957,7 +935,7 @@
 	while(readOffset < readLength)
 	{
 		// it seems like it can take quite a long time for FRS data to be read, so we have to increase the timeout
-		if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE, .3f))
+		if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE, 300ms))
 		{
 #if BNO_DEBUG
 			_debugPort->printf("Error: did not receive FRS read response after sending read request!\n");
@@ -965,8 +943,8 @@
 			return false;
 		}
 
-		uint8_t status = static_cast<uint8_t>(shtpData[1] & 0b1111);
-		uint8_t dataLength = shtpData[1] >> 4;
+		uint8_t status = static_cast<uint8_t>(rxShtpData[1] & 0b1111);
+		uint8_t dataLength = rxShtpData[1] >> 4;
 
 		// check status
 		if(status == 1)
@@ -1025,7 +1003,7 @@
 		}
 
 		// now, _finally_, read the dang words
-		readBuffer[readOffset] = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | (shtpData[4]);
+		readBuffer[readOffset] = (rxShtpData[7] << 24) | (rxShtpData[6] << 16) | (rxShtpData[5] << 8) | (rxShtpData[4]);
 
 		// check if we only wanted the first word
 		++readOffset;
@@ -1034,7 +1012,7 @@
 			break;
 		}
 
-		readBuffer[readOffset] = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | (shtpData[8]);
+		readBuffer[readOffset] = (rxShtpData[11] << 24) | (rxShtpData[10] << 16) | (rxShtpData[9] << 8) | (rxShtpData[8]);
 		readOffset++;
 	}
 
@@ -1043,23 +1021,23 @@
 
 }
 
-bool BNO080::writeFRSRecord(uint16_t recordID, uint32_t* buffer, uint16_t length)
+bool BNO080Base::writeFRSRecord(uint16_t recordID, uint32_t* buffer, uint16_t length)
 {
 	// send initial write request, which tells the chip where we're writing
 	zeroBuffer();
 
-	shtpData[0] = SHTP_REPORT_FRS_WRITE_REQUEST;
+	txShtpData[0] = SHTP_REPORT_FRS_WRITE_REQUEST;
 	// length to write (must be <= record length)
-	shtpData[2] = static_cast<uint8_t>(length & 0xFF);
-	shtpData[3] = static_cast<uint8_t>(length >> 8);
+	txShtpData[2] = static_cast<uint8_t>(length & 0xFF);
+	txShtpData[3] = static_cast<uint8_t>(length >> 8);
 	// record ID
-	shtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
-	shtpData[5] = static_cast<uint8_t>(recordID >> 8);
+	txShtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
+	txShtpData[5] = static_cast<uint8_t>(recordID >> 8);
 
 	sendPacket(CHANNEL_CONTROL, 6);
 
 	// wait for FRS to become ready
-	if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, .3f))
+	if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, 300ms))
 	{
 #if BNO_DEBUG
 		_debugPort->printf("Error: did not receive FRS write ready response after sending write request!\r\n");
@@ -1067,10 +1045,10 @@
 		return false;
 	}
 
-	if(shtpData[1] != 4)
+	if(rxShtpData[1] != 4)
 	{
 #if BNO_DEBUG
-		_debugPort->printf("Error: FRS reports error initiating write operation: %hhu!\r\n", shtpData[1]);
+		_debugPort->printf("Error: FRS reports error initiating write operation: %hhu!\r\n", rxShtpData[1]);
 #endif
 		return false;
 	}
@@ -1080,25 +1058,25 @@
 	{
 		// send packet containing 2 words
 		zeroBuffer();
-		shtpData[0] = SHTP_REPORT_FRS_WRITE_DATA;
+		txShtpData[0] = SHTP_REPORT_FRS_WRITE_DATA;
 
 		// offset to write at
-		shtpData[2] = static_cast<uint8_t>(wordIndex & 0xFF);
-		shtpData[3] = static_cast<uint8_t>(wordIndex >> 8);
+		txShtpData[2] = static_cast<uint8_t>(wordIndex & 0xFF);
+		txShtpData[3] = static_cast<uint8_t>(wordIndex >> 8);
 
 		// data 0
-		*reinterpret_cast<uint32_t*>(shtpData + 4) = buffer[wordIndex];
+		*reinterpret_cast<uint32_t*>(txShtpData + 4) = buffer[wordIndex];
 
 		// data 1, if it exists
 		if(wordIndex != length - 1)
 		{
-			*reinterpret_cast<uint32_t*>(shtpData + 8) = buffer[wordIndex + 1];
+			*reinterpret_cast<uint32_t*>(txShtpData + 8) = buffer[wordIndex + 1];
 		}
 
 		sendPacket(CHANNEL_CONTROL, 12);
 
 		// wait for acknowledge
-		if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, .3f))
+		if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, 300ms))
 		{
 #if BNO_DEBUG
 			_debugPort->printf("Error: did not receive FRS write response after sending write data!\r\n");
@@ -1106,7 +1084,7 @@
 			return false;
 		}
 
-		uint8_t status = shtpData[1];
+		uint8_t status = rxShtpData[1];
 		
 		switch(status)
 		{
@@ -1192,144 +1170,8 @@
 	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()
+//Pretty prints the contents of a packet in the given buffer
+void BNO080Base::printPacket(uint8_t * buffer)
 {
 #if BNO_DEBUG
 	//Print the four byte header
@@ -1337,53 +1179,47 @@
 	for (uint8_t x = 0 ; x < 4 ; x++)
 	{
 		_debugPort->printf(" ");
-		if (shtpHeader[x] < 0x10) _debugPort->printf("0");
-		_debugPort->printf("%hhx", shtpHeader[x]);
+		_debugPort->printf("%02hhx", buffer[x]);
 	}
 
-	uint16_t printLength = packetLength;
-	if (printLength > 40) printLength = 40; //Artificial limit. We don't want the phone book.
+	//Calculate the number of data bytes in this packet
+	uint16_t totalLength = (static_cast<uint16_t>(buffer[1]) << 8 | buffer[0]);
+	totalLength &= ~(1 << 15);
+	uint16_t packetLength = totalLength - 4; //Remove the header bytes from the data count
+
+	uint16_t printLength = std::min(packetLength, static_cast<uint16_t>(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("%02hhx", buffer[x + SHTP_HEADER_SIZE]);
 	}
 
 	_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("%d", packetLength + SHTP_HEADER_SIZE);
+	_debugPort->printf(", SeqNum: %hhu", buffer[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]);
+	if (buffer[2] == 0) _debugPort->printf("Command");
+	else if (buffer[2] == 1) _debugPort->printf("Executable");
+	else if (buffer[2] == 2) _debugPort->printf("Control");
+	else if (buffer[2] == 3) _debugPort->printf("Sensor-report");
+	else if (buffer[2] == 4) _debugPort->printf("Wake-report");
+	else if (buffer[2] == 5) _debugPort->printf("Gyro-vector");
+	else _debugPort->printf("%hhu", buffer[2]);
 
 	_debugPort->printf("\n");
 #endif
 }
 
 
-void BNO080::zeroBuffer()
+void BNO080Base::zeroBuffer()
 {
-	memset(shtpHeader, 0, SHTP_HEADER_SIZE);
-	memset(shtpData, 0, STORED_PACKET_SIZE);
-	packetLength = 0;
+	memset(txPacketBuffer, 0, SHTP_HEADER_SIZE + SHTP_MAX_TX_PACKET_SIZE);
 }
 
-bool BNO080::loadReportMetadata(BNO080::Report report)
+bool BNO080Base::loadReportMetadata(BNO080Base::Report report)
 {
 	uint16_t reportMetaRecord = 0;
 
@@ -1457,3 +1293,389 @@
 
 	return true;
 }
+
+BNO080I2C::BNO080I2C(Stream *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin,
+					   uint8_t i2cAddress, int i2cPortSpeed) :
+	   BNO080Base(debugPort, user_INTPin, user_RSTPin),
+		_i2cPort(user_SDApin, user_SCLpin),
+		_i2cAddress(i2cAddress)
+{
+
+	//Get user settings
+	_i2cPortSpeed = i2cPortSpeed;
+	if(_i2cPortSpeed > 4000000)
+	{
+		_i2cPortSpeed = 4000000; //BNO080 max is 400Khz
+	}
+	_i2cPort.frequency(_i2cPortSpeed);
+}
+
+
+//Given the data packet, send the header then the data
+//Returns false if sensor does not ACK
+bool BNO080I2C::sendPacket(uint8_t channelNumber, uint8_t dataLength)
+{
+
+	uint16_t totalLength = dataLength + 4; //Add four bytes for the header
+
+	txShtpHeader[0] = totalLength & 0xFF;
+	txShtpHeader[1] = totalLength >> 8;
+	txShtpHeader[2] = channelNumber;
+	txShtpHeader[3] = sequenceNumber[channelNumber];
+
+#if BNO_DEBUG
+	_debugPort->printf("Transmitting packet: ----------------\n");
+	printPacket(txPacketBuffer);
+#endif
+
+	// send packet to IMU
+	int writeResult = _i2cPort.write(_i2cAddress << 1, reinterpret_cast<char*>(txPacketBuffer), totalLength);
+
+	if(writeResult != 0)
+	{
+		_debugPort->printf("BNO I2C write failed!\n");
+		return false;
+	}
+	return true;
+}
+
+//Check to see if there is any new data available
+//Read the contents of the incoming packet into the shtpData array
+bool BNO080I2C::receivePacket(std::chrono::milliseconds timeout)
+{
+	Timer waitStartTime;
+	waitStartTime.start();
+
+	while(_int.read() != 0)
+	{
+		if(waitStartTime.elapsed_time() > timeout)
+		{
+			_debugPort->printf("BNO I2C header 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
+	rxShtpHeader[0] = packetLSB;
+	rxShtpHeader[1] = packetMSB;
+	rxShtpHeader[2] = channelNumber;
+	rxShtpHeader[3] = sequenceNum;
+
+	if(rxShtpHeader[0] == 0xFF && rxShtpHeader[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
+	rxPacketLength = (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.
+	rxPacketLength &= ~(1 << 15);
+
+	if (rxPacketLength == 0)
+	{
+		// Packet is empty
+		return (false); //All done
+	}
+
+	rxPacketLength -= 4; //Remove the header bytes from the data count
+
+	//Read incoming data into the shtpData array
+	for (uint16_t dataSpot = 0 ; dataSpot < rxPacketLength ; dataSpot++)
+	{
+		bool sendACK = dataSpot < rxPacketLength - 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 < (sizeof(rxPacketBuffer) - SHTP_HEADER_SIZE)) //BNO080 can respond with upto 270 bytes, avoid overflow
+			rxShtpData[dataSpot] = incoming; //Store data into the shtpData array
+	}
+
+	_i2cPort.stop();
+
+	/*
+	// first read the header to get the length of the packet
+	int i2cResult;
+	i2cResult = _i2cPort.read(_i2cAddress << 1, reinterpret_cast<char*>(packetBuffer), SHTP_HEADER_SIZE, false);
+	if(i2cResult != 0)
+	{
+		_debugPort->printf("BNO I2C length read failed!\n");
+		return false;
+	}
+
+	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
+	uint16_t totalLength = (static_cast<uint16_t>(shtpHeader[1]) << 8 | shtpHeader[0]);
+
+	// 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.
+	totalLength &= ~(1 << 15);
+
+	if (totalLength == 0)
+	{
+		// Packet is empty
+		return (false); //All done
+	}
+
+	// only receive as many bytes as we can fit
+	if(totalLength > sizeof(packetBuffer))
+	{
+		totalLength = sizeof(packetBuffer);
+	}
+
+	packetLength = totalLength - 4; //Remove the header bytes from the data count
+
+	waitStartTime.reset();
+
+	while(_int.read() != 0)
+	{
+		if(waitStartTime.elapsed_time() > timeout)
+		{
+			_debugPort->printf("BNO I2C wait timeout\n");
+			return false;
+		}
+
+	}
+
+	//Read the actual packet bytes
+	_debugPort->printf("Attempting to read %" PRIu16 " bytes\n", totalLength);
+	i2cResult = _i2cPort.read(_i2cAddress << 1, reinterpret_cast<char*>(packetBuffer), totalLength);
+	if(i2cResult != 0)
+	{
+		_debugPort->printf("BNO I2C read failed!\n");
+		return false;
+	}
+	*/
+
+#if BNO_DEBUG
+	_debugPort->printf("Recieved packet: ----------------\n");
+	printPacket(rxPacketBuffer); // note: add 4 for the header length
+#endif
+
+	return (true); //We're done!
+}
+
+BNO080SPI::BNO080SPI(Stream *debugPort, PinName rstPin, PinName intPin, PinName wakePin, PinName misoPin,
+					 PinName mosiPin, PinName sclkPin, PinName csPin, int spiSpeed):
+BNO080Base(debugPort, intPin, rstPin),
+_spiPort(mosiPin, misoPin, sclkPin, csPin, use_gpio_ssel),
+_wakePin(wakePin, 1), // wake pin needs to be 1 on reset so that the BNO boots up into SPI mode
+_spiSpeed(spiSpeed)
+{
+	_spiPort.frequency(spiSpeed);
+	_spiPort.format(8, 3);
+	_spiPort.set_default_write_value(0x0);
+}
+
+bool BNO080SPI::sendPacket(uint8_t channelNumber, uint8_t dataLength)
+{
+	if(_int.read() == 0)
+	{
+		// The BNO is already awake because it has a packet it wants to send to us
+	}
+	else
+	{
+		// assert WAKE to tell the BNO to prepare for a transfer
+		_wakePin = 0;
+
+		Timer waitStartTime;
+		waitStartTime.start();
+		const std::chrono::milliseconds timeout = 10ms;
+
+		while(_int.read() != 0)
+		{
+			if(waitStartTime.elapsed_time() > timeout)
+			{
+				_debugPort->printf("BNO SPI wake wait timeout\n");
+				_wakePin = 1;
+				return false;
+			}
+		}
+		_wakePin = 1;
+	}
+
+	uint16_t totalLength = dataLength + 4; //Add four bytes for the header
+
+	txShtpHeader[0] = totalLength & 0xFF;
+	txShtpHeader[1] = totalLength >> 8;
+	txShtpHeader[2] = channelNumber;
+	txShtpHeader[3] = sequenceNumber[channelNumber];
+
+#if BNO_DEBUG
+	_debugPort->printf("Transmitting packet: ----------------\n");
+	printPacket(txPacketBuffer);
+#endif
+
+	// wipe out any existing RX packet header so we know if we received a packet
+	memset(rxPacketBuffer, 0, SHTP_HEADER_SIZE);
+
+	// send packet to IMU.
+	// This also might receive the first part of another packet, which there is no way to avoid.
+	_spiPort.write(reinterpret_cast<char*>(txPacketBuffer), totalLength, reinterpret_cast<char*>(rxPacketBuffer), totalLength);
+
+	if(rxShtpHeader[0] == 0 && rxShtpHeader[0] == 0)
+	{
+		// no header data so no packet received
+		return true;
+	}
+	else
+	{
+		// received first part of data packet while writing
+		if(receiveCompletePacket(totalLength))
+		{
+			// received data packet, send to proper channels
+			processPacket();
+			return true;
+		}
+
+		// receive failed
+		return false;
+	}
+}
+
+//Check to see if there is any new data available
+//Read the contents of the incoming packet into the shtpData array
+bool BNO080SPI::receivePacket(std::chrono::milliseconds timeout)
+{
+	Timer waitStartTime;
+	waitStartTime.start();
+
+	while (_int.read() != 0)
+	{
+		if (waitStartTime.elapsed_time() > timeout)
+		{
+			_debugPort->printf("BNO SPI header wait timeout\n");
+			return false;
+		}
+
+	}
+
+	// read the header bytes first
+	_spiPort.write(nullptr, 0, reinterpret_cast<char *>(rxPacketBuffer), SHTP_HEADER_SIZE);
+
+	// now read the data
+	return receiveCompletePacket(SHTP_HEADER_SIZE, timeout);
+}
+
+bool BNO080SPI::receiveCompletePacket(size_t bytesRead, std::chrono::milliseconds timeout)
+{
+	// Process header bytes
+	// ------------------------------------------------------------------------
+	if (rxShtpHeader[0] == 0xFF && rxShtpHeader[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
+	uint16_t totalLength = (static_cast<uint16_t>(rxShtpHeader[1]) << 8 | rxShtpHeader[0]);
+
+	// Clear the MSbit.
+	totalLength &= ~(1 << 15);
+
+	if (totalLength == 0)
+	{
+		// Packet is empty
+		return (false); //All done
+	}
+
+	rxPacketLength = totalLength - SHTP_HEADER_SIZE;
+
+	if(totalLength <= bytesRead)
+	{
+		// the original transaction already read the completed packet!  We're done.
+		return true;
+	}
+
+	// Receive data
+	// ------------------------------------------------------------------------
+
+	// Wait for it to be ready to talk to us again.
+	// Note: in my testing this takes about 200ms
+	Timer waitStartTime;
+	waitStartTime.start();
+	while (_int.read() != 0)
+	{
+		if (waitStartTime.elapsed_time() > timeout)
+		{
+			_debugPort->printf("BNO SPI continued packet header wait timeout\n");
+			return false;
+		}
+
+	}
+
+	if(rxPacketLength > SHTP_RX_PACKET_SIZE)
+	{
+		_debugPort->printf("Packet too long (%" PRIu16 " bytes), increase SHTP_RX_PACKET_SIZE\n", rxPacketLength);
+		_debugPort->printf("Packet dropped, expect subsequent driver errors.\n");
+		return false;
+	}
+
+	if(bytesRead == SHTP_HEADER_SIZE)
+	{
+		// just read the entire packet into the buffer
+		_spiPort.write(nullptr, 0, reinterpret_cast<char*>(rxPacketBuffer), totalLength);
+	}
+	else
+	{
+		// we want to receive a new header, plus the remaining data bytes that haven't been read.
+		size_t receiveLength = SHTP_HEADER_SIZE + (totalLength - bytesRead);
+
+		// read remaining bytes into the data buffer starting at the next byte
+		_spiPort.write(nullptr, 0, reinterpret_cast<char*>(rxPacketBuffer) + bytesRead, receiveLength);
+
+		// erase the new header we just read, leaving only the data as a contiguous block
+		std::memmove(rxPacketBuffer + bytesRead, rxPacketBuffer + bytesRead + SHTP_HEADER_SIZE, receiveLength - SHTP_HEADER_SIZE);
+	}
+
+#if BNO_DEBUG
+	_debugPort->printf("Recieved packet: ----------------\n");
+	printPacket(rxPacketBuffer);
+#endif
+
+	//ThisThread::sleep_for(4ms);
+
+	// done!
+	return true;
+}
\ No newline at end of file