Sheila Pham / BNOWrapper

start the wrapper burrito

BNO080.cpp@8:217f510db255, 2019-07-29 (annotated)

Committer:
sepham
Date:
Mon Jul 29 17:55:43 2019 +0000
Revision:
8:217f510db255
Parent:
7:050dc348e690 
strating the wrapper

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