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:
Jamie Smith
Date:
Sat Dec 29 04:09:34 2018 -0800
Revision:
2:2269b723d16a
Parent:
1:aac28ffd63ed
Child:
3:197ad972fb7c
Backport to C++98

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jamie Smith 1:aac28ffd63ed 1 //
Jamie Smith 1:aac28ffd63ed 2 // USC RPL BNO080 driver.
Jamie Smith 1:aac28ffd63ed 3 //
Jamie Smith 1:aac28ffd63ed 4
Jamie Smith 1:aac28ffd63ed 5 /*
Jamie Smith 1:aac28ffd63ed 6 * Overview of BNO080 Communications
Jamie Smith 1:aac28ffd63ed 7 * ===============================================
Jamie Smith 1:aac28ffd63ed 8 *
Jamie Smith 1:aac28ffd63ed 9 * Hilcrest has developed a protocol called SHTP (Sensor Hub Transport Protocol) for binary communications with
Jamie Smith 1:aac28ffd63ed 10 * the BNO080 and the other IMUs it sells. Over this protocol, SH-2 (Sensor Hub 2) messages are sent to configure
Jamie Smith 1:aac28ffd63ed 11 * the chip and read data back.
Jamie Smith 1:aac28ffd63ed 12 *
Jamie Smith 1:aac28ffd63ed 13 * SHTP messages are divided at two hierarchical levels: first the channel, then the report ID. Each category
Jamie Smith 1:aac28ffd63ed 14 * of messages (system commands, sensor data reports, etc.) has its own channel, and the individual messages
Jamie Smith 1:aac28ffd63ed 15 * in each channel are identified by their report id, which is the first byte of the message payload (note that the
Jamie Smith 1:aac28ffd63ed 16 * datasheets don't *always* call the first byte the report ID, but that byte does identify the report, so I'm going
Jamie Smith 1:aac28ffd63ed 17 * with it).
Jamie Smith 1:aac28ffd63ed 18 *
Jamie Smith 1:aac28ffd63ed 19 * ===============================================
Jamie Smith 1:aac28ffd63ed 20 *
Jamie Smith 1:aac28ffd63ed 21 * Information about the BNO080 is split into three datasheets. Here's the download links and what they cover:
Jamie Smith 1:aac28ffd63ed 22 *
Jamie Smith 1:aac28ffd63ed 23 * - the BNO080 datasheet: http://www.hillcrestlabs.com/download/5a05f340566d07c196001ec1
Jamie Smith 1:aac28ffd63ed 24 * -- Chip pinouts
Jamie Smith 1:aac28ffd63ed 25 * -- Example circuits
Jamie Smith 1:aac28ffd63ed 26 * -- Physical specifications
Jamie Smith 1:aac28ffd63ed 27 * -- Supported reports and configuration settings (at a high level)
Jamie Smith 1:aac28ffd63ed 28 * -- List of packets on the SHTP executable channel
Jamie Smith 1:aac28ffd63ed 29 *
Jamie Smith 1:aac28ffd63ed 30 * - the SHTP protocol: http://www.hillcrestlabs.com/download/59de8f99cd829e94dc0029d7
Jamie Smith 1:aac28ffd63ed 31 * -- SHTP transmit and receive protcols (for SPI, I2C, and UART)
Jamie Smith 1:aac28ffd63ed 32 * -- SHTP binary format
Jamie Smith 1:aac28ffd63ed 33 * -- packet types on the SHTP command channel
Jamie Smith 1:aac28ffd63ed 34 *
Jamie Smith 1:aac28ffd63ed 35 * - the SH-2 reference: http://www.hillcrestlabs.com/download/59de8f398934bf6faa00293f
Jamie Smith 1:aac28ffd63ed 36 * -- list of packets and their formats for all channels other than command and executable
Jamie Smith 1:aac28ffd63ed 37 * -- list of FRS (Flash Record System) entries and their formats
Jamie Smith 1:aac28ffd63ed 38 *
Jamie Smith 1:aac28ffd63ed 39 * ===============================================
Jamie Smith 1:aac28ffd63ed 40 *
Jamie Smith 1:aac28ffd63ed 41 * Overview of SHTP channels:
Jamie Smith 1:aac28ffd63ed 42 *
Jamie Smith 1:aac28ffd63ed 43 * 0 -> Command
Jamie Smith 1:aac28ffd63ed 44 * -- Used for protocol-global packets, currently only the advertisement packet (which lists all the channels) and error reports
Jamie Smith 1:aac28ffd63ed 45 *
Jamie Smith 1:aac28ffd63ed 46 * 1 -> Executable
Jamie Smith 1:aac28ffd63ed 47 * -- Used for things that control the software on the chip: commands to reset and sleep
Jamie Smith 1:aac28ffd63ed 48 * -- Also used by the chip to report when it's done booting up
Jamie Smith 1:aac28ffd63ed 49 *
Jamie Smith 1:aac28ffd63ed 50 * 2 -> Control
Jamie Smith 1:aac28ffd63ed 51 * -- Used to send configuration commands to the IMU and for it to send back responses.
Jamie Smith 1:aac28ffd63ed 52 * -- Common report IDs: Command Request (0xF2), Set Feature (0xFD)
Jamie Smith 1:aac28ffd63ed 53 *
Jamie Smith 1:aac28ffd63ed 54 * 3 -> Sensor Reports
Jamie Smith 1:aac28ffd63ed 55 * -- Used for sensors to send back data reports.
Jamie Smith 1:aac28ffd63ed 56 * -- AFAIK the only report ID on this channel will be 0xFB (Report Base Timestamp); sensor data is send in a series of structures
Jamie Smith 1:aac28ffd63ed 57 * following an 0xFB
Jamie Smith 1:aac28ffd63ed 58 *
Jamie Smith 1:aac28ffd63ed 59 * 4 -> Wake Sensor Reports
Jamie Smith 1:aac28ffd63ed 60 * -- same as above, but for sensors configured to wake the device
Jamie Smith 1:aac28ffd63ed 61 *
Jamie Smith 1:aac28ffd63ed 62 * 5 -> Gyro Rotation Vector
Jamie Smith 1:aac28ffd63ed 63 * -- a dedicated channel for the Gyro Rotation Vector sensor report
Jamie Smith 1:aac28ffd63ed 64 * -- Why does this get its own channel? I don't know!!!
Jamie Smith 1:aac28ffd63ed 65 */
Jamie Smith 1:aac28ffd63ed 66
Jamie Smith 1:aac28ffd63ed 67 #include "BNO080.h"
Jamie Smith 1:aac28ffd63ed 68 #include "BNO080Constants.h"
Jamie Smith 1:aac28ffd63ed 69
Jamie Smith 1:aac28ffd63ed 70 /// Set to 1 to enable debug printouts. Should be very useful if the chip is giving you trouble.
Jamie Smith 1:aac28ffd63ed 71 /// When debugging, it is recommended to use the highest possible serial baudrate so as not to interrupt the timing of operations.
Jamie Smith 2:2269b723d16a 72 #define BNO_DEBUG 0
Jamie Smith 1:aac28ffd63ed 73
Jamie Smith 1:aac28ffd63ed 74 BNO080::BNO080(Serial *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin,
Jamie Smith 1:aac28ffd63ed 75 uint8_t i2cAddress, int i2cPortSpeed) :
Jamie Smith 1:aac28ffd63ed 76 _debugPort(debugPort),
Jamie Smith 1:aac28ffd63ed 77 _i2cPort(user_SDApin, user_SCLpin),
Jamie Smith 1:aac28ffd63ed 78 _i2cAddress(i2cAddress),
Jamie Smith 1:aac28ffd63ed 79 _int(user_INTPin),
Jamie Smith 2:2269b723d16a 80 _rst(user_RSTPin, 1),
Jamie Smith 2:2269b723d16a 81 commandSequenceNumber(0),
Jamie Smith 2:2269b723d16a 82 stability(UNKNOWN),
Jamie Smith 2:2269b723d16a 83 stepDetected(false),
Jamie Smith 2:2269b723d16a 84 stepCount(0),
Jamie Smith 2:2269b723d16a 85 significantMotionDetected(false),
Jamie Smith 2:2269b723d16a 86 shakeDetected(false),
Jamie Smith 2:2269b723d16a 87 xAxisShake(false),
Jamie Smith 2:2269b723d16a 88 yAxisShake(false),
Jamie Smith 2:2269b723d16a 89 zAxisShake(false)
Jamie Smith 1:aac28ffd63ed 90 {
Jamie Smith 2:2269b723d16a 91 // zero sequence numbers
Jamie Smith 2:2269b723d16a 92 memset(sequenceNumber, 0, sizeof(sequenceNumber));
Jamie Smith 2:2269b723d16a 93
Jamie Smith 1:aac28ffd63ed 94 //Get user settings
Jamie Smith 1:aac28ffd63ed 95 _i2cPortSpeed = i2cPortSpeed;
Jamie Smith 1:aac28ffd63ed 96 if(_i2cPortSpeed > 4000000)
Jamie Smith 1:aac28ffd63ed 97 {
Jamie Smith 1:aac28ffd63ed 98 _i2cPortSpeed = 4000000; //BNO080 max is 400Khz
Jamie Smith 1:aac28ffd63ed 99 }
Jamie Smith 1:aac28ffd63ed 100 _i2cPort.frequency(_i2cPortSpeed);
Jamie Smith 1:aac28ffd63ed 101
Jamie Smith 1:aac28ffd63ed 102 }
Jamie Smith 1:aac28ffd63ed 103
Jamie Smith 1:aac28ffd63ed 104 bool BNO080::begin()
Jamie Smith 1:aac28ffd63ed 105 {
Jamie Smith 1:aac28ffd63ed 106 //Configure the BNO080 for SPI communication
Jamie Smith 1:aac28ffd63ed 107
Jamie Smith 1:aac28ffd63ed 108 _rst = 0; // Reset BNO080
Jamie Smith 1:aac28ffd63ed 109 wait(.002f); // Min length not specified in datasheet?
Jamie Smith 1:aac28ffd63ed 110 _rst = 1; // Bring out of reset
Jamie Smith 1:aac28ffd63ed 111
Jamie Smith 1:aac28ffd63ed 112 // wait for a falling edge (NOT just a low) on the INT pin to denote startup
Jamie Smith 1:aac28ffd63ed 113 Timer timeoutTimer;
Jamie Smith 1:aac28ffd63ed 114
Jamie Smith 1:aac28ffd63ed 115 bool highDetected = false;
Jamie Smith 1:aac28ffd63ed 116 bool lowDetected = false;
Jamie Smith 1:aac28ffd63ed 117
Jamie Smith 1:aac28ffd63ed 118 while(true)
Jamie Smith 1:aac28ffd63ed 119 {
Jamie Smith 1:aac28ffd63ed 120 if(timeoutTimer.read() > BNO080_RESET_TIMEOUT)
Jamie Smith 1:aac28ffd63ed 121 {
Jamie Smith 1:aac28ffd63ed 122 _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n");
Jamie Smith 1:aac28ffd63ed 123 return false;
Jamie Smith 1:aac28ffd63ed 124 }
Jamie Smith 1:aac28ffd63ed 125
Jamie Smith 1:aac28ffd63ed 126 // simple edge detector
Jamie Smith 1:aac28ffd63ed 127 if(!highDetected)
Jamie Smith 1:aac28ffd63ed 128 {
Jamie Smith 1:aac28ffd63ed 129 if(_int == 1)
Jamie Smith 1:aac28ffd63ed 130 {
Jamie Smith 1:aac28ffd63ed 131 highDetected = true;
Jamie Smith 1:aac28ffd63ed 132 }
Jamie Smith 1:aac28ffd63ed 133 }
Jamie Smith 1:aac28ffd63ed 134 else if(!lowDetected)
Jamie Smith 1:aac28ffd63ed 135 {
Jamie Smith 1:aac28ffd63ed 136 if(_int == 0)
Jamie Smith 1:aac28ffd63ed 137 {
Jamie Smith 1:aac28ffd63ed 138 lowDetected = true;
Jamie Smith 1:aac28ffd63ed 139 }
Jamie Smith 1:aac28ffd63ed 140 }
Jamie Smith 1:aac28ffd63ed 141 else
Jamie Smith 1:aac28ffd63ed 142 {
Jamie Smith 1:aac28ffd63ed 143 // high and low detected
Jamie Smith 1:aac28ffd63ed 144 break;
Jamie Smith 1:aac28ffd63ed 145 }
Jamie Smith 1:aac28ffd63ed 146 }
Jamie Smith 1:aac28ffd63ed 147
Jamie Smith 1:aac28ffd63ed 148 _debugPort->printf("BNO080 detected!\n");
Jamie Smith 1:aac28ffd63ed 149
Jamie Smith 1:aac28ffd63ed 150 // At system startup, the hub must send its full advertisement message (see SHTP 5.2 and 5.3) to the
Jamie Smith 1:aac28ffd63ed 151 // host. It must not send any other data until this step is complete.
Jamie Smith 1:aac28ffd63ed 152 // We don't actually care what's in it, we're just using it as a signal to indicate that the reset is complete.
Jamie Smith 1:aac28ffd63ed 153 receivePacket();
Jamie Smith 1:aac28ffd63ed 154
Jamie Smith 1:aac28ffd63ed 155 // now, after startup, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command
Jamie Smith 1:aac28ffd63ed 156 waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET);
Jamie Smith 1:aac28ffd63ed 157
Jamie Smith 1:aac28ffd63ed 158 // Next, officially tell it to initialize, and wait for a successful Initialize Response
Jamie Smith 1:aac28ffd63ed 159 zeroBuffer();
Jamie Smith 1:aac28ffd63ed 160 shtpData[3] = 0;
Jamie Smith 1:aac28ffd63ed 161 sendCommand(COMMAND_INITIALIZE);
Jamie Smith 1:aac28ffd63ed 162
Jamie Smith 1:aac28ffd63ed 163
Jamie Smith 1:aac28ffd63ed 164 if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE) || shtpData[2] != COMMAND_INITIALIZE || shtpData[5] != 0)
Jamie Smith 1:aac28ffd63ed 165 {
Jamie Smith 1:aac28ffd63ed 166 _debugPort->printf("BNO080 reports initialization failed.\n");
Jamie Smith 1:aac28ffd63ed 167 __enable_irq();
Jamie Smith 1:aac28ffd63ed 168 return false;
Jamie Smith 1:aac28ffd63ed 169 }
Jamie Smith 1:aac28ffd63ed 170 else
Jamie Smith 1:aac28ffd63ed 171 {
Jamie Smith 1:aac28ffd63ed 172 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 173 _debugPort->printf("BNO080 reports initialization successful!\n");
Jamie Smith 1:aac28ffd63ed 174 #endif
Jamie Smith 1:aac28ffd63ed 175 }
Jamie Smith 1:aac28ffd63ed 176
Jamie Smith 1:aac28ffd63ed 177
Jamie Smith 1:aac28ffd63ed 178 // Finally, we want to interrogate the device about its model and version.
Jamie Smith 1:aac28ffd63ed 179 zeroBuffer();
Jamie Smith 1:aac28ffd63ed 180 shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info
Jamie Smith 1:aac28ffd63ed 181 shtpData[1] = 0; //Reserved
Jamie Smith 1:aac28ffd63ed 182 sendPacket(CHANNEL_CONTROL, 2);
Jamie Smith 1:aac28ffd63ed 183
Jamie Smith 1:aac28ffd63ed 184 waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5);
Jamie Smith 1:aac28ffd63ed 185
Jamie Smith 1:aac28ffd63ed 186 if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE)
Jamie Smith 1:aac28ffd63ed 187 {
Jamie Smith 1:aac28ffd63ed 188 majorSoftwareVersion = shtpData[2];
Jamie Smith 1:aac28ffd63ed 189 minorSoftwareVersion = shtpData[3];
Jamie Smith 1:aac28ffd63ed 190 patchSoftwareVersion = (shtpData[13] << 8) | shtpData[12];
Jamie Smith 1:aac28ffd63ed 191 partNumber = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | shtpData[4];
Jamie Smith 1:aac28ffd63ed 192 buildNumber = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | shtpData[8];
Jamie Smith 1:aac28ffd63ed 193
Jamie Smith 1:aac28ffd63ed 194 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 195 _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n",
Jamie Smith 1:aac28ffd63ed 196 majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion,
Jamie Smith 1:aac28ffd63ed 197 buildNumber, partNumber);
Jamie Smith 1:aac28ffd63ed 198 #endif
Jamie Smith 1:aac28ffd63ed 199
Jamie Smith 1:aac28ffd63ed 200 }
Jamie Smith 1:aac28ffd63ed 201 else
Jamie Smith 1:aac28ffd63ed 202 {
Jamie Smith 1:aac28ffd63ed 203 _debugPort->printf("Bad response from product ID command.\n");
Jamie Smith 1:aac28ffd63ed 204 return false;
Jamie Smith 1:aac28ffd63ed 205 }
Jamie Smith 1:aac28ffd63ed 206
Jamie Smith 1:aac28ffd63ed 207 // successful init
Jamie Smith 1:aac28ffd63ed 208 return true;
Jamie Smith 1:aac28ffd63ed 209
Jamie Smith 1:aac28ffd63ed 210 }
Jamie Smith 1:aac28ffd63ed 211
Jamie Smith 1:aac28ffd63ed 212 void BNO080::tare(bool zOnly)
Jamie Smith 1:aac28ffd63ed 213 {
Jamie Smith 1:aac28ffd63ed 214 zeroBuffer();
Jamie Smith 1:aac28ffd63ed 215
Jamie Smith 1:aac28ffd63ed 216 // from SH-2 section 6.4.4.1
Jamie Smith 1:aac28ffd63ed 217 shtpData[3] = 0; // perform tare now
Jamie Smith 1:aac28ffd63ed 218
Jamie Smith 1:aac28ffd63ed 219 if(zOnly)
Jamie Smith 1:aac28ffd63ed 220 {
Jamie Smith 1:aac28ffd63ed 221 shtpData[4] = 0b100; // tare Z axis
Jamie Smith 1:aac28ffd63ed 222 }
Jamie Smith 1:aac28ffd63ed 223 else
Jamie Smith 1:aac28ffd63ed 224 {
Jamie Smith 1:aac28ffd63ed 225 shtpData[4] = 0b111; // tare X, Y, and Z axes
Jamie Smith 1:aac28ffd63ed 226 }
Jamie Smith 1:aac28ffd63ed 227
Jamie Smith 1:aac28ffd63ed 228 shtpData[5] = 0; // reorient all motion outputs
Jamie Smith 1:aac28ffd63ed 229
Jamie Smith 1:aac28ffd63ed 230 sendCommand(COMMAND_TARE);
Jamie Smith 1:aac28ffd63ed 231 }
Jamie Smith 1:aac28ffd63ed 232
Jamie Smith 1:aac28ffd63ed 233 bool BNO080::enableCalibration(bool calibrateAccel, bool calibrateGyro, bool calibrateMag)
Jamie Smith 1:aac28ffd63ed 234 {
Jamie Smith 1:aac28ffd63ed 235 // send the Configure ME Calibration command
Jamie Smith 1:aac28ffd63ed 236 zeroBuffer();
Jamie Smith 1:aac28ffd63ed 237
Jamie Smith 1:aac28ffd63ed 238 shtpData[3] = static_cast<uint8_t>(calibrateAccel ? 1 : 0);
Jamie Smith 1:aac28ffd63ed 239 shtpData[4] = static_cast<uint8_t>(calibrateGyro ? 1 : 0);
Jamie Smith 1:aac28ffd63ed 240 shtpData[5] = static_cast<uint8_t>(calibrateMag ? 1 : 0);
Jamie Smith 1:aac28ffd63ed 241
Jamie Smith 1:aac28ffd63ed 242 shtpData[6] = 0; // Configure ME Calibration command
Jamie Smith 1:aac28ffd63ed 243
Jamie Smith 1:aac28ffd63ed 244 shtpData[7] = 0; // planar accelerometer calibration always disabled
Jamie Smith 1:aac28ffd63ed 245
Jamie Smith 1:aac28ffd63ed 246 sendCommand(COMMAND_ME_CALIBRATE);
Jamie Smith 1:aac28ffd63ed 247
Jamie Smith 1:aac28ffd63ed 248 // now, wait for the response
Jamie Smith 1:aac28ffd63ed 249 if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE))
Jamie Smith 1:aac28ffd63ed 250 {
Jamie Smith 1:aac28ffd63ed 251 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 252 _debugPort->printf("Timeout waiting for calibration response!\n");
Jamie Smith 1:aac28ffd63ed 253 #endif
Jamie Smith 1:aac28ffd63ed 254 return false;
Jamie Smith 1:aac28ffd63ed 255 }
Jamie Smith 1:aac28ffd63ed 256
Jamie Smith 1:aac28ffd63ed 257 if(shtpData[2] != COMMAND_ME_CALIBRATE)
Jamie Smith 1:aac28ffd63ed 258 {
Jamie Smith 1:aac28ffd63ed 259 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 260 _debugPort->printf("Received wrong response to calibration command!\n");
Jamie Smith 1:aac28ffd63ed 261 #endif
Jamie Smith 1:aac28ffd63ed 262 return false;
Jamie Smith 1:aac28ffd63ed 263 }
Jamie Smith 1:aac28ffd63ed 264
Jamie Smith 1:aac28ffd63ed 265 if(shtpData[5] != 0)
Jamie Smith 1:aac28ffd63ed 266 {
Jamie Smith 1:aac28ffd63ed 267 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 268 _debugPort->printf("IMU reports calibrate command failed!\n");
Jamie Smith 1:aac28ffd63ed 269 #endif
Jamie Smith 1:aac28ffd63ed 270 return false;
Jamie Smith 1:aac28ffd63ed 271 }
Jamie Smith 1:aac28ffd63ed 272
Jamie Smith 1:aac28ffd63ed 273 // acknowledge checks out!
Jamie Smith 1:aac28ffd63ed 274 return true;
Jamie Smith 1:aac28ffd63ed 275 }
Jamie Smith 1:aac28ffd63ed 276
Jamie Smith 1:aac28ffd63ed 277 bool BNO080::saveCalibration()
Jamie Smith 1:aac28ffd63ed 278 {
Jamie Smith 1:aac28ffd63ed 279 zeroBuffer();
Jamie Smith 1:aac28ffd63ed 280
Jamie Smith 1:aac28ffd63ed 281 // no arguments
Jamie Smith 1:aac28ffd63ed 282 sendCommand(COMMAND_SAVE_DCD);
Jamie Smith 1:aac28ffd63ed 283
Jamie Smith 1:aac28ffd63ed 284 // now, wait for the response
Jamie Smith 1:aac28ffd63ed 285 if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE))
Jamie Smith 1:aac28ffd63ed 286 {
Jamie Smith 1:aac28ffd63ed 287 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 288 _debugPort->printf("Timeout waiting for calibration response!\n");
Jamie Smith 1:aac28ffd63ed 289 #endif
Jamie Smith 1:aac28ffd63ed 290 return false;
Jamie Smith 1:aac28ffd63ed 291 }
Jamie Smith 1:aac28ffd63ed 292
Jamie Smith 1:aac28ffd63ed 293 if(shtpData[2] != COMMAND_SAVE_DCD)
Jamie Smith 1:aac28ffd63ed 294 {
Jamie Smith 1:aac28ffd63ed 295 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 296 _debugPort->printf("Received wrong response to calibration command!\n");
Jamie Smith 1:aac28ffd63ed 297 #endif
Jamie Smith 1:aac28ffd63ed 298 return false;
Jamie Smith 1:aac28ffd63ed 299 }
Jamie Smith 1:aac28ffd63ed 300
Jamie Smith 1:aac28ffd63ed 301 if(shtpData[5] != 0)
Jamie Smith 1:aac28ffd63ed 302 {
Jamie Smith 1:aac28ffd63ed 303 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 304 _debugPort->printf("IMU reports calibrate command failed!\n");
Jamie Smith 1:aac28ffd63ed 305 #endif
Jamie Smith 1:aac28ffd63ed 306 return false;
Jamie Smith 1:aac28ffd63ed 307 }
Jamie Smith 1:aac28ffd63ed 308
Jamie Smith 1:aac28ffd63ed 309 // acknowledge checks out!
Jamie Smith 1:aac28ffd63ed 310 return true;
Jamie Smith 1:aac28ffd63ed 311 }
Jamie Smith 1:aac28ffd63ed 312
Jamie Smith 1:aac28ffd63ed 313 void BNO080::setSensorOrientation(Quaternion orientation)
Jamie Smith 1:aac28ffd63ed 314 {
Jamie Smith 1:aac28ffd63ed 315 zeroBuffer();
Jamie Smith 1:aac28ffd63ed 316
Jamie Smith 1:aac28ffd63ed 317 _debugPort->printf("y: %f", orientation.y());
Jamie Smith 1:aac28ffd63ed 318
Jamie Smith 1:aac28ffd63ed 319 // convert floats to Q
Jamie Smith 1:aac28ffd63ed 320 int16_t Q_x = floatToQ(orientation.x(), ORIENTATION_QUAT_Q_POINT);
Jamie Smith 1:aac28ffd63ed 321 int16_t Q_y = floatToQ(orientation.y(), ORIENTATION_QUAT_Q_POINT);
Jamie Smith 1:aac28ffd63ed 322 int16_t Q_z = floatToQ(orientation.z(), ORIENTATION_QUAT_Q_POINT);
Jamie Smith 1:aac28ffd63ed 323 int16_t Q_w = floatToQ(orientation.w(), ORIENTATION_QUAT_Q_POINT);
Jamie Smith 1:aac28ffd63ed 324
Jamie Smith 1:aac28ffd63ed 325 _debugPort->printf("Q_y: %hd", Q_y);
Jamie Smith 1:aac28ffd63ed 326
Jamie Smith 1:aac28ffd63ed 327 shtpData[3] = 2; // set reorientation
Jamie Smith 1:aac28ffd63ed 328
Jamie Smith 1:aac28ffd63ed 329 shtpData[4] = static_cast<uint8_t>(Q_x & 0xFF); //P1 - X component LSB
Jamie Smith 1:aac28ffd63ed 330 shtpData[5] = static_cast<uint8_t>(Q_x >> 8); //P2 - X component MSB
Jamie Smith 1:aac28ffd63ed 331
Jamie Smith 1:aac28ffd63ed 332 shtpData[6] = static_cast<uint8_t>(Q_y & 0xFF); //P3 - Y component LSB
Jamie Smith 1:aac28ffd63ed 333 shtpData[7] = static_cast<uint8_t>(Q_y >> 8); //P4 - Y component MSB
Jamie Smith 1:aac28ffd63ed 334
Jamie Smith 1:aac28ffd63ed 335 shtpData[8] = static_cast<uint8_t>(Q_z & 0xFF); //P5 - Z component LSB
Jamie Smith 1:aac28ffd63ed 336 shtpData[9] = static_cast<uint8_t>(Q_z >> 8); //P6 - Z component MSB
Jamie Smith 1:aac28ffd63ed 337
Jamie Smith 1:aac28ffd63ed 338 shtpData[10] = static_cast<uint8_t>(Q_w & 0xFF); //P7 - W component LSB
Jamie Smith 1:aac28ffd63ed 339 shtpData[11] = static_cast<uint8_t>(Q_w >> 8); //P8 - W component MSB
Jamie Smith 1:aac28ffd63ed 340
Jamie Smith 1:aac28ffd63ed 341 //Using this shtpData packet, send a command
Jamie Smith 1:aac28ffd63ed 342 sendCommand(COMMAND_TARE); // Send tare command
Jamie Smith 1:aac28ffd63ed 343
Jamie Smith 1:aac28ffd63ed 344 // NOTE: unlike literally every other command, a sensor orientation command is never acknowledged in any way.
Jamie Smith 1:aac28ffd63ed 345 }
Jamie Smith 1:aac28ffd63ed 346
Jamie Smith 1:aac28ffd63ed 347
Jamie Smith 1:aac28ffd63ed 348 bool BNO080::updateData()
Jamie Smith 1:aac28ffd63ed 349 {
Jamie Smith 1:aac28ffd63ed 350 if(_int.read() != 0)
Jamie Smith 1:aac28ffd63ed 351 {
Jamie Smith 1:aac28ffd63ed 352 // no waiting packets
Jamie Smith 1:aac28ffd63ed 353 return false;
Jamie Smith 1:aac28ffd63ed 354 }
Jamie Smith 1:aac28ffd63ed 355
Jamie Smith 1:aac28ffd63ed 356 while(_int.read() == 0)
Jamie Smith 1:aac28ffd63ed 357 {
Jamie Smith 1:aac28ffd63ed 358 if(!receivePacket())
Jamie Smith 1:aac28ffd63ed 359 {
Jamie Smith 1:aac28ffd63ed 360 // comms error
Jamie Smith 1:aac28ffd63ed 361 return false;
Jamie Smith 1:aac28ffd63ed 362 }
Jamie Smith 1:aac28ffd63ed 363
Jamie Smith 1:aac28ffd63ed 364 processPacket();
Jamie Smith 1:aac28ffd63ed 365 }
Jamie Smith 1:aac28ffd63ed 366
Jamie Smith 1:aac28ffd63ed 367 // packets were received, so data may have changed
Jamie Smith 1:aac28ffd63ed 368 return true;
Jamie Smith 1:aac28ffd63ed 369 }
Jamie Smith 1:aac28ffd63ed 370
Jamie Smith 1:aac28ffd63ed 371 uint8_t BNO080::getReportStatus(Report report)
Jamie Smith 1:aac28ffd63ed 372 {
Jamie Smith 1:aac28ffd63ed 373 uint8_t reportNum = static_cast<uint8_t>(report);
Jamie Smith 1:aac28ffd63ed 374 if(reportNum > STATUS_ARRAY_LEN)
Jamie Smith 1:aac28ffd63ed 375 {
Jamie Smith 1:aac28ffd63ed 376 return 0;
Jamie Smith 1:aac28ffd63ed 377 }
Jamie Smith 1:aac28ffd63ed 378
Jamie Smith 1:aac28ffd63ed 379 return reportStatus[reportNum];
Jamie Smith 1:aac28ffd63ed 380 }
Jamie Smith 1:aac28ffd63ed 381
Jamie Smith 1:aac28ffd63ed 382 const char* BNO080::getReportStatusString(Report report)
Jamie Smith 1:aac28ffd63ed 383 {
Jamie Smith 1:aac28ffd63ed 384 switch(getReportStatus(report))
Jamie Smith 1:aac28ffd63ed 385 {
Jamie Smith 1:aac28ffd63ed 386 case 0:
Jamie Smith 1:aac28ffd63ed 387 return "Unreliable";
Jamie Smith 1:aac28ffd63ed 388 case 1:
Jamie Smith 1:aac28ffd63ed 389 return "Accuracy Low";
Jamie Smith 1:aac28ffd63ed 390 case 2:
Jamie Smith 1:aac28ffd63ed 391 return "Accuracy Medium";
Jamie Smith 1:aac28ffd63ed 392 case 3:
Jamie Smith 1:aac28ffd63ed 393 return "Accuracy High";
Jamie Smith 1:aac28ffd63ed 394 default:
Jamie Smith 1:aac28ffd63ed 395 return "Error";
Jamie Smith 1:aac28ffd63ed 396 }
Jamie Smith 1:aac28ffd63ed 397 }
Jamie Smith 1:aac28ffd63ed 398
Jamie Smith 1:aac28ffd63ed 399 bool BNO080::hasNewData(Report report)
Jamie Smith 1:aac28ffd63ed 400 {
Jamie Smith 1:aac28ffd63ed 401 uint8_t reportNum = static_cast<uint8_t>(report);
Jamie Smith 1:aac28ffd63ed 402 if(reportNum > STATUS_ARRAY_LEN)
Jamie Smith 1:aac28ffd63ed 403 {
Jamie Smith 1:aac28ffd63ed 404 return false;
Jamie Smith 1:aac28ffd63ed 405 }
Jamie Smith 1:aac28ffd63ed 406
Jamie Smith 1:aac28ffd63ed 407 bool newData = reportHasBeenUpdated[reportNum];
Jamie Smith 1:aac28ffd63ed 408 reportHasBeenUpdated[reportNum] = false; // clear flag
Jamie Smith 1:aac28ffd63ed 409 return newData;
Jamie Smith 1:aac28ffd63ed 410 }
Jamie Smith 1:aac28ffd63ed 411
Jamie Smith 1:aac28ffd63ed 412 //Sends the packet to enable the rotation vector
Jamie Smith 1:aac28ffd63ed 413 void BNO080::enableReport(Report report, uint16_t timeBetweenReports)
Jamie Smith 1:aac28ffd63ed 414 {
Jamie Smith 1:aac28ffd63ed 415 // check time
Jamie Smith 1:aac28ffd63ed 416 float periodSeconds = timeBetweenReports / 1000.0;
Jamie Smith 1:aac28ffd63ed 417
Jamie Smith 1:aac28ffd63ed 418 if(periodSeconds < getMinPeriod(report))
Jamie Smith 1:aac28ffd63ed 419 {
Jamie Smith 1:aac28ffd63ed 420 _debugPort->printf("Error: attempt made to set report 0x%02hhx to period of %.06f s, which is smaller than its min period of %.06f s.\n",
Jamie Smith 1:aac28ffd63ed 421 static_cast<uint8_t>(report), periodSeconds, getMinPeriod(report));
Jamie Smith 1:aac28ffd63ed 422 return;
Jamie Smith 1:aac28ffd63ed 423 }
Jamie Smith 1:aac28ffd63ed 424 /*
Jamie Smith 1:aac28ffd63ed 425 else if(getMaxPeriod(report) > 0 && periodSeconds > getMaxPeriod(report))
Jamie Smith 1:aac28ffd63ed 426 {
Jamie Smith 1:aac28ffd63ed 427 _debugPort->printf("Error: attempt made to set report 0x%02hhx to period of %.06f s, which is larger than its max period of %.06f s.\n",
Jamie Smith 1:aac28ffd63ed 428 static_cast<uint8_t>(report), periodSeconds, getMaxPeriod(report));
Jamie Smith 1:aac28ffd63ed 429 return;
Jamie Smith 1:aac28ffd63ed 430 }
Jamie Smith 1:aac28ffd63ed 431 */
Jamie Smith 1:aac28ffd63ed 432 setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports);
Jamie Smith 1:aac28ffd63ed 433
Jamie Smith 1:aac28ffd63ed 434 // note: we don't wait for ACKs on these packets because they can take quite a while, like half a second, to come in
Jamie Smith 1:aac28ffd63ed 435 }
Jamie Smith 1:aac28ffd63ed 436
Jamie Smith 1:aac28ffd63ed 437 void BNO080::disableReport(Report report)
Jamie Smith 1:aac28ffd63ed 438 {
Jamie Smith 1:aac28ffd63ed 439 // set the report's polling period to zero to disable it
Jamie Smith 1:aac28ffd63ed 440 setFeatureCommand(static_cast<uint8_t>(report), 0);
Jamie Smith 1:aac28ffd63ed 441 }
Jamie Smith 1:aac28ffd63ed 442
Jamie Smith 1:aac28ffd63ed 443 uint32_t BNO080::getSerialNumber()
Jamie Smith 1:aac28ffd63ed 444 {
Jamie Smith 1:aac28ffd63ed 445 uint32_t serNoBuffer;
Jamie Smith 1:aac28ffd63ed 446
Jamie Smith 1:aac28ffd63ed 447 if(!readFRSRecord(FRS_RECORDID_SERIAL_NUMBER, &serNoBuffer, 1))
Jamie Smith 1:aac28ffd63ed 448 {
Jamie Smith 1:aac28ffd63ed 449 return 0;
Jamie Smith 1:aac28ffd63ed 450 }
Jamie Smith 1:aac28ffd63ed 451
Jamie Smith 1:aac28ffd63ed 452 return serNoBuffer;
Jamie Smith 1:aac28ffd63ed 453 }
Jamie Smith 1:aac28ffd63ed 454
Jamie Smith 1:aac28ffd63ed 455 float BNO080::getRange(Report report)
Jamie Smith 1:aac28ffd63ed 456 {
Jamie Smith 1:aac28ffd63ed 457 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 458
Jamie Smith 1:aac28ffd63ed 459 return qToFloat_dword(metadataRecord[1], getQ1(report));
Jamie Smith 1:aac28ffd63ed 460 }
Jamie Smith 1:aac28ffd63ed 461
Jamie Smith 1:aac28ffd63ed 462
Jamie Smith 1:aac28ffd63ed 463 float BNO080::getResolution(Report report)
Jamie Smith 1:aac28ffd63ed 464 {
Jamie Smith 1:aac28ffd63ed 465 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 466
Jamie Smith 1:aac28ffd63ed 467 return qToFloat_dword(metadataRecord[2], getQ1(report));
Jamie Smith 1:aac28ffd63ed 468 }
Jamie Smith 1:aac28ffd63ed 469
Jamie Smith 1:aac28ffd63ed 470 float BNO080::getPower(Report report)
Jamie Smith 1:aac28ffd63ed 471 {
Jamie Smith 1:aac28ffd63ed 472 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 473
Jamie Smith 1:aac28ffd63ed 474 uint16_t powerQ = static_cast<uint16_t>(metadataRecord[3] & 0xFFFF);
Jamie Smith 1:aac28ffd63ed 475
Jamie Smith 1:aac28ffd63ed 476 return qToFloat_dword(powerQ, POWER_Q_POINT);
Jamie Smith 1:aac28ffd63ed 477 }
Jamie Smith 1:aac28ffd63ed 478
Jamie Smith 1:aac28ffd63ed 479 float BNO080::getMinPeriod(Report report)
Jamie Smith 1:aac28ffd63ed 480 {
Jamie Smith 1:aac28ffd63ed 481 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 482
Jamie Smith 1:aac28ffd63ed 483 return metadataRecord[4] / 1e6f; // convert from microseconds to seconds
Jamie Smith 1:aac28ffd63ed 484 }
Jamie Smith 1:aac28ffd63ed 485
Jamie Smith 1:aac28ffd63ed 486 float BNO080::getMaxPeriod(Report report)
Jamie Smith 1:aac28ffd63ed 487 {
Jamie Smith 1:aac28ffd63ed 488 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 489
Jamie Smith 1:aac28ffd63ed 490 if(getMetaVersion() == 3)
Jamie Smith 1:aac28ffd63ed 491 {
Jamie Smith 1:aac28ffd63ed 492 // no max period entry in this record format
Jamie Smith 1:aac28ffd63ed 493 return -1.0f;
Jamie Smith 1:aac28ffd63ed 494 }
Jamie Smith 1:aac28ffd63ed 495
Jamie Smith 1:aac28ffd63ed 496 return metadataRecord[9] / 1e6f; // convert from microseconds to seconds
Jamie Smith 1:aac28ffd63ed 497 }
Jamie Smith 1:aac28ffd63ed 498
Jamie Smith 1:aac28ffd63ed 499 void BNO080::printMetadataSummary(Report report)
Jamie Smith 1:aac28ffd63ed 500 {
Jamie Smith 1:aac28ffd63ed 501 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 502 if(!loadReportMetadata(report))
Jamie Smith 1:aac28ffd63ed 503 {
Jamie Smith 1:aac28ffd63ed 504 _debugPort->printf("Failed to load report metadata!\n");
Jamie Smith 1:aac28ffd63ed 505 }
Jamie Smith 1:aac28ffd63ed 506
Jamie Smith 1:aac28ffd63ed 507 _debugPort->printf("======= Metadata for report 0x%02hhx =======\n", static_cast<uint8_t>(report));
Jamie Smith 1:aac28ffd63ed 508
Jamie Smith 1:aac28ffd63ed 509 _debugPort->printf("Range: +- %.04f units\n", getRange(report));
Jamie Smith 1:aac28ffd63ed 510 _debugPort->printf("Resolution: %.04f units\n", getResolution(report));
Jamie Smith 1:aac28ffd63ed 511 _debugPort->printf("Power Used: %.03f mA\n", getPower(report));
Jamie Smith 1:aac28ffd63ed 512 _debugPort->printf("Min Period: %.06f s\n", getMinPeriod(report));
Jamie Smith 1:aac28ffd63ed 513 _debugPort->printf("Max Period: %.06f s\n\n", getMaxPeriod(report));
Jamie Smith 1:aac28ffd63ed 514
Jamie Smith 1:aac28ffd63ed 515 #endif
Jamie Smith 1:aac28ffd63ed 516 }
Jamie Smith 1:aac28ffd63ed 517
Jamie Smith 1:aac28ffd63ed 518 int16_t BNO080::getQ1(Report report)
Jamie Smith 1:aac28ffd63ed 519 {
Jamie Smith 1:aac28ffd63ed 520 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 521
Jamie Smith 1:aac28ffd63ed 522 return static_cast<int16_t>(metadataRecord[7] & 0xFFFF);
Jamie Smith 1:aac28ffd63ed 523 }
Jamie Smith 1:aac28ffd63ed 524
Jamie Smith 1:aac28ffd63ed 525 int16_t BNO080::getQ2(Report report)
Jamie Smith 1:aac28ffd63ed 526 {
Jamie Smith 1:aac28ffd63ed 527 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 528
Jamie Smith 1:aac28ffd63ed 529 return static_cast<int16_t>(metadataRecord[7] >> 16);
Jamie Smith 1:aac28ffd63ed 530 }
Jamie Smith 1:aac28ffd63ed 531
Jamie Smith 1:aac28ffd63ed 532 int16_t BNO080::getQ3(Report report)
Jamie Smith 1:aac28ffd63ed 533 {
Jamie Smith 1:aac28ffd63ed 534 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 535
Jamie Smith 1:aac28ffd63ed 536 return static_cast<int16_t>(metadataRecord[8] >> 16);
Jamie Smith 1:aac28ffd63ed 537 }
Jamie Smith 1:aac28ffd63ed 538
Jamie Smith 1:aac28ffd63ed 539 void BNO080::processPacket()
Jamie Smith 1:aac28ffd63ed 540 {
Jamie Smith 1:aac28ffd63ed 541 if(shtpHeader[2] == CHANNEL_CONTROL)
Jamie Smith 1:aac28ffd63ed 542 {
Jamie Smith 1:aac28ffd63ed 543 // currently no command reports are read
Jamie Smith 1:aac28ffd63ed 544 }
Jamie Smith 1:aac28ffd63ed 545 else if(shtpHeader[2] == CHANNEL_EXECUTABLE)
Jamie Smith 1:aac28ffd63ed 546 {
Jamie Smith 1:aac28ffd63ed 547 // currently no executable reports are read
Jamie Smith 1:aac28ffd63ed 548 }
Jamie Smith 1:aac28ffd63ed 549 else if(shtpHeader[2] == CHANNEL_COMMAND)
Jamie Smith 1:aac28ffd63ed 550 {
Jamie Smith 1:aac28ffd63ed 551
Jamie Smith 1:aac28ffd63ed 552 }
Jamie Smith 1:aac28ffd63ed 553 else if(shtpHeader[2] == CHANNEL_REPORTS || shtpHeader[2] == CHANNEL_WAKE_REPORTS)
Jamie Smith 1:aac28ffd63ed 554 {
Jamie Smith 1:aac28ffd63ed 555 if(shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP)
Jamie Smith 1:aac28ffd63ed 556 {
Jamie Smith 1:aac28ffd63ed 557 // sensor data packet
Jamie Smith 1:aac28ffd63ed 558 parseSensorDataPacket();
Jamie Smith 1:aac28ffd63ed 559 }
Jamie Smith 1:aac28ffd63ed 560 }
Jamie Smith 1:aac28ffd63ed 561 }
Jamie Smith 1:aac28ffd63ed 562
Jamie Smith 1:aac28ffd63ed 563 // sizes of various sensor data packet elements
Jamie Smith 1:aac28ffd63ed 564 #define SIZEOF_BASE_TIMESTAMP 5
Jamie Smith 1:aac28ffd63ed 565 #define SIZEOF_TIMESTAMP_REBASE 5
Jamie Smith 1:aac28ffd63ed 566 #define SIZEOF_ACCELEROMETER 10
Jamie Smith 1:aac28ffd63ed 567 #define SIZEOF_LINEAR_ACCELERATION 10
Jamie Smith 1:aac28ffd63ed 568 #define SIZEOF_GYROSCOPE_CALIBRATED 10
Jamie Smith 1:aac28ffd63ed 569 #define SIZEOF_MAGNETIC_FIELD_CALIBRATED 10
Jamie Smith 1:aac28ffd63ed 570 #define SIZEOF_MAGNETIC_FIELD_UNCALIBRATED 16
Jamie Smith 1:aac28ffd63ed 571 #define SIZEOF_ROTATION_VECTOR 14
Jamie Smith 1:aac28ffd63ed 572 #define SIZEOF_GAME_ROTATION_VECTOR 12
Jamie Smith 1:aac28ffd63ed 573 #define SIZEOF_GEOMAGNETIC_ROTATION_VECTOR 14
Jamie Smith 1:aac28ffd63ed 574 #define SIZEOF_TAP_DETECTOR 5
Jamie Smith 1:aac28ffd63ed 575 #define SIZEOF_STABILITY_REPORT 6
Jamie Smith 1:aac28ffd63ed 576 #define SIZEOF_STEP_DETECTOR 8
Jamie Smith 1:aac28ffd63ed 577 #define SIZEOF_STEP_COUNTER 12
Jamie Smith 1:aac28ffd63ed 578 #define SIZEOF_SIGNIFICANT_MOTION 6
Jamie Smith 1:aac28ffd63ed 579 #define SIZEOF_SHAKE_DETECTOR 6
Jamie Smith 1:aac28ffd63ed 580
Jamie Smith 1:aac28ffd63ed 581 void BNO080::parseSensorDataPacket()
Jamie Smith 1:aac28ffd63ed 582 {
Jamie Smith 1:aac28ffd63ed 583 size_t currReportOffset = 0;
Jamie Smith 1:aac28ffd63ed 584
Jamie Smith 1:aac28ffd63ed 585 // every sensor data report first contains a timestamp offset to show how long it has been between when
Jamie Smith 1:aac28ffd63ed 586 // the host interrupt was sent and when the packet was transmitted.
Jamie Smith 1:aac28ffd63ed 587 // We don't use interrupts and don't care about times, so we can throw this out.
Jamie Smith 1:aac28ffd63ed 588 currReportOffset += SIZEOF_BASE_TIMESTAMP;
Jamie Smith 1:aac28ffd63ed 589
Jamie Smith 1:aac28ffd63ed 590 while(currReportOffset < packetLength)
Jamie Smith 1:aac28ffd63ed 591 {
Jamie Smith 1:aac28ffd63ed 592 if(currReportOffset >= STORED_PACKET_SIZE)
Jamie Smith 1:aac28ffd63ed 593 {
Jamie Smith 1:aac28ffd63ed 594 _debugPort->printf("Error: sensor report longer than packet buffer!\n");
Jamie Smith 1:aac28ffd63ed 595 return;
Jamie Smith 1:aac28ffd63ed 596 }
Jamie Smith 1:aac28ffd63ed 597
Jamie Smith 1:aac28ffd63ed 598 // lots of sensor reports use 3 16-bit numbers stored in bytes 4 through 9
Jamie Smith 1:aac28ffd63ed 599 // we can save some time by parsing those out here.
Jamie Smith 1:aac28ffd63ed 600 uint16_t data1 = (uint16_t)shtpData[currReportOffset + 5] << 8 | shtpData[currReportOffset + 4];
Jamie Smith 1:aac28ffd63ed 601 uint16_t data2 = (uint16_t)shtpData[currReportOffset + 7] << 8 | shtpData[currReportOffset + 6];
Jamie Smith 1:aac28ffd63ed 602 uint16_t data3 = (uint16_t)shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8];
Jamie Smith 1:aac28ffd63ed 603
Jamie Smith 1:aac28ffd63ed 604 uint8_t reportNum = shtpData[currReportOffset];
Jamie Smith 1:aac28ffd63ed 605
Jamie Smith 1:aac28ffd63ed 606 if(reportNum != SENSOR_REPORTID_TIMESTAMP_REBASE)
Jamie Smith 1:aac28ffd63ed 607 {
Jamie Smith 1:aac28ffd63ed 608 // set status from byte 2
Jamie Smith 1:aac28ffd63ed 609 reportStatus[reportNum] = static_cast<uint8_t>(shtpData[currReportOffset + 2] & 0b11);
Jamie Smith 1:aac28ffd63ed 610
Jamie Smith 1:aac28ffd63ed 611 // set updated flag
Jamie Smith 1:aac28ffd63ed 612 reportHasBeenUpdated[reportNum] = true;
Jamie Smith 1:aac28ffd63ed 613 }
Jamie Smith 1:aac28ffd63ed 614
Jamie Smith 1:aac28ffd63ed 615 switch(shtpData[currReportOffset])
Jamie Smith 1:aac28ffd63ed 616 {
Jamie Smith 1:aac28ffd63ed 617 case SENSOR_REPORTID_TIMESTAMP_REBASE:
Jamie Smith 1:aac28ffd63ed 618 currReportOffset += SIZEOF_TIMESTAMP_REBASE;
Jamie Smith 1:aac28ffd63ed 619 break;
Jamie Smith 1:aac28ffd63ed 620
Jamie Smith 1:aac28ffd63ed 621 case SENSOR_REPORTID_ACCELEROMETER:
Jamie Smith 1:aac28ffd63ed 622
Jamie Smith 1:aac28ffd63ed 623 totalAcceleration = TVector3(
Jamie Smith 1:aac28ffd63ed 624 qToFloat(data1, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 625 qToFloat(data2, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 626 qToFloat(data3, ACCELEROMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 627
Jamie Smith 1:aac28ffd63ed 628 currReportOffset += SIZEOF_ACCELEROMETER;
Jamie Smith 1:aac28ffd63ed 629 break;
Jamie Smith 1:aac28ffd63ed 630
Jamie Smith 1:aac28ffd63ed 631 case SENSOR_REPORTID_LINEAR_ACCELERATION:
Jamie Smith 1:aac28ffd63ed 632
Jamie Smith 1:aac28ffd63ed 633 linearAcceleration = TVector3(
Jamie Smith 1:aac28ffd63ed 634 qToFloat(data1, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 635 qToFloat(data2, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 636 qToFloat(data3, ACCELEROMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 637
Jamie Smith 1:aac28ffd63ed 638 currReportOffset += SIZEOF_LINEAR_ACCELERATION;
Jamie Smith 1:aac28ffd63ed 639 break;
Jamie Smith 1:aac28ffd63ed 640
Jamie Smith 1:aac28ffd63ed 641 case SENSOR_REPORTID_GRAVITY:
Jamie Smith 1:aac28ffd63ed 642
Jamie Smith 1:aac28ffd63ed 643 gravityAcceleration = TVector3(
Jamie Smith 1:aac28ffd63ed 644 qToFloat(data1, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 645 qToFloat(data2, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 646 qToFloat(data3, ACCELEROMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 647
Jamie Smith 1:aac28ffd63ed 648 currReportOffset += SIZEOF_LINEAR_ACCELERATION;
Jamie Smith 1:aac28ffd63ed 649 break;
Jamie Smith 1:aac28ffd63ed 650
Jamie Smith 1:aac28ffd63ed 651 case SENSOR_REPORTID_GYROSCOPE_CALIBRATED:
Jamie Smith 1:aac28ffd63ed 652
Jamie Smith 1:aac28ffd63ed 653 gyroRotation = TVector3(
Jamie Smith 1:aac28ffd63ed 654 qToFloat(data1, GYRO_Q_POINT),
Jamie Smith 1:aac28ffd63ed 655 qToFloat(data2, GYRO_Q_POINT),
Jamie Smith 1:aac28ffd63ed 656 qToFloat(data3, GYRO_Q_POINT));
Jamie Smith 1:aac28ffd63ed 657
Jamie Smith 1:aac28ffd63ed 658 currReportOffset += SIZEOF_GYROSCOPE_CALIBRATED;
Jamie Smith 1:aac28ffd63ed 659 break;
Jamie Smith 1:aac28ffd63ed 660
Jamie Smith 1:aac28ffd63ed 661 case SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED:
Jamie Smith 1:aac28ffd63ed 662
Jamie Smith 1:aac28ffd63ed 663 magField = TVector3(
Jamie Smith 1:aac28ffd63ed 664 qToFloat(data1, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 665 qToFloat(data2, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 666 qToFloat(data3, MAGNETOMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 667
Jamie Smith 1:aac28ffd63ed 668 currReportOffset += SIZEOF_MAGNETIC_FIELD_CALIBRATED;
Jamie Smith 1:aac28ffd63ed 669 break;
Jamie Smith 1:aac28ffd63ed 670
Jamie Smith 1:aac28ffd63ed 671 case SENSOR_REPORTID_MAGNETIC_FIELD_UNCALIBRATED:
Jamie Smith 1:aac28ffd63ed 672 {
Jamie Smith 1:aac28ffd63ed 673 magFieldUncalibrated = TVector3(
Jamie Smith 1:aac28ffd63ed 674 qToFloat(data1, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 675 qToFloat(data2, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 676 qToFloat(data3, MAGNETOMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 677
Jamie Smith 1:aac28ffd63ed 678 uint16_t ironOffsetXQ = shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
Jamie Smith 1:aac28ffd63ed 679 uint16_t ironOffsetYQ = shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12];
Jamie Smith 1:aac28ffd63ed 680 uint16_t ironOffsetZQ = shtpData[currReportOffset + 15] << 8 | shtpData[currReportOffset + 14];
Jamie Smith 1:aac28ffd63ed 681
Jamie Smith 1:aac28ffd63ed 682 hardIronOffset = TVector3(
Jamie Smith 1:aac28ffd63ed 683 qToFloat(ironOffsetXQ, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 684 qToFloat(ironOffsetYQ, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 685 qToFloat(ironOffsetZQ, MAGNETOMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 686
Jamie Smith 1:aac28ffd63ed 687 currReportOffset += SIZEOF_MAGNETIC_FIELD_UNCALIBRATED;
Jamie Smith 1:aac28ffd63ed 688 }
Jamie Smith 1:aac28ffd63ed 689 break;
Jamie Smith 1:aac28ffd63ed 690
Jamie Smith 1:aac28ffd63ed 691 case SENSOR_REPORTID_ROTATION_VECTOR:
Jamie Smith 1:aac28ffd63ed 692 {
Jamie Smith 1:aac28ffd63ed 693 uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
Jamie Smith 1:aac28ffd63ed 694 uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12];
Jamie Smith 1:aac28ffd63ed 695
Jamie Smith 1:aac28ffd63ed 696 rotationVector = TVector4(
Jamie Smith 1:aac28ffd63ed 697 qToFloat(data1, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 698 qToFloat(data2, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 699 qToFloat(data3, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 700 qToFloat(realPartQ, ROTATION_Q_POINT));
Jamie Smith 1:aac28ffd63ed 701
Jamie Smith 1:aac28ffd63ed 702 rotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT);
Jamie Smith 1:aac28ffd63ed 703
Jamie Smith 1:aac28ffd63ed 704 currReportOffset += SIZEOF_ROTATION_VECTOR;
Jamie Smith 1:aac28ffd63ed 705 }
Jamie Smith 1:aac28ffd63ed 706 break;
Jamie Smith 1:aac28ffd63ed 707
Jamie Smith 1:aac28ffd63ed 708 case SENSOR_REPORTID_GAME_ROTATION_VECTOR:
Jamie Smith 1:aac28ffd63ed 709 {
Jamie Smith 1:aac28ffd63ed 710 uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
Jamie Smith 1:aac28ffd63ed 711
Jamie Smith 1:aac28ffd63ed 712 gameRotationVector = TVector4(
Jamie Smith 1:aac28ffd63ed 713 qToFloat(data1, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 714 qToFloat(data2, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 715 qToFloat(data3, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 716 qToFloat(realPartQ, ROTATION_Q_POINT));
Jamie Smith 1:aac28ffd63ed 717
Jamie Smith 1:aac28ffd63ed 718 currReportOffset += SIZEOF_GAME_ROTATION_VECTOR;
Jamie Smith 1:aac28ffd63ed 719 }
Jamie Smith 1:aac28ffd63ed 720 break;
Jamie Smith 1:aac28ffd63ed 721
Jamie Smith 1:aac28ffd63ed 722 case SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR:
Jamie Smith 1:aac28ffd63ed 723 {
Jamie Smith 1:aac28ffd63ed 724 uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
Jamie Smith 1:aac28ffd63ed 725 uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12];
Jamie Smith 1:aac28ffd63ed 726
Jamie Smith 1:aac28ffd63ed 727 geomagneticRotationVector = TVector4(
Jamie Smith 1:aac28ffd63ed 728 qToFloat(data1, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 729 qToFloat(data2, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 730 qToFloat(data3, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 731 qToFloat(realPartQ, ROTATION_Q_POINT));
Jamie Smith 1:aac28ffd63ed 732
Jamie Smith 1:aac28ffd63ed 733 geomagneticRotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT);
Jamie Smith 1:aac28ffd63ed 734
Jamie Smith 1:aac28ffd63ed 735 currReportOffset += SIZEOF_GEOMAGNETIC_ROTATION_VECTOR;
Jamie Smith 1:aac28ffd63ed 736 }
Jamie Smith 1:aac28ffd63ed 737 break;
Jamie Smith 1:aac28ffd63ed 738
Jamie Smith 1:aac28ffd63ed 739 case SENSOR_REPORTID_TAP_DETECTOR:
Jamie Smith 1:aac28ffd63ed 740
Jamie Smith 1:aac28ffd63ed 741 // since we got the report, a tap was detected
Jamie Smith 1:aac28ffd63ed 742 tapDetected = true;
Jamie Smith 1:aac28ffd63ed 743
Jamie Smith 1:aac28ffd63ed 744 doubleTap = (shtpData[currReportOffset + 4] & (1 << 6)) != 0;
Jamie Smith 1:aac28ffd63ed 745
Jamie Smith 1:aac28ffd63ed 746 currReportOffset += SIZEOF_TAP_DETECTOR;
Jamie Smith 1:aac28ffd63ed 747 break;
Jamie Smith 1:aac28ffd63ed 748
Jamie Smith 1:aac28ffd63ed 749 case SENSOR_REPORTID_STABILITY_CLASSIFIER:
Jamie Smith 1:aac28ffd63ed 750 {
Jamie Smith 1:aac28ffd63ed 751 uint8_t classificationNumber = shtpData[currReportOffset + 4];
Jamie Smith 1:aac28ffd63ed 752
Jamie Smith 1:aac28ffd63ed 753 if(classificationNumber > 4)
Jamie Smith 1:aac28ffd63ed 754 {
Jamie Smith 1:aac28ffd63ed 755 classificationNumber = 0;
Jamie Smith 1:aac28ffd63ed 756 }
Jamie Smith 1:aac28ffd63ed 757
Jamie Smith 1:aac28ffd63ed 758 stability = static_cast<Stability>(classificationNumber);
Jamie Smith 1:aac28ffd63ed 759
Jamie Smith 1:aac28ffd63ed 760 currReportOffset += SIZEOF_STABILITY_REPORT;
Jamie Smith 1:aac28ffd63ed 761 }
Jamie Smith 1:aac28ffd63ed 762 break;
Jamie Smith 1:aac28ffd63ed 763
Jamie Smith 1:aac28ffd63ed 764 case SENSOR_REPORTID_STEP_DETECTOR:
Jamie Smith 1:aac28ffd63ed 765
Jamie Smith 1:aac28ffd63ed 766 // the fact that we got the report means that a step was detected
Jamie Smith 1:aac28ffd63ed 767 stepDetected = true;
Jamie Smith 1:aac28ffd63ed 768
Jamie Smith 1:aac28ffd63ed 769 currReportOffset += SIZEOF_STEP_DETECTOR;
Jamie Smith 1:aac28ffd63ed 770
Jamie Smith 1:aac28ffd63ed 771 break;
Jamie Smith 1:aac28ffd63ed 772
Jamie Smith 1:aac28ffd63ed 773 case SENSOR_REPORTID_STEP_COUNTER:
Jamie Smith 1:aac28ffd63ed 774
Jamie Smith 1:aac28ffd63ed 775 stepCount = shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8];
Jamie Smith 1:aac28ffd63ed 776
Jamie Smith 1:aac28ffd63ed 777 currReportOffset += SIZEOF_STEP_COUNTER;
Jamie Smith 1:aac28ffd63ed 778
Jamie Smith 1:aac28ffd63ed 779 break;
Jamie Smith 1:aac28ffd63ed 780
Jamie Smith 1:aac28ffd63ed 781 case SENSOR_REPORTID_SIGNIFICANT_MOTION:
Jamie Smith 1:aac28ffd63ed 782
Jamie Smith 1:aac28ffd63ed 783 // the fact that we got the report means that significant motion was detected
Jamie Smith 1:aac28ffd63ed 784 significantMotionDetected = true;
Jamie Smith 1:aac28ffd63ed 785
Jamie Smith 1:aac28ffd63ed 786 currReportOffset += SIZEOF_SIGNIFICANT_MOTION;
Jamie Smith 1:aac28ffd63ed 787
Jamie Smith 1:aac28ffd63ed 788 case SENSOR_REPORTID_SHAKE_DETECTOR:
Jamie Smith 1:aac28ffd63ed 789
Jamie Smith 1:aac28ffd63ed 790 shakeDetected = true;
Jamie Smith 1:aac28ffd63ed 791
Jamie Smith 1:aac28ffd63ed 792 xAxisShake = (shtpData[currReportOffset + 4] & 1) != 0;
Jamie Smith 1:aac28ffd63ed 793 yAxisShake = (shtpData[currReportOffset + 4] & (1 << 1)) != 0;
Jamie Smith 1:aac28ffd63ed 794 zAxisShake = (shtpData[currReportOffset + 4] & (1 << 2)) != 0;
Jamie Smith 1:aac28ffd63ed 795
Jamie Smith 1:aac28ffd63ed 796 currReportOffset += SIZEOF_SHAKE_DETECTOR;
Jamie Smith 1:aac28ffd63ed 797
Jamie Smith 1:aac28ffd63ed 798 default:
Jamie Smith 1:aac28ffd63ed 799 _debugPort->printf("Error: unrecognized report ID in sensor report: %hhx. Byte %u, length %hu\n", shtpData[currReportOffset], currReportOffset, packetLength);
Jamie Smith 1:aac28ffd63ed 800 return;
Jamie Smith 1:aac28ffd63ed 801 }
Jamie Smith 1:aac28ffd63ed 802 }
Jamie Smith 1:aac28ffd63ed 803
Jamie Smith 1:aac28ffd63ed 804 }
Jamie Smith 1:aac28ffd63ed 805
Jamie Smith 1:aac28ffd63ed 806 bool BNO080::waitForPacket(int channel, uint8_t reportID, float timeout)
Jamie Smith 1:aac28ffd63ed 807 {
Jamie Smith 1:aac28ffd63ed 808 Timer timeoutTimer;
Jamie Smith 1:aac28ffd63ed 809 timeoutTimer.start();
Jamie Smith 1:aac28ffd63ed 810
Jamie Smith 1:aac28ffd63ed 811 while(timeoutTimer.read() <= timeout)
Jamie Smith 1:aac28ffd63ed 812 {
Jamie Smith 1:aac28ffd63ed 813 if(_int.read() == 0)
Jamie Smith 1:aac28ffd63ed 814 {
Jamie Smith 1:aac28ffd63ed 815 if(!receivePacket(timeout))
Jamie Smith 1:aac28ffd63ed 816 {
Jamie Smith 1:aac28ffd63ed 817 return false;
Jamie Smith 1:aac28ffd63ed 818 }
Jamie Smith 1:aac28ffd63ed 819
Jamie Smith 1:aac28ffd63ed 820 if(channel == shtpHeader[2] && reportID == shtpData[0])
Jamie Smith 1:aac28ffd63ed 821 {
Jamie Smith 1:aac28ffd63ed 822 // found correct packet!
Jamie Smith 1:aac28ffd63ed 823 return true;
Jamie Smith 1:aac28ffd63ed 824 }
Jamie Smith 1:aac28ffd63ed 825 else
Jamie Smith 1:aac28ffd63ed 826 {
Jamie Smith 1:aac28ffd63ed 827 // other data packet, send to proper channels
Jamie Smith 1:aac28ffd63ed 828 processPacket();
Jamie Smith 1:aac28ffd63ed 829 }
Jamie Smith 1:aac28ffd63ed 830 }
Jamie Smith 1:aac28ffd63ed 831 }
Jamie Smith 1:aac28ffd63ed 832
Jamie Smith 1:aac28ffd63ed 833 _debugPort->printf("Packet wait timeout.\n");
Jamie Smith 1:aac28ffd63ed 834 return false;
Jamie Smith 1:aac28ffd63ed 835 }
Jamie Smith 1:aac28ffd63ed 836
Jamie Smith 1:aac28ffd63ed 837 //Given a register value and a Q point, convert to float
Jamie Smith 1:aac28ffd63ed 838 //See https://en.wikipedia.org/wiki/Q_(number_format)
Jamie Smith 1:aac28ffd63ed 839 float BNO080::qToFloat(int16_t fixedPointValue, uint8_t qPoint)
Jamie Smith 1:aac28ffd63ed 840 {
Jamie Smith 1:aac28ffd63ed 841 float qFloat = fixedPointValue;
Jamie Smith 1:aac28ffd63ed 842 qFloat *= pow(2, qPoint * -1);
Jamie Smith 1:aac28ffd63ed 843 return (qFloat);
Jamie Smith 1:aac28ffd63ed 844 }
Jamie Smith 1:aac28ffd63ed 845
Jamie Smith 1:aac28ffd63ed 846 float BNO080::qToFloat_dword(uint32_t fixedPointValue, int16_t qPoint)
Jamie Smith 1:aac28ffd63ed 847 {
Jamie Smith 1:aac28ffd63ed 848 float qFloat = fixedPointValue;
Jamie Smith 1:aac28ffd63ed 849 qFloat *= pow(2, qPoint * -1);
Jamie Smith 1:aac28ffd63ed 850 return (qFloat);
Jamie Smith 1:aac28ffd63ed 851 }
Jamie Smith 1:aac28ffd63ed 852
Jamie Smith 1:aac28ffd63ed 853 //Given a floating point value and a Q point, convert to Q
Jamie Smith 1:aac28ffd63ed 854 //See https://en.wikipedia.org/wiki/Q_(number_format)
Jamie Smith 1:aac28ffd63ed 855 int16_t BNO080::floatToQ(float qFloat, uint8_t qPoint)
Jamie Smith 1:aac28ffd63ed 856 {
Jamie Smith 1:aac28ffd63ed 857 int16_t qVal = static_cast<int16_t>(qFloat * pow(2, qPoint));
Jamie Smith 1:aac28ffd63ed 858 return qVal;
Jamie Smith 1:aac28ffd63ed 859 }
Jamie Smith 1:aac28ffd63ed 860
Jamie Smith 1:aac28ffd63ed 861 //Tell the sensor to do a command
Jamie Smith 1:aac28ffd63ed 862 //See 6.3.8 page 41, Command request
Jamie Smith 1:aac28ffd63ed 863 //The caller is expected to set P0 through P8 prior to calling
Jamie Smith 1:aac28ffd63ed 864 void BNO080::sendCommand(uint8_t command)
Jamie Smith 1:aac28ffd63ed 865 {
Jamie Smith 1:aac28ffd63ed 866 shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request
Jamie Smith 1:aac28ffd63ed 867 shtpData[1] = commandSequenceNumber++; //Increments automatically each function call
Jamie Smith 1:aac28ffd63ed 868 shtpData[2] = command; //Command
Jamie Smith 1:aac28ffd63ed 869
Jamie Smith 1:aac28ffd63ed 870 //Caller must set these
Jamie Smith 1:aac28ffd63ed 871 /*shtpData[3] = 0; //P0
Jamie Smith 1:aac28ffd63ed 872 shtpData[4] = 0; //P1
Jamie Smith 1:aac28ffd63ed 873 shtpData[5] = 0; //P2
Jamie Smith 1:aac28ffd63ed 874 shtpData[6] = 0;
Jamie Smith 1:aac28ffd63ed 875 shtpData[7] = 0;
Jamie Smith 1:aac28ffd63ed 876 shtpData[8] = 0;
Jamie Smith 1:aac28ffd63ed 877 shtpData[9] = 0;
Jamie Smith 1:aac28ffd63ed 878 shtpData[10] = 0;
Jamie Smith 1:aac28ffd63ed 879 shtpData[11] = 0;*/
Jamie Smith 1:aac28ffd63ed 880
Jamie Smith 1:aac28ffd63ed 881 //Transmit packet on channel 2, 12 bytes
Jamie Smith 1:aac28ffd63ed 882 sendPacket(CHANNEL_CONTROL, 12);
Jamie Smith 1:aac28ffd63ed 883 }
Jamie Smith 1:aac28ffd63ed 884
Jamie Smith 1:aac28ffd63ed 885 //Given a sensor's report ID, this tells the BNO080 to begin reporting the values
Jamie Smith 1:aac28ffd63ed 886 //Also sets the specific config word. Useful for personal activity classifier
Jamie Smith 1:aac28ffd63ed 887 void BNO080::setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig)
Jamie Smith 1:aac28ffd63ed 888 {
Jamie Smith 1:aac28ffd63ed 889 uint32_t microsBetweenReports = static_cast<uint32_t>(timeBetweenReports * 1000);
Jamie Smith 1:aac28ffd63ed 890
Jamie Smith 1:aac28ffd63ed 891 const uint32_t batchMicros = 0;
Jamie Smith 1:aac28ffd63ed 892
Jamie Smith 1:aac28ffd63ed 893 shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55
Jamie Smith 1:aac28ffd63ed 894 shtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
Jamie Smith 1:aac28ffd63ed 895 shtpData[2] = 0; //Feature flags
Jamie Smith 1:aac28ffd63ed 896 shtpData[3] = 0; //Change sensitivity (LSB)
Jamie Smith 1:aac28ffd63ed 897 shtpData[4] = 0; //Change sensitivity (MSB)
Jamie Smith 1:aac28ffd63ed 898 shtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms
Jamie Smith 1:aac28ffd63ed 899 shtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval
Jamie Smith 1:aac28ffd63ed 900 shtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval
Jamie Smith 1:aac28ffd63ed 901 shtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
Jamie Smith 1:aac28ffd63ed 902 shtpData[9] = (batchMicros >> 0) & 0xFF; //Batch Interval (LSB)
Jamie Smith 1:aac28ffd63ed 903 shtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval
Jamie Smith 1:aac28ffd63ed 904 shtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval
Jamie Smith 1:aac28ffd63ed 905 shtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB)
Jamie Smith 1:aac28ffd63ed 906 shtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB)
Jamie Smith 1:aac28ffd63ed 907 shtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config
Jamie Smith 1:aac28ffd63ed 908 shtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config
Jamie Smith 1:aac28ffd63ed 909 shtpData[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB)
Jamie Smith 1:aac28ffd63ed 910
Jamie Smith 1:aac28ffd63ed 911 //Transmit packet on channel 2, 17 bytes
Jamie Smith 1:aac28ffd63ed 912 sendPacket(CHANNEL_CONTROL, 17);
Jamie Smith 1:aac28ffd63ed 913 }
Jamie Smith 1:aac28ffd63ed 914
Jamie Smith 1:aac28ffd63ed 915 bool BNO080::readFRSRecord(uint16_t recordID, uint32_t* readBuffer, uint16_t readLength)
Jamie Smith 1:aac28ffd63ed 916 {
Jamie Smith 1:aac28ffd63ed 917 // send initial read request
Jamie Smith 1:aac28ffd63ed 918 zeroBuffer();
Jamie Smith 1:aac28ffd63ed 919
Jamie Smith 1:aac28ffd63ed 920 shtpData[0] = SHTP_REPORT_FRS_READ_REQUEST;
Jamie Smith 1:aac28ffd63ed 921 // read offset of 0 -> start at the start of the record
Jamie Smith 1:aac28ffd63ed 922 shtpData[2] = 0;
Jamie Smith 1:aac28ffd63ed 923 shtpData[3] = 0;
Jamie Smith 1:aac28ffd63ed 924 // record ID
Jamie Smith 1:aac28ffd63ed 925 shtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
Jamie Smith 1:aac28ffd63ed 926 shtpData[5] = static_cast<uint8_t>(recordID >> 8);
Jamie Smith 1:aac28ffd63ed 927 // block size
Jamie Smith 1:aac28ffd63ed 928 shtpData[6] = static_cast<uint8_t>(readLength & 0xFF);
Jamie Smith 1:aac28ffd63ed 929 shtpData[7] = static_cast<uint8_t>(readLength >> 8);
Jamie Smith 1:aac28ffd63ed 930
Jamie Smith 1:aac28ffd63ed 931 sendPacket(CHANNEL_CONTROL, 8);
Jamie Smith 1:aac28ffd63ed 932
Jamie Smith 1:aac28ffd63ed 933 // now, read back the responses
Jamie Smith 1:aac28ffd63ed 934 size_t readOffset = 0;
Jamie Smith 1:aac28ffd63ed 935 while(readOffset < readLength)
Jamie Smith 1:aac28ffd63ed 936 {
Jamie Smith 1:aac28ffd63ed 937 if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE))
Jamie Smith 1:aac28ffd63ed 938 {
Jamie Smith 1:aac28ffd63ed 939 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 940 _debugPort->printf("Error: did not receive FRS read response after sending read request!\n");
Jamie Smith 1:aac28ffd63ed 941 #endif
Jamie Smith 1:aac28ffd63ed 942 return false;
Jamie Smith 1:aac28ffd63ed 943 }
Jamie Smith 1:aac28ffd63ed 944
Jamie Smith 1:aac28ffd63ed 945 uint8_t status = static_cast<uint8_t>(shtpData[1] & 0b1111);
Jamie Smith 1:aac28ffd63ed 946 uint8_t dataLength = shtpData[1] >> 4;
Jamie Smith 1:aac28ffd63ed 947
Jamie Smith 1:aac28ffd63ed 948 // check status
Jamie Smith 1:aac28ffd63ed 949 if(status == 1)
Jamie Smith 1:aac28ffd63ed 950 {
Jamie Smith 1:aac28ffd63ed 951 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 952 _debugPort->printf("Error: FRS reports invalid record ID!\n");
Jamie Smith 1:aac28ffd63ed 953 #endif
Jamie Smith 1:aac28ffd63ed 954 return false;
Jamie Smith 1:aac28ffd63ed 955 }
Jamie Smith 1:aac28ffd63ed 956 else if(status == 2)
Jamie Smith 1:aac28ffd63ed 957 {
Jamie Smith 1:aac28ffd63ed 958 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 959 _debugPort->printf("Error: FRS is busy!\n");
Jamie Smith 1:aac28ffd63ed 960 #endif
Jamie Smith 1:aac28ffd63ed 961 return false;
Jamie Smith 1:aac28ffd63ed 962 }
Jamie Smith 1:aac28ffd63ed 963 else if(status == 4)
Jamie Smith 1:aac28ffd63ed 964 {
Jamie Smith 1:aac28ffd63ed 965 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 966 _debugPort->printf("Error: FRS reports offset is out of range!\n");
Jamie Smith 1:aac28ffd63ed 967 #endif
Jamie Smith 1:aac28ffd63ed 968 return false;
Jamie Smith 1:aac28ffd63ed 969 }
Jamie Smith 1:aac28ffd63ed 970 else if(status == 5)
Jamie Smith 1:aac28ffd63ed 971 {
Jamie Smith 1:aac28ffd63ed 972 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 973 _debugPort->printf("Error: FRS reports record %hx is empty!\n", recordID);
Jamie Smith 1:aac28ffd63ed 974 #endif
Jamie Smith 1:aac28ffd63ed 975 return false;
Jamie Smith 1:aac28ffd63ed 976 }
Jamie Smith 1:aac28ffd63ed 977 else if(status == 8)
Jamie Smith 1:aac28ffd63ed 978 {
Jamie Smith 1:aac28ffd63ed 979 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 980 _debugPort->printf("Error: FRS reports flash memory device unavailable!\n");
Jamie Smith 1:aac28ffd63ed 981 #endif
Jamie Smith 1:aac28ffd63ed 982 return false;
Jamie Smith 1:aac28ffd63ed 983 }
Jamie Smith 1:aac28ffd63ed 984
Jamie Smith 1:aac28ffd63ed 985 // check data length
Jamie Smith 1:aac28ffd63ed 986 if(dataLength == 0)
Jamie Smith 1:aac28ffd63ed 987 {
Jamie Smith 1:aac28ffd63ed 988 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 989 _debugPort->printf("Error: Received FRS packet with 0 data length!\n");
Jamie Smith 1:aac28ffd63ed 990 #endif
Jamie Smith 1:aac28ffd63ed 991 return false;
Jamie Smith 1:aac28ffd63ed 992 }
Jamie Smith 1:aac28ffd63ed 993 else if(dataLength == 1)
Jamie Smith 1:aac28ffd63ed 994 {
Jamie Smith 1:aac28ffd63ed 995 if(readOffset + 1 != readLength)
Jamie Smith 1:aac28ffd63ed 996 {
Jamie Smith 1:aac28ffd63ed 997 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 998 _debugPort->printf("Error: Received 1 length packet but more than 1 byte remains to be be read!\n");
Jamie Smith 1:aac28ffd63ed 999 #endif
Jamie Smith 1:aac28ffd63ed 1000 return false;
Jamie Smith 1:aac28ffd63ed 1001 }
Jamie Smith 1:aac28ffd63ed 1002 }
Jamie Smith 1:aac28ffd63ed 1003
Jamie Smith 1:aac28ffd63ed 1004 // now, _finally_, read the dang words
Jamie Smith 1:aac28ffd63ed 1005 readBuffer[readOffset] = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | (shtpData[4]);
Jamie Smith 1:aac28ffd63ed 1006
Jamie Smith 1:aac28ffd63ed 1007 // check if we only wanted the first word
Jamie Smith 1:aac28ffd63ed 1008 ++readOffset;
Jamie Smith 1:aac28ffd63ed 1009 if(readOffset == readLength)
Jamie Smith 1:aac28ffd63ed 1010 {
Jamie Smith 1:aac28ffd63ed 1011 break;
Jamie Smith 1:aac28ffd63ed 1012 }
Jamie Smith 1:aac28ffd63ed 1013
Jamie Smith 1:aac28ffd63ed 1014 readBuffer[readOffset] = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | (shtpData[8]);
Jamie Smith 1:aac28ffd63ed 1015 readOffset++;
Jamie Smith 1:aac28ffd63ed 1016 }
Jamie Smith 1:aac28ffd63ed 1017
Jamie Smith 1:aac28ffd63ed 1018 // read successful
Jamie Smith 1:aac28ffd63ed 1019 return true;
Jamie Smith 1:aac28ffd63ed 1020
Jamie Smith 1:aac28ffd63ed 1021 }
Jamie Smith 1:aac28ffd63ed 1022
Jamie Smith 1:aac28ffd63ed 1023 //Given the data packet, send the header then the data
Jamie Smith 1:aac28ffd63ed 1024 //Returns false if sensor does not ACK
Jamie Smith 1:aac28ffd63ed 1025 bool BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength)
Jamie Smith 1:aac28ffd63ed 1026 {
Jamie Smith 1:aac28ffd63ed 1027 // start the transaction and contact the IMU
Jamie Smith 1:aac28ffd63ed 1028 _i2cPort.start();
Jamie Smith 1:aac28ffd63ed 1029
Jamie Smith 1:aac28ffd63ed 1030 // to indicate an i2c read, shift the 7 bit address up 1 bit and keep bit 0 as a 0
Jamie Smith 1:aac28ffd63ed 1031 int writeResult = _i2cPort.write(_i2cAddress << 1);
Jamie Smith 1:aac28ffd63ed 1032
Jamie Smith 1:aac28ffd63ed 1033 if(writeResult != 1)
Jamie Smith 1:aac28ffd63ed 1034 {
Jamie Smith 1:aac28ffd63ed 1035 _debugPort->printf("BNO I2C write failed!\n");
Jamie Smith 1:aac28ffd63ed 1036 _i2cPort.stop();
Jamie Smith 1:aac28ffd63ed 1037 return false;
Jamie Smith 1:aac28ffd63ed 1038 }
Jamie Smith 1:aac28ffd63ed 1039
Jamie Smith 1:aac28ffd63ed 1040
Jamie Smith 1:aac28ffd63ed 1041 uint16_t totalLength = dataLength + 4; //Add four bytes for the header
Jamie Smith 1:aac28ffd63ed 1042 packetLength = dataLength;
Jamie Smith 1:aac28ffd63ed 1043
Jamie Smith 1:aac28ffd63ed 1044 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 1045 shtpHeader[0] = totalLength & 0xFF;
Jamie Smith 1:aac28ffd63ed 1046 shtpHeader[1] = totalLength >> 8;
Jamie Smith 1:aac28ffd63ed 1047 shtpHeader[2] = channelNumber;
Jamie Smith 1:aac28ffd63ed 1048 shtpHeader[3] = sequenceNumber[channelNumber];
Jamie Smith 1:aac28ffd63ed 1049
Jamie Smith 1:aac28ffd63ed 1050 _debugPort->printf("Transmitting packet: ----------------\n");
Jamie Smith 1:aac28ffd63ed 1051 printPacket();
Jamie Smith 1:aac28ffd63ed 1052 #endif
Jamie Smith 1:aac28ffd63ed 1053
Jamie Smith 1:aac28ffd63ed 1054 //Send the 4 byte packet header
Jamie Smith 1:aac28ffd63ed 1055 _i2cPort.write(totalLength & 0xFF); //Packet length LSB
Jamie Smith 1:aac28ffd63ed 1056 _i2cPort.write(totalLength >> 8); //Packet length MSB
Jamie Smith 1:aac28ffd63ed 1057 _i2cPort.write(channelNumber); //Channel number
Jamie Smith 1:aac28ffd63ed 1058 _i2cPort.write(sequenceNumber[channelNumber]++); //Send the sequence number, increments with each packet sent, different counter for each channel
Jamie Smith 1:aac28ffd63ed 1059
Jamie Smith 1:aac28ffd63ed 1060 //Send the user's data packet
Jamie Smith 1:aac28ffd63ed 1061 for (uint8_t i = 0 ; i < dataLength ; i++)
Jamie Smith 1:aac28ffd63ed 1062 {
Jamie Smith 1:aac28ffd63ed 1063 _i2cPort.write(shtpData[i]);
Jamie Smith 1:aac28ffd63ed 1064 }
Jamie Smith 1:aac28ffd63ed 1065 _i2cPort.stop();
Jamie Smith 1:aac28ffd63ed 1066
Jamie Smith 1:aac28ffd63ed 1067 return (true);
Jamie Smith 1:aac28ffd63ed 1068 }
Jamie Smith 1:aac28ffd63ed 1069
Jamie Smith 1:aac28ffd63ed 1070 //Check to see if there is any new data available
Jamie Smith 1:aac28ffd63ed 1071 //Read the contents of the incoming packet into the shtpData array
Jamie Smith 1:aac28ffd63ed 1072 bool BNO080::receivePacket(float timeout)
Jamie Smith 1:aac28ffd63ed 1073 {
Jamie Smith 1:aac28ffd63ed 1074 Timer waitStartTime;
Jamie Smith 1:aac28ffd63ed 1075 waitStartTime.start();
Jamie Smith 1:aac28ffd63ed 1076
Jamie Smith 1:aac28ffd63ed 1077 while(_int.read() != 0)
Jamie Smith 1:aac28ffd63ed 1078 {
Jamie Smith 1:aac28ffd63ed 1079 if(waitStartTime.read() > timeout)
Jamie Smith 1:aac28ffd63ed 1080 {
Jamie Smith 1:aac28ffd63ed 1081 _debugPort->printf("BNO I2C wait timeout\n");
Jamie Smith 1:aac28ffd63ed 1082 return false;
Jamie Smith 1:aac28ffd63ed 1083 }
Jamie Smith 1:aac28ffd63ed 1084
Jamie Smith 1:aac28ffd63ed 1085 }
Jamie Smith 1:aac28ffd63ed 1086
Jamie Smith 1:aac28ffd63ed 1087 // start the transaction and contact the IMU
Jamie Smith 1:aac28ffd63ed 1088 _i2cPort.start();
Jamie Smith 1:aac28ffd63ed 1089
Jamie Smith 1:aac28ffd63ed 1090 // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1
Jamie Smith 1:aac28ffd63ed 1091 int writeResult = _i2cPort.write((_i2cAddress << 1) | 0x1);
Jamie Smith 1:aac28ffd63ed 1092
Jamie Smith 1:aac28ffd63ed 1093 if(writeResult != 1)
Jamie Smith 1:aac28ffd63ed 1094 {
Jamie Smith 1:aac28ffd63ed 1095 _debugPort->printf("BNO I2C read failed!\n");
Jamie Smith 1:aac28ffd63ed 1096 return false;
Jamie Smith 1:aac28ffd63ed 1097 }
Jamie Smith 1:aac28ffd63ed 1098
Jamie Smith 1:aac28ffd63ed 1099 //Get the first four bytes, aka the packet header
Jamie Smith 1:aac28ffd63ed 1100 uint8_t packetLSB = static_cast<uint8_t>(_i2cPort.read(true));
Jamie Smith 1:aac28ffd63ed 1101 uint8_t packetMSB = static_cast<uint8_t>(_i2cPort.read(true));
Jamie Smith 1:aac28ffd63ed 1102 uint8_t channelNumber = static_cast<uint8_t>(_i2cPort.read(true));
Jamie Smith 1:aac28ffd63ed 1103 uint8_t sequenceNum = static_cast<uint8_t>(_i2cPort.read(true)); //Not sure if we need to store this or not
Jamie Smith 1:aac28ffd63ed 1104
Jamie Smith 1:aac28ffd63ed 1105 //Store the header info
Jamie Smith 1:aac28ffd63ed 1106 shtpHeader[0] = packetLSB;
Jamie Smith 1:aac28ffd63ed 1107 shtpHeader[1] = packetMSB;
Jamie Smith 1:aac28ffd63ed 1108 shtpHeader[2] = channelNumber;
Jamie Smith 1:aac28ffd63ed 1109 shtpHeader[3] = sequenceNum;
Jamie Smith 1:aac28ffd63ed 1110
Jamie Smith 1:aac28ffd63ed 1111 if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF)
Jamie Smith 1:aac28ffd63ed 1112 {
Jamie Smith 1:aac28ffd63ed 1113 // invalid according to BNO080 datasheet section 1.4.1
Jamie Smith 1:aac28ffd63ed 1114
Jamie Smith 1:aac28ffd63ed 1115 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 1116 _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n");
Jamie Smith 1:aac28ffd63ed 1117 #endif
Jamie Smith 1:aac28ffd63ed 1118 return false;
Jamie Smith 1:aac28ffd63ed 1119 }
Jamie Smith 1:aac28ffd63ed 1120
Jamie Smith 1:aac28ffd63ed 1121 //Calculate the number of data bytes in this packet
Jamie Smith 1:aac28ffd63ed 1122 packetLength = (static_cast<uint16_t>(packetMSB) << 8 | packetLSB);
Jamie Smith 1:aac28ffd63ed 1123
Jamie Smith 1:aac28ffd63ed 1124 // Clear the MSbit.
Jamie Smith 1:aac28ffd63ed 1125 // 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)
Jamie Smith 1:aac28ffd63ed 1126 // but we don't actually care about any of the advertisement packets
Jamie Smith 1:aac28ffd63ed 1127 // that use this, so we can just cut off the rest of the packet by releasing chip select.
Jamie Smith 1:aac28ffd63ed 1128 packetLength &= ~(1 << 15);
Jamie Smith 1:aac28ffd63ed 1129
Jamie Smith 1:aac28ffd63ed 1130 if (packetLength == 0)
Jamie Smith 1:aac28ffd63ed 1131 {
Jamie Smith 1:aac28ffd63ed 1132 // Packet is empty
Jamie Smith 1:aac28ffd63ed 1133 return (false); //All done
Jamie Smith 1:aac28ffd63ed 1134 }
Jamie Smith 1:aac28ffd63ed 1135
Jamie Smith 1:aac28ffd63ed 1136 packetLength -= 4; //Remove the header bytes from the data count
Jamie Smith 1:aac28ffd63ed 1137
Jamie Smith 1:aac28ffd63ed 1138 //Read incoming data into the shtpData array
Jamie Smith 1:aac28ffd63ed 1139 for (uint16_t dataSpot = 0 ; dataSpot < packetLength ; dataSpot++)
Jamie Smith 1:aac28ffd63ed 1140 {
Jamie Smith 1:aac28ffd63ed 1141 bool sendACK = dataSpot < packetLength - 1;
Jamie Smith 1:aac28ffd63ed 1142
Jamie Smith 1:aac28ffd63ed 1143 // per the datasheet, 0xFF is used as filler for the receiver to transmit back
Jamie Smith 1:aac28ffd63ed 1144 uint8_t incoming = static_cast<uint8_t>(_i2cPort.read(sendACK));
Jamie Smith 1:aac28ffd63ed 1145 if (dataSpot < STORED_PACKET_SIZE) //BNO080 can respond with upto 270 bytes, avoid overflow
Jamie Smith 1:aac28ffd63ed 1146 shtpData[dataSpot] = incoming; //Store data into the shtpData array
Jamie Smith 1:aac28ffd63ed 1147 }
Jamie Smith 1:aac28ffd63ed 1148
Jamie Smith 1:aac28ffd63ed 1149 _i2cPort.stop();
Jamie Smith 1:aac28ffd63ed 1150
Jamie Smith 1:aac28ffd63ed 1151 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 1152 _debugPort->printf("Recieved packet: ----------------\n");
Jamie Smith 1:aac28ffd63ed 1153 printPacket(); // note: add 4 for the header length
Jamie Smith 1:aac28ffd63ed 1154 #endif
Jamie Smith 1:aac28ffd63ed 1155
Jamie Smith 1:aac28ffd63ed 1156 return (true); //We're done!
Jamie Smith 1:aac28ffd63ed 1157 }
Jamie Smith 1:aac28ffd63ed 1158
Jamie Smith 1:aac28ffd63ed 1159 //Pretty prints the contents of the current shtp header and data packets
Jamie Smith 1:aac28ffd63ed 1160 void BNO080::printPacket()
Jamie Smith 1:aac28ffd63ed 1161 {
Jamie Smith 1:aac28ffd63ed 1162 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 1163 //Print the four byte header
Jamie Smith 1:aac28ffd63ed 1164 _debugPort->printf("Header:");
Jamie Smith 1:aac28ffd63ed 1165 for (uint8_t x = 0 ; x < 4 ; x++)
Jamie Smith 1:aac28ffd63ed 1166 {
Jamie Smith 1:aac28ffd63ed 1167 _debugPort->printf(" ");
Jamie Smith 1:aac28ffd63ed 1168 if (shtpHeader[x] < 0x10) _debugPort->printf("0");
Jamie Smith 1:aac28ffd63ed 1169 _debugPort->printf("%hhx", shtpHeader[x]);
Jamie Smith 1:aac28ffd63ed 1170 }
Jamie Smith 1:aac28ffd63ed 1171
Jamie Smith 1:aac28ffd63ed 1172 uint16_t printLength = packetLength;
Jamie Smith 1:aac28ffd63ed 1173 if (printLength > 40) printLength = 40; //Artificial limit. We don't want the phone book.
Jamie Smith 1:aac28ffd63ed 1174
Jamie Smith 1:aac28ffd63ed 1175 _debugPort->printf(" Body:");
Jamie Smith 1:aac28ffd63ed 1176 for (uint16_t x = 0 ; x < printLength ; x++)
Jamie Smith 1:aac28ffd63ed 1177 {
Jamie Smith 1:aac28ffd63ed 1178 _debugPort->printf(" ");
Jamie Smith 1:aac28ffd63ed 1179 if (shtpData[x] < 0x10) _debugPort->printf("0");
Jamie Smith 1:aac28ffd63ed 1180 _debugPort->printf("%hhx", shtpData[x]);
Jamie Smith 1:aac28ffd63ed 1181 }
Jamie Smith 1:aac28ffd63ed 1182
Jamie Smith 1:aac28ffd63ed 1183 _debugPort->printf(", Length:");
Jamie Smith 1:aac28ffd63ed 1184 _debugPort->printf("%hhu", packetLength + SHTP_HEADER_SIZE);
Jamie Smith 1:aac28ffd63ed 1185
Jamie Smith 1:aac28ffd63ed 1186 if(shtpHeader[1] >> 7)
Jamie Smith 1:aac28ffd63ed 1187 {
Jamie Smith 1:aac28ffd63ed 1188 _debugPort->printf("[C]");
Jamie Smith 1:aac28ffd63ed 1189 }
Jamie Smith 1:aac28ffd63ed 1190
Jamie Smith 1:aac28ffd63ed 1191 _debugPort->printf(", SeqNum: %hhu", shtpHeader[3]);
Jamie Smith 1:aac28ffd63ed 1192
Jamie Smith 1:aac28ffd63ed 1193 _debugPort->printf(", Channel:");
Jamie Smith 1:aac28ffd63ed 1194 if (shtpHeader[2] == 0) _debugPort->printf("Command");
Jamie Smith 1:aac28ffd63ed 1195 else if (shtpHeader[2] == 1) _debugPort->printf("Executable");
Jamie Smith 1:aac28ffd63ed 1196 else if (shtpHeader[2] == 2) _debugPort->printf("Control");
Jamie Smith 1:aac28ffd63ed 1197 else if (shtpHeader[2] == 3) _debugPort->printf("Sensor-report");
Jamie Smith 1:aac28ffd63ed 1198 else if (shtpHeader[2] == 4) _debugPort->printf("Wake-report");
Jamie Smith 1:aac28ffd63ed 1199 else if (shtpHeader[2] == 5) _debugPort->printf("Gyro-vector");
Jamie Smith 1:aac28ffd63ed 1200 else _debugPort->printf("%hhu", shtpHeader[2]);
Jamie Smith 1:aac28ffd63ed 1201
Jamie Smith 1:aac28ffd63ed 1202 _debugPort->printf("\n");
Jamie Smith 1:aac28ffd63ed 1203 #endif
Jamie Smith 1:aac28ffd63ed 1204 }
Jamie Smith 1:aac28ffd63ed 1205
Jamie Smith 1:aac28ffd63ed 1206
Jamie Smith 1:aac28ffd63ed 1207 void BNO080::zeroBuffer()
Jamie Smith 1:aac28ffd63ed 1208 {
Jamie Smith 1:aac28ffd63ed 1209 memset(shtpHeader, 0, SHTP_HEADER_SIZE);
Jamie Smith 1:aac28ffd63ed 1210 memset(shtpData, 0, STORED_PACKET_SIZE);
Jamie Smith 1:aac28ffd63ed 1211 packetLength = 0;
Jamie Smith 1:aac28ffd63ed 1212 }
Jamie Smith 1:aac28ffd63ed 1213
Jamie Smith 1:aac28ffd63ed 1214 bool BNO080::loadReportMetadata(BNO080::Report report)
Jamie Smith 1:aac28ffd63ed 1215 {
Jamie Smith 1:aac28ffd63ed 1216 uint16_t reportMetaRecord;
Jamie Smith 1:aac28ffd63ed 1217
Jamie Smith 1:aac28ffd63ed 1218 // first, convert the report into the correct FRS record ID for that report's metadata
Jamie Smith 1:aac28ffd63ed 1219 // data from SH-2 section 5.1
Jamie Smith 1:aac28ffd63ed 1220 switch(report)
Jamie Smith 1:aac28ffd63ed 1221 {
Jamie Smith 2:2269b723d16a 1222 case TOTAL_ACCELERATION:
Jamie Smith 1:aac28ffd63ed 1223 reportMetaRecord = 0xE301;
Jamie Smith 1:aac28ffd63ed 1224 break;
Jamie Smith 2:2269b723d16a 1225 case LINEAR_ACCELERATION:
Jamie Smith 1:aac28ffd63ed 1226 reportMetaRecord = 0xE303;
Jamie Smith 1:aac28ffd63ed 1227 break;
Jamie Smith 2:2269b723d16a 1228 case GRAVITY_ACCELERATION:
Jamie Smith 1:aac28ffd63ed 1229 reportMetaRecord = 0xE304;
Jamie Smith 1:aac28ffd63ed 1230 break;
Jamie Smith 2:2269b723d16a 1231 case GYROSCOPE:
Jamie Smith 1:aac28ffd63ed 1232 reportMetaRecord = 0xE306;
Jamie Smith 1:aac28ffd63ed 1233 break;
Jamie Smith 2:2269b723d16a 1234 case MAG_FIELD:
Jamie Smith 1:aac28ffd63ed 1235 reportMetaRecord = 0xE309;
Jamie Smith 1:aac28ffd63ed 1236 break;
Jamie Smith 2:2269b723d16a 1237 case MAG_FIELD_UNCALIBRATED:
Jamie Smith 1:aac28ffd63ed 1238 reportMetaRecord = 0xE30A;
Jamie Smith 1:aac28ffd63ed 1239 break;
Jamie Smith 2:2269b723d16a 1240 case ROTATION:
Jamie Smith 1:aac28ffd63ed 1241 reportMetaRecord = 0xE30B;
Jamie Smith 1:aac28ffd63ed 1242 break;
Jamie Smith 2:2269b723d16a 1243 case GEOMAGNETIC_ROTATION:
Jamie Smith 1:aac28ffd63ed 1244 reportMetaRecord = 0xE30D;
Jamie Smith 1:aac28ffd63ed 1245 break;
Jamie Smith 2:2269b723d16a 1246 case GAME_ROTATION:
Jamie Smith 1:aac28ffd63ed 1247 reportMetaRecord = 0xE30C;
Jamie Smith 1:aac28ffd63ed 1248 break;
Jamie Smith 2:2269b723d16a 1249 case TAP_DETECTOR:
Jamie Smith 1:aac28ffd63ed 1250 reportMetaRecord = 0xE313;
Jamie Smith 1:aac28ffd63ed 1251 break;
Jamie Smith 2:2269b723d16a 1252 case STABILITY_CLASSIFIER:
Jamie Smith 1:aac28ffd63ed 1253 reportMetaRecord = 0xE317;
Jamie Smith 1:aac28ffd63ed 1254 break;
Jamie Smith 2:2269b723d16a 1255 case STEP_DETECTOR:
Jamie Smith 1:aac28ffd63ed 1256 reportMetaRecord = 0xE314;
Jamie Smith 1:aac28ffd63ed 1257 break;
Jamie Smith 2:2269b723d16a 1258 case STEP_COUNTER:
Jamie Smith 1:aac28ffd63ed 1259 reportMetaRecord = 0xE315;
Jamie Smith 1:aac28ffd63ed 1260 break;
Jamie Smith 2:2269b723d16a 1261 case SIGNIFICANT_MOTION:
Jamie Smith 1:aac28ffd63ed 1262 reportMetaRecord = 0xE316;
Jamie Smith 1:aac28ffd63ed 1263 break;
Jamie Smith 2:2269b723d16a 1264 case SHAKE_DETECTOR:
Jamie Smith 1:aac28ffd63ed 1265 reportMetaRecord = 0xE318;
Jamie Smith 1:aac28ffd63ed 1266 break;
Jamie Smith 1:aac28ffd63ed 1267 }
Jamie Smith 1:aac28ffd63ed 1268
Jamie Smith 1:aac28ffd63ed 1269 // if we already have that data stored, everything's OK
Jamie Smith 1:aac28ffd63ed 1270 if(bufferMetadataRecord == reportMetaRecord)
Jamie Smith 1:aac28ffd63ed 1271 {
Jamie Smith 1:aac28ffd63ed 1272 return true;
Jamie Smith 1:aac28ffd63ed 1273 }
Jamie Smith 1:aac28ffd63ed 1274
Jamie Smith 1:aac28ffd63ed 1275 // now, load the metadata into the buffer
Jamie Smith 1:aac28ffd63ed 1276 if(!readFRSRecord(reportMetaRecord, metadataRecord, METADATA_BUFFER_LEN))
Jamie Smith 1:aac28ffd63ed 1277 {
Jamie Smith 1:aac28ffd63ed 1278 // clear this so future calls won't try to use the cached version
Jamie Smith 1:aac28ffd63ed 1279 bufferMetadataRecord = 0;
Jamie Smith 1:aac28ffd63ed 1280
Jamie Smith 1:aac28ffd63ed 1281 return false;
Jamie Smith 1:aac28ffd63ed 1282 }
Jamie Smith 1:aac28ffd63ed 1283
Jamie Smith 1:aac28ffd63ed 1284 bufferMetadataRecord = reportMetaRecord;
Jamie Smith 1:aac28ffd63ed 1285
Jamie Smith 1:aac28ffd63ed 1286 return true;
Jamie Smith 1:aac28ffd63ed 1287 }