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:
Thu Jan 30 02:16:15 2020 -0800
Revision:
4:70d05578f041
Parent:
3:197ad972fb7c
Child:
5:c75be9000d75
Child:
6:5ba996be5312
Fix a couple more minor issues

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