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 03:31:00 2018 -0800
Revision:
1:aac28ffd63ed
Parent:
0:f677e13975d0
Child:
2:2269b723d16a
Update from latest upstream, add missing headers

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