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:
Fri Jun 14 20:31:37 2019 -0700
Revision:
3:197ad972fb7c
Parent:
2:2269b723d16a
Child:
4:70d05578f041
Add permanent orientation support, fix a few bugs

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