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
Committer:
MultipleMonomials
Date:
Sun Dec 23 05:23:21 2018 +0000
Revision:
0:f677e13975d0
Child:
1:aac28ffd63ed
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MultipleMonomials 0:f677e13975d0 1 //
MultipleMonomials 0:f677e13975d0 2 // USC RPL BNO080 driver.
MultipleMonomials 0:f677e13975d0 3 //
MultipleMonomials 0:f677e13975d0 4
MultipleMonomials 0:f677e13975d0 5 /*
MultipleMonomials 0:f677e13975d0 6 * Overview of BNO080 Communications
MultipleMonomials 0:f677e13975d0 7 * ===============================================
MultipleMonomials 0:f677e13975d0 8 *
MultipleMonomials 0:f677e13975d0 9 * Hilcrest has developed a protocol called SHTP (Sensor Hub Transport Protocol) for binary communications with
MultipleMonomials 0:f677e13975d0 10 * the BNO080 and the other IMUs it sells. Over this protocol, SH-2 (Sensor Hub 2) messages are sent to configure
MultipleMonomials 0:f677e13975d0 11 * the chip and read data back.
MultipleMonomials 0:f677e13975d0 12 *
MultipleMonomials 0:f677e13975d0 13 * SHTP messages are divided at two hierarchical levels: first the channel, then the report ID. Each category
MultipleMonomials 0:f677e13975d0 14 * of messages (system commands, sensor data reports, etc.) has its own channel, and the individual messages
MultipleMonomials 0:f677e13975d0 15 * in each channel are identified by their report id, which is the first byte of the message payload (note that the
MultipleMonomials 0:f677e13975d0 16 * datasheets don't *always* call the first byte the report ID, but that byte does identify the report, so I'm going
MultipleMonomials 0:f677e13975d0 17 * with it).
MultipleMonomials 0:f677e13975d0 18 *
MultipleMonomials 0:f677e13975d0 19 * ===============================================
MultipleMonomials 0:f677e13975d0 20 *
MultipleMonomials 0:f677e13975d0 21 * Information about the BNO080 is split into three datasheets. Here's the download links and what they cover:
MultipleMonomials 0:f677e13975d0 22 *
MultipleMonomials 0:f677e13975d0 23 * - the BNO080 datasheet: http://www.hillcrestlabs.com/download/5a05f340566d07c196001ec1
MultipleMonomials 0:f677e13975d0 24 * -- Chip pinouts
MultipleMonomials 0:f677e13975d0 25 * -- Example circuits
MultipleMonomials 0:f677e13975d0 26 * -- Physical specifications
MultipleMonomials 0:f677e13975d0 27 * -- Supported reports and configuration settings (at a high level)
MultipleMonomials 0:f677e13975d0 28 * -- List of packets on the SHTP executable channel
MultipleMonomials 0:f677e13975d0 29 *
MultipleMonomials 0:f677e13975d0 30 * - the SHTP protocol: http://www.hillcrestlabs.com/download/59de8f99cd829e94dc0029d7
MultipleMonomials 0:f677e13975d0 31 * -- SHTP transmit and receive protcols (for SPI, I2C, and UART)
MultipleMonomials 0:f677e13975d0 32 * -- SHTP binary format
MultipleMonomials 0:f677e13975d0 33 * -- packet types on the SHTP command channel
MultipleMonomials 0:f677e13975d0 34 *
MultipleMonomials 0:f677e13975d0 35 * - the SH-2 reference: http://www.hillcrestlabs.com/download/59de8f398934bf6faa00293f
MultipleMonomials 0:f677e13975d0 36 * -- list of packets and their formats for all channels other than command and executable
MultipleMonomials 0:f677e13975d0 37 * -- list of FRS (Flash Record System) entries and their formats
MultipleMonomials 0:f677e13975d0 38 *
MultipleMonomials 0:f677e13975d0 39 * ===============================================
MultipleMonomials 0:f677e13975d0 40 *
MultipleMonomials 0:f677e13975d0 41 * Overview of SHTP channels:
MultipleMonomials 0:f677e13975d0 42 *
MultipleMonomials 0:f677e13975d0 43 * 0 -> Command
MultipleMonomials 0:f677e13975d0 44 * -- Used for protocol-global packets, currently only the advertisement packet (which lists all the channels) and error reports
MultipleMonomials 0:f677e13975d0 45 *
MultipleMonomials 0:f677e13975d0 46 * 1 -> Executable
MultipleMonomials 0:f677e13975d0 47 * -- Used for things that control the software on the chip: commands to reset and sleep
MultipleMonomials 0:f677e13975d0 48 * -- Also used by the chip to report when it's done booting up
MultipleMonomials 0:f677e13975d0 49 *
MultipleMonomials 0:f677e13975d0 50 * 2 -> Control
MultipleMonomials 0:f677e13975d0 51 * -- Used to send configuration commands to the IMU and for it to send back responses.
MultipleMonomials 0:f677e13975d0 52 * -- Common report IDs: Command Request (0xF2), Set Feature (0xFD)
MultipleMonomials 0:f677e13975d0 53 *
MultipleMonomials 0:f677e13975d0 54 * 3 -> Sensor Reports
MultipleMonomials 0:f677e13975d0 55 * -- Used for sensors to send back data reports.
MultipleMonomials 0:f677e13975d0 56 * -- AFAIK the only report ID on this channel will be 0xFB (Report Base Timestamp); sensor data is send in a series of structures
MultipleMonomials 0:f677e13975d0 57 * following an 0xFB
MultipleMonomials 0:f677e13975d0 58 *
MultipleMonomials 0:f677e13975d0 59 * 4 -> Wake Sensor Reports
MultipleMonomials 0:f677e13975d0 60 * -- same as above, but for sensors configured to wake the device
MultipleMonomials 0:f677e13975d0 61 *
MultipleMonomials 0:f677e13975d0 62 * 5 -> Gyro Rotation Vector
MultipleMonomials 0:f677e13975d0 63 * -- a dedicated channel for the Gyro Rotation Vector sensor report
MultipleMonomials 0:f677e13975d0 64 * -- Why does this get its own channel? I don't know!!!
MultipleMonomials 0:f677e13975d0 65 */
MultipleMonomials 0:f677e13975d0 66
MultipleMonomials 0:f677e13975d0 67 #include "BNO080.h"
MultipleMonomials 0:f677e13975d0 68 #include "BNO080Constants.h"
MultipleMonomials 0:f677e13975d0 69
MultipleMonomials 0:f677e13975d0 70 /// Set to 1 to enable debug printouts. Should be very useful if the chip is giving you trouble.
MultipleMonomials 0:f677e13975d0 71 /// When debugging, it is recommended to use the highest possible serial baudrate so as not to interrupt the timing of operations.
MultipleMonomials 0:f677e13975d0 72 #define BNO_DEBUG 1
MultipleMonomials 0:f677e13975d0 73
MultipleMonomials 0:f677e13975d0 74 BNO080::BNO080(Serial *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin,
MultipleMonomials 0:f677e13975d0 75 uint8_t i2cAddress, int i2cPortSpeed) :
MultipleMonomials 0:f677e13975d0 76 _debugPort(debugPort),
MultipleMonomials 0:f677e13975d0 77 _i2cPort(user_SDApin, user_SCLpin),
MultipleMonomials 0:f677e13975d0 78 _i2cAddress(i2cAddress),
MultipleMonomials 0:f677e13975d0 79 _int(user_INTPin),
MultipleMonomials 0:f677e13975d0 80 _rst(user_RSTPin, 1),
MultipleMonomials 0:f677e13975d0 81 _scope(p21, 1)
MultipleMonomials 0:f677e13975d0 82 {
MultipleMonomials 0:f677e13975d0 83 //Get user settings
MultipleMonomials 0:f677e13975d0 84 _i2cPortSpeed = i2cPortSpeed;
MultipleMonomials 0:f677e13975d0 85 if(_i2cPortSpeed > 4000000)
MultipleMonomials 0:f677e13975d0 86 {
MultipleMonomials 0:f677e13975d0 87 _i2cPortSpeed = 4000000; //BNO080 max is 400Khz
MultipleMonomials 0:f677e13975d0 88 }
MultipleMonomials 0:f677e13975d0 89 _i2cPort.frequency(_i2cPortSpeed);
MultipleMonomials 0:f677e13975d0 90
MultipleMonomials 0:f677e13975d0 91 }
MultipleMonomials 0:f677e13975d0 92
MultipleMonomials 0:f677e13975d0 93 bool BNO080::begin()
MultipleMonomials 0:f677e13975d0 94 {
MultipleMonomials 0:f677e13975d0 95 //Configure the BNO080 for SPI communication
MultipleMonomials 0:f677e13975d0 96
MultipleMonomials 0:f677e13975d0 97 _rst = 0; // Reset BNO080
MultipleMonomials 0:f677e13975d0 98 wait(.002f); // Min length not specified in datasheet?
MultipleMonomials 0:f677e13975d0 99 _rst = 1; // Bring out of reset
MultipleMonomials 0:f677e13975d0 100
MultipleMonomials 0:f677e13975d0 101 // wait for a falling edge (NOT just a low) on the INT pin to denote startup
MultipleMonomials 0:f677e13975d0 102 Timer timeoutTimer;
MultipleMonomials 0:f677e13975d0 103
MultipleMonomials 0:f677e13975d0 104 bool highDetected = false;
MultipleMonomials 0:f677e13975d0 105 bool lowDetected = false;
MultipleMonomials 0:f677e13975d0 106
MultipleMonomials 0:f677e13975d0 107 while(true)
MultipleMonomials 0:f677e13975d0 108 {
MultipleMonomials 0:f677e13975d0 109 if(timeoutTimer.read() > BNO080_RESET_TIMEOUT)
MultipleMonomials 0:f677e13975d0 110 {
MultipleMonomials 0:f677e13975d0 111 _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n");
MultipleMonomials 0:f677e13975d0 112 return false;
MultipleMonomials 0:f677e13975d0 113 }
MultipleMonomials 0:f677e13975d0 114
MultipleMonomials 0:f677e13975d0 115 // simple edge detector
MultipleMonomials 0:f677e13975d0 116 if(!highDetected)
MultipleMonomials 0:f677e13975d0 117 {
MultipleMonomials 0:f677e13975d0 118 if(_int == 1)
MultipleMonomials 0:f677e13975d0 119 {
MultipleMonomials 0:f677e13975d0 120 highDetected = true;
MultipleMonomials 0:f677e13975d0 121 }
MultipleMonomials 0:f677e13975d0 122 }
MultipleMonomials 0:f677e13975d0 123 else if(!lowDetected)
MultipleMonomials 0:f677e13975d0 124 {
MultipleMonomials 0:f677e13975d0 125 if(_int == 0)
MultipleMonomials 0:f677e13975d0 126 {
MultipleMonomials 0:f677e13975d0 127 lowDetected = true;
MultipleMonomials 0:f677e13975d0 128 }
MultipleMonomials 0:f677e13975d0 129 }
MultipleMonomials 0:f677e13975d0 130 else
MultipleMonomials 0:f677e13975d0 131 {
MultipleMonomials 0:f677e13975d0 132 // high and low detected
MultipleMonomials 0:f677e13975d0 133 break;
MultipleMonomials 0:f677e13975d0 134 }
MultipleMonomials 0:f677e13975d0 135 }
MultipleMonomials 0:f677e13975d0 136
MultipleMonomials 0:f677e13975d0 137 _debugPort->printf("BNO080 detected!\n");
MultipleMonomials 0:f677e13975d0 138
MultipleMonomials 0:f677e13975d0 139 // At system startup, the hub must send its full advertisement message (see SHTP 5.2 and 5.3) to the
MultipleMonomials 0:f677e13975d0 140 // host. It must not send any other data until this step is complete.
MultipleMonomials 0:f677e13975d0 141 // We don't actually care what's in it, we're just using it as a signal to indicate that the reset is complete.
MultipleMonomials 0:f677e13975d0 142 receivePacket();
MultipleMonomials 0:f677e13975d0 143
MultipleMonomials 0:f677e13975d0 144 // now, after startup, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command
MultipleMonomials 0:f677e13975d0 145 waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET);
MultipleMonomials 0:f677e13975d0 146
MultipleMonomials 0:f677e13975d0 147 // Next, officially tell it to initialize, and wait for a successful Initialize Response
MultipleMonomials 0:f677e13975d0 148 zeroBuffer();
MultipleMonomials 0:f677e13975d0 149 shtpData[3] = 0;
MultipleMonomials 0:f677e13975d0 150 _scope = 0;
MultipleMonomials 0:f677e13975d0 151 sendCommand(COMMAND_INITIALIZE);
MultipleMonomials 0:f677e13975d0 152
MultipleMonomials 0:f677e13975d0 153
MultipleMonomials 0:f677e13975d0 154 if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE) || shtpData[2] != COMMAND_INITIALIZE || shtpData[5] != 0)
MultipleMonomials 0:f677e13975d0 155 {
MultipleMonomials 0:f677e13975d0 156 _debugPort->printf("BNO080 reports initialization failed.\n");
MultipleMonomials 0:f677e13975d0 157 __enable_irq();
MultipleMonomials 0:f677e13975d0 158 return false;
MultipleMonomials 0:f677e13975d0 159 }
MultipleMonomials 0:f677e13975d0 160 else
MultipleMonomials 0:f677e13975d0 161 {
MultipleMonomials 0:f677e13975d0 162 #if BNO_DEBUG
MultipleMonomials 0:f677e13975d0 163 _debugPort->printf("BNO080 reports initialization successful!\n");
MultipleMonomials 0:f677e13975d0 164 #endif
MultipleMonomials 0:f677e13975d0 165 }
MultipleMonomials 0:f677e13975d0 166
MultipleMonomials 0:f677e13975d0 167
MultipleMonomials 0:f677e13975d0 168 // Finally, we want to interrogate the device about its model and version.
MultipleMonomials 0:f677e13975d0 169 zeroBuffer();
MultipleMonomials 0:f677e13975d0 170 shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info
MultipleMonomials 0:f677e13975d0 171 shtpData[1] = 0; //Reserved
MultipleMonomials 0:f677e13975d0 172 sendPacket(CHANNEL_CONTROL, 2);
MultipleMonomials 0:f677e13975d0 173
MultipleMonomials 0:f677e13975d0 174 waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5);
MultipleMonomials 0:f677e13975d0 175
MultipleMonomials 0:f677e13975d0 176 if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE)
MultipleMonomials 0:f677e13975d0 177 {
MultipleMonomials 0:f677e13975d0 178 majorSoftwareVersion = shtpData[2];
MultipleMonomials 0:f677e13975d0 179 minorSoftwareVersion = shtpData[3];
MultipleMonomials 0:f677e13975d0 180 patchSoftwareVersion = (shtpData[13] << 8) | shtpData[12];
MultipleMonomials 0:f677e13975d0 181 partNumber = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | shtpData[4];
MultipleMonomials 0:f677e13975d0 182 buildNumber = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | shtpData[8];
MultipleMonomials 0:f677e13975d0 183
MultipleMonomials 0:f677e13975d0 184 #if BNO_DEBUG
MultipleMonomials 0:f677e13975d0 185 _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n",
MultipleMonomials 0:f677e13975d0 186 majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion,
MultipleMonomials 0:f677e13975d0 187 buildNumber, partNumber);
MultipleMonomials 0:f677e13975d0 188 #endif
MultipleMonomials 0:f677e13975d0 189
MultipleMonomials 0:f677e13975d0 190 }
MultipleMonomials 0:f677e13975d0 191 else
MultipleMonomials 0:f677e13975d0 192 {
MultipleMonomials 0:f677e13975d0 193 _debugPort->printf("Bad response from product ID command.\n");
MultipleMonomials 0:f677e13975d0 194 return false;
MultipleMonomials 0:f677e13975d0 195 }
MultipleMonomials 0:f677e13975d0 196
MultipleMonomials 0:f677e13975d0 197 // successful init
MultipleMonomials 0:f677e13975d0 198 return true;
MultipleMonomials 0:f677e13975d0 199
MultipleMonomials 0:f677e13975d0 200 }
MultipleMonomials 0:f677e13975d0 201
MultipleMonomials 0:f677e13975d0 202 void BNO080::tare(bool zOnly)
MultipleMonomials 0:f677e13975d0 203 {
MultipleMonomials 0:f677e13975d0 204 zeroBuffer();
MultipleMonomials 0:f677e13975d0 205
MultipleMonomials 0:f677e13975d0 206 // from SH-2 section 6.4.4.1
MultipleMonomials 0:f677e13975d0 207 shtpData[3] = 0; // perform tare now
MultipleMonomials 0:f677e13975d0 208
MultipleMonomials 0:f677e13975d0 209 if(zOnly)
MultipleMonomials 0:f677e13975d0 210 {
MultipleMonomials 0:f677e13975d0 211 shtpData[4] = 0b100; // tare Z axis
MultipleMonomials 0:f677e13975d0 212 }
MultipleMonomials 0:f677e13975d0 213 else
MultipleMonomials 0:f677e13975d0 214 {
MultipleMonomials 0:f677e13975d0 215 shtpData[4] = 0b111; // tare X, Y, and Z axes
MultipleMonomials 0:f677e13975d0 216 }
MultipleMonomials 0:f677e13975d0 217
MultipleMonomials 0:f677e13975d0 218 shtpData[5] = 0; // reorient all motion outputs
MultipleMonomials 0:f677e13975d0 219
MultipleMonomials 0:f677e13975d0 220 sendCommand(COMMAND_TARE);
MultipleMonomials 0:f677e13975d0 221 }
MultipleMonomials 0:f677e13975d0 222
MultipleMonomials 0:f677e13975d0 223 bool BNO080::updateData()
MultipleMonomials 0:f677e13975d0 224 {
MultipleMonomials 0:f677e13975d0 225 if(_int.read() != 0)
MultipleMonomials 0:f677e13975d0 226 {
MultipleMonomials 0:f677e13975d0 227 // no waiting packets
MultipleMonomials 0:f677e13975d0 228 return false;
MultipleMonomials 0:f677e13975d0 229 }
MultipleMonomials 0:f677e13975d0 230
MultipleMonomials 0:f677e13975d0 231 while(_int.read() == 0)
MultipleMonomials 0:f677e13975d0 232 {
MultipleMonomials 0:f677e13975d0 233 if(!receivePacket())
MultipleMonomials 0:f677e13975d0 234 {
MultipleMonomials 0:f677e13975d0 235 // comms error
MultipleMonomials 0:f677e13975d0 236 return false;
MultipleMonomials 0:f677e13975d0 237 }
MultipleMonomials 0:f677e13975d0 238
MultipleMonomials 0:f677e13975d0 239 processPacket();
MultipleMonomials 0:f677e13975d0 240 }
MultipleMonomials 0:f677e13975d0 241
MultipleMonomials 0:f677e13975d0 242 // packets were received, so data may have changed
MultipleMonomials 0:f677e13975d0 243 return true;
MultipleMonomials 0:f677e13975d0 244 }
MultipleMonomials 0:f677e13975d0 245
MultipleMonomials 0:f677e13975d0 246 uint8_t BNO080::getReportStatus(Report report)
MultipleMonomials 0:f677e13975d0 247 {
MultipleMonomials 0:f677e13975d0 248 uint8_t reportNum = static_cast<uint8_t>(report);
MultipleMonomials 0:f677e13975d0 249 if(reportNum > STATUS_ARRAY_LEN)
MultipleMonomials 0:f677e13975d0 250 {
MultipleMonomials 0:f677e13975d0 251 return 0;
MultipleMonomials 0:f677e13975d0 252 }
MultipleMonomials 0:f677e13975d0 253
MultipleMonomials 0:f677e13975d0 254 return reportStatus[reportNum];
MultipleMonomials 0:f677e13975d0 255 }
MultipleMonomials 0:f677e13975d0 256
MultipleMonomials 0:f677e13975d0 257 //Sends the packet to enable the rotation vector
MultipleMonomials 0:f677e13975d0 258 void BNO080::enableReport(Report report, uint16_t timeBetweenReports)
MultipleMonomials 0:f677e13975d0 259 {
MultipleMonomials 0:f677e13975d0 260 setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports);
MultipleMonomials 0:f677e13975d0 261
MultipleMonomials 0:f677e13975d0 262 // note: we don't wait for ACKs on these packets because they can take quite a while, like half a second, to come in
MultipleMonomials 0:f677e13975d0 263 }
MultipleMonomials 0:f677e13975d0 264
MultipleMonomials 0:f677e13975d0 265
MultipleMonomials 0:f677e13975d0 266 void BNO080::processPacket()
MultipleMonomials 0:f677e13975d0 267 {
MultipleMonomials 0:f677e13975d0 268 if(shtpHeader[2] == CHANNEL_CONTROL)
MultipleMonomials 0:f677e13975d0 269 {
MultipleMonomials 0:f677e13975d0 270 // currently no command reports are read
MultipleMonomials 0:f677e13975d0 271 }
MultipleMonomials 0:f677e13975d0 272 else if(shtpHeader[2] == CHANNEL_EXECUTABLE)
MultipleMonomials 0:f677e13975d0 273 {
MultipleMonomials 0:f677e13975d0 274 // currently no executable reports are read
MultipleMonomials 0:f677e13975d0 275 }
MultipleMonomials 0:f677e13975d0 276 else if(shtpHeader[2] == CHANNEL_COMMAND)
MultipleMonomials 0:f677e13975d0 277 {
MultipleMonomials 0:f677e13975d0 278
MultipleMonomials 0:f677e13975d0 279 }
MultipleMonomials 0:f677e13975d0 280 else if(shtpHeader[2] == CHANNEL_REPORTS || shtpHeader[2] == CHANNEL_WAKE_REPORTS)
MultipleMonomials 0:f677e13975d0 281 {
MultipleMonomials 0:f677e13975d0 282 if(shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP)
MultipleMonomials 0:f677e13975d0 283 {
MultipleMonomials 0:f677e13975d0 284 // sensor data packet
MultipleMonomials 0:f677e13975d0 285 parseSensorDataPacket();
MultipleMonomials 0:f677e13975d0 286 }
MultipleMonomials 0:f677e13975d0 287 }
MultipleMonomials 0:f677e13975d0 288 }
MultipleMonomials 0:f677e13975d0 289
MultipleMonomials 0:f677e13975d0 290 // sizes of various sensor data packet elements
MultipleMonomials 0:f677e13975d0 291 #define SIZEOF_BASE_TIMESTAMP 5
MultipleMonomials 0:f677e13975d0 292 #define SIZEOF_TIMESTAMP_REBASE 5
MultipleMonomials 0:f677e13975d0 293 #define SIZEOF_ACCELEROMETER 10
MultipleMonomials 0:f677e13975d0 294 #define SIZEOF_LINEAR_ACCELERATION 10
MultipleMonomials 0:f677e13975d0 295 #define SIZEOF_GYROSCOPE_CALIBRATED 10
MultipleMonomials 0:f677e13975d0 296 #define SIZEOF_MAGNETIC_FIELD_CALIBRATED 10
MultipleMonomials 0:f677e13975d0 297 #define SIZEOF_ROTATION_VECTOR 14
MultipleMonomials 0:f677e13975d0 298 #define SIZEOF_GAME_ROTATION_VECTOR 12
MultipleMonomials 0:f677e13975d0 299 #define SIZEOF_GEOMAGNETIC_ROTATION_VECTOR 14
MultipleMonomials 0:f677e13975d0 300 #define SIZEOF_TAP_DETECTOR 5
MultipleMonomials 0:f677e13975d0 301
MultipleMonomials 0:f677e13975d0 302
MultipleMonomials 0:f677e13975d0 303 void BNO080::parseSensorDataPacket()
MultipleMonomials 0:f677e13975d0 304 {
MultipleMonomials 0:f677e13975d0 305 size_t currReportOffset = 0;
MultipleMonomials 0:f677e13975d0 306
MultipleMonomials 0:f677e13975d0 307 // every sensor data report first contains a timestamp offset to show how long it has been between when
MultipleMonomials 0:f677e13975d0 308 // the host interrupt was sent and when the packet was transmitted.
MultipleMonomials 0:f677e13975d0 309 // We don't use interrupts and don't care about times, so we can throw this out.
MultipleMonomials 0:f677e13975d0 310 currReportOffset += SIZEOF_BASE_TIMESTAMP;
MultipleMonomials 0:f677e13975d0 311
MultipleMonomials 0:f677e13975d0 312 while(currReportOffset < packetLength)
MultipleMonomials 0:f677e13975d0 313 {
MultipleMonomials 0:f677e13975d0 314 // lots of sensor reports use 3 16-bit numbers stored in bytes 4 through 9
MultipleMonomials 0:f677e13975d0 315 // we can save some time by parsing those out here.
MultipleMonomials 0:f677e13975d0 316 uint16_t data1 = (uint16_t)shtpData[currReportOffset + 5] << 8 | shtpData[currReportOffset + 4];
MultipleMonomials 0:f677e13975d0 317 uint16_t data2 = (uint16_t)shtpData[currReportOffset + 7] << 8 | shtpData[currReportOffset + 6];
MultipleMonomials 0:f677e13975d0 318 uint16_t data3 = (uint16_t)shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8];
MultipleMonomials 0:f677e13975d0 319
MultipleMonomials 0:f677e13975d0 320 uint8_t reportNum = shtpData[currReportOffset];
MultipleMonomials 0:f677e13975d0 321
MultipleMonomials 0:f677e13975d0 322 if(reportNum != SENSOR_REPORTID_TIMESTAMP_REBASE)
MultipleMonomials 0:f677e13975d0 323 {
MultipleMonomials 0:f677e13975d0 324 // set status from byte 2
MultipleMonomials 0:f677e13975d0 325 reportStatus[reportNum] = static_cast<uint8_t>(shtpData[currReportOffset + 2] & 0b11);
MultipleMonomials 0:f677e13975d0 326 }
MultipleMonomials 0:f677e13975d0 327
MultipleMonomials 0:f677e13975d0 328 switch(shtpData[currReportOffset])
MultipleMonomials 0:f677e13975d0 329 {
MultipleMonomials 0:f677e13975d0 330 case SENSOR_REPORTID_TIMESTAMP_REBASE:
MultipleMonomials 0:f677e13975d0 331 currReportOffset += SIZEOF_TIMESTAMP_REBASE;
MultipleMonomials 0:f677e13975d0 332 break;
MultipleMonomials 0:f677e13975d0 333
MultipleMonomials 0:f677e13975d0 334 case SENSOR_REPORTID_ACCELEROMETER:
MultipleMonomials 0:f677e13975d0 335
MultipleMonomials 0:f677e13975d0 336 totalAcceleration = TVector3(
MultipleMonomials 0:f677e13975d0 337 qToFloat(data1, ACCELEROMETER_Q_POINT),
MultipleMonomials 0:f677e13975d0 338 qToFloat(data2, ACCELEROMETER_Q_POINT),
MultipleMonomials 0:f677e13975d0 339 qToFloat(data3, ACCELEROMETER_Q_POINT));
MultipleMonomials 0:f677e13975d0 340
MultipleMonomials 0:f677e13975d0 341 currReportOffset += SIZEOF_ACCELEROMETER;
MultipleMonomials 0:f677e13975d0 342 break;
MultipleMonomials 0:f677e13975d0 343
MultipleMonomials 0:f677e13975d0 344 case SENSOR_REPORTID_LINEAR_ACCELERATION:
MultipleMonomials 0:f677e13975d0 345
MultipleMonomials 0:f677e13975d0 346 linearAcceleration = TVector3(
MultipleMonomials 0:f677e13975d0 347 qToFloat(data1, ACCELEROMETER_Q_POINT),
MultipleMonomials 0:f677e13975d0 348 qToFloat(data2, ACCELEROMETER_Q_POINT),
MultipleMonomials 0:f677e13975d0 349 qToFloat(data3, ACCELEROMETER_Q_POINT));
MultipleMonomials 0:f677e13975d0 350
MultipleMonomials 0:f677e13975d0 351 currReportOffset += SIZEOF_LINEAR_ACCELERATION;
MultipleMonomials 0:f677e13975d0 352 break;
MultipleMonomials 0:f677e13975d0 353
MultipleMonomials 0:f677e13975d0 354 case SENSOR_REPORTID_GRAVITY:
MultipleMonomials 0:f677e13975d0 355
MultipleMonomials 0:f677e13975d0 356 gravityAcceleration = TVector3(
MultipleMonomials 0:f677e13975d0 357 qToFloat(data1, ACCELEROMETER_Q_POINT),
MultipleMonomials 0:f677e13975d0 358 qToFloat(data2, ACCELEROMETER_Q_POINT),
MultipleMonomials 0:f677e13975d0 359 qToFloat(data3, ACCELEROMETER_Q_POINT));
MultipleMonomials 0:f677e13975d0 360
MultipleMonomials 0:f677e13975d0 361 currReportOffset += SIZEOF_LINEAR_ACCELERATION;
MultipleMonomials 0:f677e13975d0 362 break;
MultipleMonomials 0:f677e13975d0 363
MultipleMonomials 0:f677e13975d0 364 case SENSOR_REPORTID_GYROSCOPE_CALIBRATED:
MultipleMonomials 0:f677e13975d0 365
MultipleMonomials 0:f677e13975d0 366 gyroRotation = TVector3(
MultipleMonomials 0:f677e13975d0 367 qToFloat(data1, GYRO_Q_POINT),
MultipleMonomials 0:f677e13975d0 368 qToFloat(data2, GYRO_Q_POINT),
MultipleMonomials 0:f677e13975d0 369 qToFloat(data3, GYRO_Q_POINT));
MultipleMonomials 0:f677e13975d0 370
MultipleMonomials 0:f677e13975d0 371 currReportOffset += SIZEOF_GYROSCOPE_CALIBRATED;
MultipleMonomials 0:f677e13975d0 372 break;
MultipleMonomials 0:f677e13975d0 373
MultipleMonomials 0:f677e13975d0 374 case SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED:
MultipleMonomials 0:f677e13975d0 375
MultipleMonomials 0:f677e13975d0 376 magField = TVector3(
MultipleMonomials 0:f677e13975d0 377 qToFloat(data1, MAGNETOMETER_Q_POINT),
MultipleMonomials 0:f677e13975d0 378 qToFloat(data2, MAGNETOMETER_Q_POINT),
MultipleMonomials 0:f677e13975d0 379 qToFloat(data3, MAGNETOMETER_Q_POINT));
MultipleMonomials 0:f677e13975d0 380
MultipleMonomials 0:f677e13975d0 381 currReportOffset += SIZEOF_MAGNETIC_FIELD_CALIBRATED;
MultipleMonomials 0:f677e13975d0 382 break;
MultipleMonomials 0:f677e13975d0 383
MultipleMonomials 0:f677e13975d0 384 case SENSOR_REPORTID_ROTATION_VECTOR:
MultipleMonomials 0:f677e13975d0 385 {
MultipleMonomials 0:f677e13975d0 386 uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
MultipleMonomials 0:f677e13975d0 387 uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12];
MultipleMonomials 0:f677e13975d0 388
MultipleMonomials 0:f677e13975d0 389 rotationVector = TVector4(
MultipleMonomials 0:f677e13975d0 390 qToFloat(data1, ROTATION_Q_POINT),
MultipleMonomials 0:f677e13975d0 391 qToFloat(data2, ROTATION_Q_POINT),
MultipleMonomials 0:f677e13975d0 392 qToFloat(data3, ROTATION_Q_POINT),
MultipleMonomials 0:f677e13975d0 393 qToFloat(realPartQ, ROTATION_Q_POINT));
MultipleMonomials 0:f677e13975d0 394
MultipleMonomials 0:f677e13975d0 395 rotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT);
MultipleMonomials 0:f677e13975d0 396
MultipleMonomials 0:f677e13975d0 397 currReportOffset += SIZEOF_ROTATION_VECTOR;
MultipleMonomials 0:f677e13975d0 398 }
MultipleMonomials 0:f677e13975d0 399 break;
MultipleMonomials 0:f677e13975d0 400
MultipleMonomials 0:f677e13975d0 401 case SENSOR_REPORTID_GAME_ROTATION_VECTOR:
MultipleMonomials 0:f677e13975d0 402 {
MultipleMonomials 0:f677e13975d0 403 uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
MultipleMonomials 0:f677e13975d0 404
MultipleMonomials 0:f677e13975d0 405 gameRotationVector = TVector4(
MultipleMonomials 0:f677e13975d0 406 qToFloat(data1, ROTATION_Q_POINT),
MultipleMonomials 0:f677e13975d0 407 qToFloat(data2, ROTATION_Q_POINT),
MultipleMonomials 0:f677e13975d0 408 qToFloat(data3, ROTATION_Q_POINT),
MultipleMonomials 0:f677e13975d0 409 qToFloat(realPartQ, ROTATION_Q_POINT));
MultipleMonomials 0:f677e13975d0 410
MultipleMonomials 0:f677e13975d0 411 currReportOffset += SIZEOF_GAME_ROTATION_VECTOR;
MultipleMonomials 0:f677e13975d0 412 }
MultipleMonomials 0:f677e13975d0 413 break;
MultipleMonomials 0:f677e13975d0 414
MultipleMonomials 0:f677e13975d0 415 case SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR:
MultipleMonomials 0:f677e13975d0 416 {
MultipleMonomials 0:f677e13975d0 417 uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
MultipleMonomials 0:f677e13975d0 418 uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12];
MultipleMonomials 0:f677e13975d0 419
MultipleMonomials 0:f677e13975d0 420 geomagneticRotationVector = TVector4(
MultipleMonomials 0:f677e13975d0 421 qToFloat(data1, ROTATION_Q_POINT),
MultipleMonomials 0:f677e13975d0 422 qToFloat(data2, ROTATION_Q_POINT),
MultipleMonomials 0:f677e13975d0 423 qToFloat(data3, ROTATION_Q_POINT),
MultipleMonomials 0:f677e13975d0 424 qToFloat(realPartQ, ROTATION_Q_POINT));
MultipleMonomials 0:f677e13975d0 425
MultipleMonomials 0:f677e13975d0 426 geomagneticRotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT);
MultipleMonomials 0:f677e13975d0 427
MultipleMonomials 0:f677e13975d0 428 currReportOffset += SIZEOF_GEOMAGNETIC_ROTATION_VECTOR;
MultipleMonomials 0:f677e13975d0 429 }
MultipleMonomials 0:f677e13975d0 430 break;
MultipleMonomials 0:f677e13975d0 431
MultipleMonomials 0:f677e13975d0 432 case SENSOR_REPORTID_TAP_DETECTOR:
MultipleMonomials 0:f677e13975d0 433
MultipleMonomials 0:f677e13975d0 434 // since we got the report, a tap was detected
MultipleMonomials 0:f677e13975d0 435 tapDetected = true;
MultipleMonomials 0:f677e13975d0 436
MultipleMonomials 0:f677e13975d0 437 doubleTap = (shtpData[currReportOffset + 4] & (1 << 6)) != 0;
MultipleMonomials 0:f677e13975d0 438
MultipleMonomials 0:f677e13975d0 439 currReportOffset += SIZEOF_TAP_DETECTOR;
MultipleMonomials 0:f677e13975d0 440 break;
MultipleMonomials 0:f677e13975d0 441
MultipleMonomials 0:f677e13975d0 442 default:
MultipleMonomials 0:f677e13975d0 443 _debugPort->printf("Error: unrecognized report ID in sensor report: %hhx. Byte %u, length %hu\n", shtpData[currReportOffset], currReportOffset, packetLength);
MultipleMonomials 0:f677e13975d0 444 return;
MultipleMonomials 0:f677e13975d0 445 }
MultipleMonomials 0:f677e13975d0 446 }
MultipleMonomials 0:f677e13975d0 447
MultipleMonomials 0:f677e13975d0 448 }
MultipleMonomials 0:f677e13975d0 449
MultipleMonomials 0:f677e13975d0 450 bool BNO080::waitForPacket(int channel, uint8_t reportID, float timeout)
MultipleMonomials 0:f677e13975d0 451 {
MultipleMonomials 0:f677e13975d0 452 Timer timeoutTimer;
MultipleMonomials 0:f677e13975d0 453 timeoutTimer.start();
MultipleMonomials 0:f677e13975d0 454
MultipleMonomials 0:f677e13975d0 455 while(timeoutTimer.read() <= timeout)
MultipleMonomials 0:f677e13975d0 456 {
MultipleMonomials 0:f677e13975d0 457 if(_int.read() == 0)
MultipleMonomials 0:f677e13975d0 458 {
MultipleMonomials 0:f677e13975d0 459 if(!receivePacket(timeout))
MultipleMonomials 0:f677e13975d0 460 {
MultipleMonomials 0:f677e13975d0 461 return false;
MultipleMonomials 0:f677e13975d0 462 }
MultipleMonomials 0:f677e13975d0 463
MultipleMonomials 0:f677e13975d0 464 if(channel == shtpHeader[2] && reportID == shtpData[0])
MultipleMonomials 0:f677e13975d0 465 {
MultipleMonomials 0:f677e13975d0 466 // found correct packet!
MultipleMonomials 0:f677e13975d0 467 return true;
MultipleMonomials 0:f677e13975d0 468 }
MultipleMonomials 0:f677e13975d0 469 else
MultipleMonomials 0:f677e13975d0 470 {
MultipleMonomials 0:f677e13975d0 471 // other data packet, send to proper channels
MultipleMonomials 0:f677e13975d0 472 processPacket();
MultipleMonomials 0:f677e13975d0 473 }
MultipleMonomials 0:f677e13975d0 474 }
MultipleMonomials 0:f677e13975d0 475 }
MultipleMonomials 0:f677e13975d0 476
MultipleMonomials 0:f677e13975d0 477 _debugPort->printf("Packet wait timeout.\n");
MultipleMonomials 0:f677e13975d0 478 return false;
MultipleMonomials 0:f677e13975d0 479 }
MultipleMonomials 0:f677e13975d0 480
MultipleMonomials 0:f677e13975d0 481 //Given a register value and a Q point, convert to float
MultipleMonomials 0:f677e13975d0 482 //See https://en.wikipedia.org/wiki/Q_(number_format)
MultipleMonomials 0:f677e13975d0 483 float BNO080::qToFloat(int16_t fixedPointValue, uint8_t qPoint)
MultipleMonomials 0:f677e13975d0 484 {
MultipleMonomials 0:f677e13975d0 485 float qFloat = fixedPointValue;
MultipleMonomials 0:f677e13975d0 486 qFloat *= pow(2, qPoint * -1);
MultipleMonomials 0:f677e13975d0 487 return (qFloat);
MultipleMonomials 0:f677e13975d0 488 }
MultipleMonomials 0:f677e13975d0 489
MultipleMonomials 0:f677e13975d0 490 //Given a floating point value and a Q point, convert to Q
MultipleMonomials 0:f677e13975d0 491 //See https://en.wikipedia.org/wiki/Q_(number_format)
MultipleMonomials 0:f677e13975d0 492 int16_t BNO080::floatToQ(float qFloat, uint8_t qPoint)
MultipleMonomials 0:f677e13975d0 493 {
MultipleMonomials 0:f677e13975d0 494 int16_t qVal = static_cast<int16_t>(qFloat * pow(2, qPoint));
MultipleMonomials 0:f677e13975d0 495 return qVal;
MultipleMonomials 0:f677e13975d0 496 }
MultipleMonomials 0:f677e13975d0 497
MultipleMonomials 0:f677e13975d0 498 //Tell the sensor to do a command
MultipleMonomials 0:f677e13975d0 499 //See 6.3.8 page 41, Command request
MultipleMonomials 0:f677e13975d0 500 //The caller is expected to set P0 through P8 prior to calling
MultipleMonomials 0:f677e13975d0 501 void BNO080::sendCommand(uint8_t command)
MultipleMonomials 0:f677e13975d0 502 {
MultipleMonomials 0:f677e13975d0 503 shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request
MultipleMonomials 0:f677e13975d0 504 shtpData[1] = commandSequenceNumber++; //Increments automatically each function call
MultipleMonomials 0:f677e13975d0 505 shtpData[2] = command; //Command
MultipleMonomials 0:f677e13975d0 506
MultipleMonomials 0:f677e13975d0 507 //Caller must set these
MultipleMonomials 0:f677e13975d0 508 /*shtpData[3] = 0; //P0
MultipleMonomials 0:f677e13975d0 509 shtpData[4] = 0; //P1
MultipleMonomials 0:f677e13975d0 510 shtpData[5] = 0; //P2
MultipleMonomials 0:f677e13975d0 511 shtpData[6] = 0;
MultipleMonomials 0:f677e13975d0 512 shtpData[7] = 0;
MultipleMonomials 0:f677e13975d0 513 shtpData[8] = 0;
MultipleMonomials 0:f677e13975d0 514 shtpData[9] = 0;
MultipleMonomials 0:f677e13975d0 515 shtpData[10] = 0;
MultipleMonomials 0:f677e13975d0 516 shtpData[11] = 0;*/
MultipleMonomials 0:f677e13975d0 517
MultipleMonomials 0:f677e13975d0 518 //Transmit packet on channel 2, 12 bytes
MultipleMonomials 0:f677e13975d0 519 sendPacket(CHANNEL_CONTROL, 12);
MultipleMonomials 0:f677e13975d0 520 }
MultipleMonomials 0:f677e13975d0 521
MultipleMonomials 0:f677e13975d0 522 //Given a sensor's report ID, this tells the BNO080 to begin reporting the values
MultipleMonomials 0:f677e13975d0 523 //Also sets the specific config word. Useful for personal activity classifier
MultipleMonomials 0:f677e13975d0 524 void BNO080::setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig)
MultipleMonomials 0:f677e13975d0 525 {
MultipleMonomials 0:f677e13975d0 526 uint32_t microsBetweenReports = static_cast<uint32_t>(timeBetweenReports * 1000);
MultipleMonomials 0:f677e13975d0 527
MultipleMonomials 0:f677e13975d0 528 const uint32_t batchMicros = 0;
MultipleMonomials 0:f677e13975d0 529
MultipleMonomials 0:f677e13975d0 530 shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55
MultipleMonomials 0:f677e13975d0 531 shtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
MultipleMonomials 0:f677e13975d0 532 shtpData[2] = 0; //Feature flags
MultipleMonomials 0:f677e13975d0 533 shtpData[3] = 0; //Change sensitivity (LSB)
MultipleMonomials 0:f677e13975d0 534 shtpData[4] = 0; //Change sensitivity (MSB)
MultipleMonomials 0:f677e13975d0 535 shtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms
MultipleMonomials 0:f677e13975d0 536 shtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval
MultipleMonomials 0:f677e13975d0 537 shtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval
MultipleMonomials 0:f677e13975d0 538 shtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
MultipleMonomials 0:f677e13975d0 539 shtpData[9] = (batchMicros >> 0) & 0xFF; //Batch Interval (LSB)
MultipleMonomials 0:f677e13975d0 540 shtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval
MultipleMonomials 0:f677e13975d0 541 shtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval
MultipleMonomials 0:f677e13975d0 542 shtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB)
MultipleMonomials 0:f677e13975d0 543 shtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB)
MultipleMonomials 0:f677e13975d0 544 shtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config
MultipleMonomials 0:f677e13975d0 545 shtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config
MultipleMonomials 0:f677e13975d0 546 shtpData[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB)
MultipleMonomials 0:f677e13975d0 547
MultipleMonomials 0:f677e13975d0 548 //Transmit packet on channel 2, 17 bytes
MultipleMonomials 0:f677e13975d0 549 sendPacket(CHANNEL_CONTROL, 17);
MultipleMonomials 0:f677e13975d0 550 }
MultipleMonomials 0:f677e13975d0 551
MultipleMonomials 0:f677e13975d0 552
MultipleMonomials 0:f677e13975d0 553 //Given the data packet, send the header then the data
MultipleMonomials 0:f677e13975d0 554 //Returns false if sensor does not ACK
MultipleMonomials 0:f677e13975d0 555 bool BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength)
MultipleMonomials 0:f677e13975d0 556 {
MultipleMonomials 0:f677e13975d0 557 // start the transaction and contact the IMU
MultipleMonomials 0:f677e13975d0 558 _i2cPort.start();
MultipleMonomials 0:f677e13975d0 559
MultipleMonomials 0:f677e13975d0 560 // to indicate an i2c read, shift the 7 bit address up 1 bit and keep bit 0 as a 0
MultipleMonomials 0:f677e13975d0 561 int writeResult = _i2cPort.write(_i2cAddress << 1);
MultipleMonomials 0:f677e13975d0 562
MultipleMonomials 0:f677e13975d0 563 if(writeResult != 1)
MultipleMonomials 0:f677e13975d0 564 {
MultipleMonomials 0:f677e13975d0 565 _debugPort->printf("BNO I2C write failed!\n");
MultipleMonomials 0:f677e13975d0 566 _scope = 0;
MultipleMonomials 0:f677e13975d0 567 _i2cPort.stop();
MultipleMonomials 0:f677e13975d0 568 return false;
MultipleMonomials 0:f677e13975d0 569 }
MultipleMonomials 0:f677e13975d0 570
MultipleMonomials 0:f677e13975d0 571
MultipleMonomials 0:f677e13975d0 572 uint16_t totalLength = dataLength + 4; //Add four bytes for the header
MultipleMonomials 0:f677e13975d0 573 packetLength = dataLength;
MultipleMonomials 0:f677e13975d0 574
MultipleMonomials 0:f677e13975d0 575 #if BNO_DEBUG
MultipleMonomials 0:f677e13975d0 576 shtpHeader[0] = totalLength & 0xFF;
MultipleMonomials 0:f677e13975d0 577 shtpHeader[1] = totalLength >> 8;
MultipleMonomials 0:f677e13975d0 578 shtpHeader[2] = channelNumber;
MultipleMonomials 0:f677e13975d0 579 shtpHeader[3] = sequenceNumber[channelNumber];
MultipleMonomials 0:f677e13975d0 580
MultipleMonomials 0:f677e13975d0 581 _debugPort->printf("Transmitting packet: ----------------\n");
MultipleMonomials 0:f677e13975d0 582 printPacket();
MultipleMonomials 0:f677e13975d0 583 #endif
MultipleMonomials 0:f677e13975d0 584
MultipleMonomials 0:f677e13975d0 585 //Send the 4 byte packet header
MultipleMonomials 0:f677e13975d0 586 _i2cPort.write(totalLength & 0xFF); //Packet length LSB
MultipleMonomials 0:f677e13975d0 587 _i2cPort.write(totalLength >> 8); //Packet length MSB
MultipleMonomials 0:f677e13975d0 588 _i2cPort.write(channelNumber); //Channel number
MultipleMonomials 0:f677e13975d0 589 _i2cPort.write(sequenceNumber[channelNumber]++); //Send the sequence number, increments with each packet sent, different counter for each channel
MultipleMonomials 0:f677e13975d0 590
MultipleMonomials 0:f677e13975d0 591 //Send the user's data packet
MultipleMonomials 0:f677e13975d0 592 for (uint8_t i = 0 ; i < dataLength ; i++)
MultipleMonomials 0:f677e13975d0 593 {
MultipleMonomials 0:f677e13975d0 594 _i2cPort.write(shtpData[i]);
MultipleMonomials 0:f677e13975d0 595 }
MultipleMonomials 0:f677e13975d0 596 _i2cPort.stop();
MultipleMonomials 0:f677e13975d0 597
MultipleMonomials 0:f677e13975d0 598 return (true);
MultipleMonomials 0:f677e13975d0 599 }
MultipleMonomials 0:f677e13975d0 600
MultipleMonomials 0:f677e13975d0 601 //Check to see if there is any new data available
MultipleMonomials 0:f677e13975d0 602 //Read the contents of the incoming packet into the shtpData array
MultipleMonomials 0:f677e13975d0 603 bool BNO080::receivePacket(float timeout)
MultipleMonomials 0:f677e13975d0 604 {
MultipleMonomials 0:f677e13975d0 605 Timer waitStartTime;
MultipleMonomials 0:f677e13975d0 606 waitStartTime.start();
MultipleMonomials 0:f677e13975d0 607
MultipleMonomials 0:f677e13975d0 608 while(_int.read() != 0)
MultipleMonomials 0:f677e13975d0 609 {
MultipleMonomials 0:f677e13975d0 610 if(waitStartTime.read() > timeout)
MultipleMonomials 0:f677e13975d0 611 {
MultipleMonomials 0:f677e13975d0 612 _debugPort->printf("BNO I2C wait timeout\n");
MultipleMonomials 0:f677e13975d0 613 return false;
MultipleMonomials 0:f677e13975d0 614 }
MultipleMonomials 0:f677e13975d0 615
MultipleMonomials 0:f677e13975d0 616 }
MultipleMonomials 0:f677e13975d0 617
MultipleMonomials 0:f677e13975d0 618 // start the transaction and contact the IMU
MultipleMonomials 0:f677e13975d0 619 _i2cPort.start();
MultipleMonomials 0:f677e13975d0 620
MultipleMonomials 0:f677e13975d0 621 // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1
MultipleMonomials 0:f677e13975d0 622 int writeResult = _i2cPort.write((_i2cAddress << 1) | 0x1);
MultipleMonomials 0:f677e13975d0 623
MultipleMonomials 0:f677e13975d0 624 if(writeResult != 1)
MultipleMonomials 0:f677e13975d0 625 {
MultipleMonomials 0:f677e13975d0 626 _debugPort->printf("BNO I2C read failed!\n");
MultipleMonomials 0:f677e13975d0 627 return false;
MultipleMonomials 0:f677e13975d0 628 }
MultipleMonomials 0:f677e13975d0 629
MultipleMonomials 0:f677e13975d0 630 //Get the first four bytes, aka the packet header
MultipleMonomials 0:f677e13975d0 631 uint8_t packetLSB = static_cast<uint8_t>(_i2cPort.read(true));
MultipleMonomials 0:f677e13975d0 632 uint8_t packetMSB = static_cast<uint8_t>(_i2cPort.read(true));
MultipleMonomials 0:f677e13975d0 633 uint8_t channelNumber = static_cast<uint8_t>(_i2cPort.read(true));
MultipleMonomials 0:f677e13975d0 634 uint8_t sequenceNum = static_cast<uint8_t>(_i2cPort.read(true)); //Not sure if we need to store this or not
MultipleMonomials 0:f677e13975d0 635
MultipleMonomials 0:f677e13975d0 636 //Store the header info
MultipleMonomials 0:f677e13975d0 637 shtpHeader[0] = packetLSB;
MultipleMonomials 0:f677e13975d0 638 shtpHeader[1] = packetMSB;
MultipleMonomials 0:f677e13975d0 639 shtpHeader[2] = channelNumber;
MultipleMonomials 0:f677e13975d0 640 shtpHeader[3] = sequenceNum;
MultipleMonomials 0:f677e13975d0 641
MultipleMonomials 0:f677e13975d0 642 if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF)
MultipleMonomials 0:f677e13975d0 643 {
MultipleMonomials 0:f677e13975d0 644 // invalid according to BNO080 datasheet section 1.4.1
MultipleMonomials 0:f677e13975d0 645
MultipleMonomials 0:f677e13975d0 646 #if BNO_DEBUG
MultipleMonomials 0:f677e13975d0 647 _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n");
MultipleMonomials 0:f677e13975d0 648 #endif
MultipleMonomials 0:f677e13975d0 649 return false;
MultipleMonomials 0:f677e13975d0 650 }
MultipleMonomials 0:f677e13975d0 651
MultipleMonomials 0:f677e13975d0 652 //Calculate the number of data bytes in this packet
MultipleMonomials 0:f677e13975d0 653 packetLength = (static_cast<uint16_t>(packetMSB) << 8 | packetLSB);
MultipleMonomials 0:f677e13975d0 654
MultipleMonomials 0:f677e13975d0 655 // Clear the MSbit.
MultipleMonomials 0:f677e13975d0 656 // 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)
MultipleMonomials 0:f677e13975d0 657 // but we don't actually care about any of the advertisement packets
MultipleMonomials 0:f677e13975d0 658 // that use this, so we can just cut off the rest of the packet by releasing chip select.
MultipleMonomials 0:f677e13975d0 659 packetLength &= ~(1 << 15);
MultipleMonomials 0:f677e13975d0 660
MultipleMonomials 0:f677e13975d0 661 if (packetLength == 0)
MultipleMonomials 0:f677e13975d0 662 {
MultipleMonomials 0:f677e13975d0 663 // Packet is empty
MultipleMonomials 0:f677e13975d0 664 return (false); //All done
MultipleMonomials 0:f677e13975d0 665 }
MultipleMonomials 0:f677e13975d0 666
MultipleMonomials 0:f677e13975d0 667 packetLength -= 4; //Remove the header bytes from the data count
MultipleMonomials 0:f677e13975d0 668
MultipleMonomials 0:f677e13975d0 669 //Read incoming data into the shtpData array
MultipleMonomials 0:f677e13975d0 670 for (uint16_t dataSpot = 0 ; dataSpot < packetLength ; dataSpot++)
MultipleMonomials 0:f677e13975d0 671 {
MultipleMonomials 0:f677e13975d0 672 bool sendACK = dataSpot < packetLength - 1;
MultipleMonomials 0:f677e13975d0 673
MultipleMonomials 0:f677e13975d0 674 // per the datasheet, 0xFF is used as filler for the receiver to transmit back
MultipleMonomials 0:f677e13975d0 675 uint8_t incoming = static_cast<uint8_t>(_i2cPort.read(sendACK));
MultipleMonomials 0:f677e13975d0 676 if (dataSpot < STORED_PACKET_SIZE) //BNO080 can respond with upto 270 bytes, avoid overflow
MultipleMonomials 0:f677e13975d0 677 shtpData[dataSpot] = incoming; //Store data into the shtpData array
MultipleMonomials 0:f677e13975d0 678 }
MultipleMonomials 0:f677e13975d0 679
MultipleMonomials 0:f677e13975d0 680 _i2cPort.stop();
MultipleMonomials 0:f677e13975d0 681
MultipleMonomials 0:f677e13975d0 682 #if BNO_DEBUG
MultipleMonomials 0:f677e13975d0 683 _debugPort->printf("Recieved packet: ----------------\n");
MultipleMonomials 0:f677e13975d0 684 printPacket(); // note: add 4 for the header length
MultipleMonomials 0:f677e13975d0 685 #endif
MultipleMonomials 0:f677e13975d0 686
MultipleMonomials 0:f677e13975d0 687 return (true); //We're done!
MultipleMonomials 0:f677e13975d0 688 }
MultipleMonomials 0:f677e13975d0 689
MultipleMonomials 0:f677e13975d0 690 //Pretty prints the contents of the current shtp header and data packets
MultipleMonomials 0:f677e13975d0 691 void BNO080::printPacket()
MultipleMonomials 0:f677e13975d0 692 {
MultipleMonomials 0:f677e13975d0 693 #if BNO_DEBUG
MultipleMonomials 0:f677e13975d0 694 //Print the four byte header
MultipleMonomials 0:f677e13975d0 695 _debugPort->printf("Header:");
MultipleMonomials 0:f677e13975d0 696 for (uint8_t x = 0 ; x < 4 ; x++)
MultipleMonomials 0:f677e13975d0 697 {
MultipleMonomials 0:f677e13975d0 698 _debugPort->printf(" ");
MultipleMonomials 0:f677e13975d0 699 if (shtpHeader[x] < 0x10) _debugPort->printf("0");
MultipleMonomials 0:f677e13975d0 700 _debugPort->printf("%hhx", shtpHeader[x]);
MultipleMonomials 0:f677e13975d0 701 }
MultipleMonomials 0:f677e13975d0 702
MultipleMonomials 0:f677e13975d0 703 uint16_t printLength = packetLength;
MultipleMonomials 0:f677e13975d0 704 if (printLength > 40) printLength = 40; //Artificial limit. We don't want the phone book.
MultipleMonomials 0:f677e13975d0 705
MultipleMonomials 0:f677e13975d0 706 _debugPort->printf(" Body:");
MultipleMonomials 0:f677e13975d0 707 for (uint16_t x = 0 ; x < printLength ; x++)
MultipleMonomials 0:f677e13975d0 708 {
MultipleMonomials 0:f677e13975d0 709 _debugPort->printf(" ");
MultipleMonomials 0:f677e13975d0 710 if (shtpData[x] < 0x10) _debugPort->printf("0");
MultipleMonomials 0:f677e13975d0 711 _debugPort->printf("%hhx", shtpData[x]);
MultipleMonomials 0:f677e13975d0 712 }
MultipleMonomials 0:f677e13975d0 713
MultipleMonomials 0:f677e13975d0 714 _debugPort->printf(", Length:");
MultipleMonomials 0:f677e13975d0 715 _debugPort->printf("%hhu", packetLength + SHTP_HEADER_SIZE);
MultipleMonomials 0:f677e13975d0 716
MultipleMonomials 0:f677e13975d0 717 if(shtpHeader[1] >> 7)
MultipleMonomials 0:f677e13975d0 718 {
MultipleMonomials 0:f677e13975d0 719 _debugPort->printf("[C]");
MultipleMonomials 0:f677e13975d0 720 }
MultipleMonomials 0:f677e13975d0 721
MultipleMonomials 0:f677e13975d0 722 _debugPort->printf(", SeqNum: %hhu", shtpHeader[3]);
MultipleMonomials 0:f677e13975d0 723
MultipleMonomials 0:f677e13975d0 724 _debugPort->printf(", Channel:");
MultipleMonomials 0:f677e13975d0 725 if (shtpHeader[2] == 0) _debugPort->printf("Command");
MultipleMonomials 0:f677e13975d0 726 else if (shtpHeader[2] == 1) _debugPort->printf("Executable");
MultipleMonomials 0:f677e13975d0 727 else if (shtpHeader[2] == 2) _debugPort->printf("Control");
MultipleMonomials 0:f677e13975d0 728 else if (shtpHeader[2] == 3) _debugPort->printf("Sensor-report");
MultipleMonomials 0:f677e13975d0 729 else if (shtpHeader[2] == 4) _debugPort->printf("Wake-report");
MultipleMonomials 0:f677e13975d0 730 else if (shtpHeader[2] == 5) _debugPort->printf("Gyro-vector");
MultipleMonomials 0:f677e13975d0 731 else _debugPort->printf("%hhu", shtpHeader[2]);
MultipleMonomials 0:f677e13975d0 732
MultipleMonomials 0:f677e13975d0 733 _debugPort->printf("\n");
MultipleMonomials 0:f677e13975d0 734 #endif
MultipleMonomials 0:f677e13975d0 735 }
MultipleMonomials 0:f677e13975d0 736
MultipleMonomials 0:f677e13975d0 737
MultipleMonomials 0:f677e13975d0 738 void BNO080::zeroBuffer()
MultipleMonomials 0:f677e13975d0 739 {
MultipleMonomials 0:f677e13975d0 740 memset(shtpHeader, 0, SHTP_HEADER_SIZE);
MultipleMonomials 0:f677e13975d0 741 memset(shtpData, 0, STORED_PACKET_SIZE);
MultipleMonomials 0:f677e13975d0 742 packetLength = 0;
MultipleMonomials 0:f677e13975d0 743 }