Affordable Smart Wheelchair / BNO080_library

Contains added code for stm32-L432KC compatibility

Dependents:   BNO080_stm32_compatible

BNO080.cpp@9:68e4b1932497, 2019-07-30 (annotated)

Committer:
JesiMiranda
Date:
Tue Jul 30 17:23:04 2019 +0000
Revision:
9:68e4b1932497
Parent:
8:92d4109beb3c 
Contains added code for stm32-L432KC compatibility

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 /// Set to 1 to enable debug printouts. Should be very useful if the chip is giving you trouble.
Jamie Smith 1:aac28ffd63ed 70 /// When debugging, it is recommended to use the highest possible serial baudrate so as not to interrupt the timing of operations.
JesiMiranda 6:546e4553cf6d 71 #define BNO_DEBUG 0
Jamie Smith 1:aac28ffd63ed 72
Jamie Smith 1:aac28ffd63ed 73 BNO080::BNO080(Serial *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin,
JesiMiranda 6:546e4553cf6d 74 uint8_t i2cAddress, int i2cPortSpeed) :
JesiMiranda 6:546e4553cf6d 75 _debugPort(debugPort),
JesiMiranda 6:546e4553cf6d 76 _i2cPort(user_SDApin, user_SCLpin),
JesiMiranda 6:546e4553cf6d 77 _i2cAddress(i2cAddress),
JesiMiranda 6:546e4553cf6d 78 _int(user_INTPin),
JesiMiranda 6:546e4553cf6d 79 _rst(user_RSTPin, 1),
JesiMiranda 6:546e4553cf6d 80 commandSequenceNumber(0),
JesiMiranda 6:546e4553cf6d 81 stability(UNKNOWN),
JesiMiranda 6:546e4553cf6d 82 stepDetected(false),
JesiMiranda 6:546e4553cf6d 83 stepCount(0),
JesiMiranda 6:546e4553cf6d 84 significantMotionDetected(false),
JesiMiranda 6:546e4553cf6d 85 shakeDetected(false),
JesiMiranda 6:546e4553cf6d 86 xAxisShake(false),
JesiMiranda 6:546e4553cf6d 87 yAxisShake(false),
JesiMiranda 6:546e4553cf6d 88 zAxisShake(false)
Jamie Smith 1:aac28ffd63ed 89 {
JesiMiranda 6:546e4553cf6d 90 // zero sequence numbers
JesiMiranda 6:546e4553cf6d 91 memset(sequenceNumber, 0, sizeof(sequenceNumber));
Jamie Smith 2:2269b723d16a 92
JesiMiranda 6:546e4553cf6d 93 //Get user settings
JesiMiranda 6:546e4553cf6d 94 _i2cPortSpeed = i2cPortSpeed;
JesiMiranda 6:546e4553cf6d 95 if(_i2cPortSpeed > 4000000) {
JesiMiranda 6:546e4553cf6d 96 _i2cPortSpeed = 4000000; //BNO080 max is 400Khz
JesiMiranda 6:546e4553cf6d 97 }
JesiMiranda 6:546e4553cf6d 98 _i2cPort.frequency(_i2cPortSpeed);
JesiMiranda 8:92d4109beb3c 99
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 //changed from sendCommand
JesiMiranda 8:92d4109beb3c 151 sendCommand(COMMAND_INITIALIZE);
JesiMiranda 8:92d4109beb3c 152
JesiMiranda 8:92d4109beb3c 153 wait(0.02f);
JesiMiranda 8:92d4109beb3c 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 8:92d4109beb3c 338 //wait(0.002f); //added
JesiMiranda 6:546e4553cf6d 339 }
Jamie Smith 1:aac28ffd63ed 340
JesiMiranda 6:546e4553cf6d 341 // packets were received, so data may have changed
JesiMiranda 6:546e4553cf6d 342 return true;
Jamie Smith 1:aac28ffd63ed 343 }
Jamie Smith 1:aac28ffd63ed 344
Jamie Smith 1:aac28ffd63ed 345 uint8_t BNO080::getReportStatus(Report report)
Jamie Smith 1:aac28ffd63ed 346 {
JesiMiranda 6:546e4553cf6d 347 uint8_t reportNum = static_cast<uint8_t>(report);
JesiMiranda 6:546e4553cf6d 348 if(reportNum > STATUS_ARRAY_LEN) {
JesiMiranda 6:546e4553cf6d 349 return 0;
JesiMiranda 6:546e4553cf6d 350 }
Jamie Smith 1:aac28ffd63ed 351
JesiMiranda 6:546e4553cf6d 352 return reportStatus[reportNum];
Jamie Smith 1:aac28ffd63ed 353 }
Jamie Smith 1:aac28ffd63ed 354
Jamie Smith 1:aac28ffd63ed 355 const char* BNO080::getReportStatusString(Report report)
Jamie Smith 1:aac28ffd63ed 356 {
JesiMiranda 6:546e4553cf6d 357 switch(getReportStatus(report)) {
JesiMiranda 6:546e4553cf6d 358 case 0:
JesiMiranda 6:546e4553cf6d 359 return "Unreliable";
JesiMiranda 6:546e4553cf6d 360 case 1:
JesiMiranda 6:546e4553cf6d 361 return "Accuracy Low";
JesiMiranda 6:546e4553cf6d 362 case 2:
JesiMiranda 6:546e4553cf6d 363 return "Accuracy Medium";
JesiMiranda 6:546e4553cf6d 364 case 3:
JesiMiranda 6:546e4553cf6d 365 return "Accuracy High";
JesiMiranda 6:546e4553cf6d 366 default:
JesiMiranda 6:546e4553cf6d 367 return "Error";
JesiMiranda 6:546e4553cf6d 368 }
Jamie Smith 1:aac28ffd63ed 369 }
Jamie Smith 1:aac28ffd63ed 370
Jamie Smith 1:aac28ffd63ed 371 bool BNO080::hasNewData(Report report)
Jamie Smith 1:aac28ffd63ed 372 {
JesiMiranda 6:546e4553cf6d 373 uint8_t reportNum = static_cast<uint8_t>(report);
JesiMiranda 6:546e4553cf6d 374 if(reportNum > STATUS_ARRAY_LEN) {
JesiMiranda 6:546e4553cf6d 375 return false;
JesiMiranda 6:546e4553cf6d 376 }
Jamie Smith 1:aac28ffd63ed 377
JesiMiranda 6:546e4553cf6d 378 bool newData = reportHasBeenUpdated[reportNum];
JesiMiranda 6:546e4553cf6d 379 reportHasBeenUpdated[reportNum] = false; // clear flag
JesiMiranda 6:546e4553cf6d 380 return newData;
Jamie Smith 1:aac28ffd63ed 381 }
Jamie Smith 1:aac28ffd63ed 382
Jamie Smith 1:aac28ffd63ed 383 //Sends the packet to enable the rotation vector
Jamie Smith 1:aac28ffd63ed 384 void BNO080::enableReport(Report report, uint16_t timeBetweenReports)
Jamie Smith 1:aac28ffd63ed 385 {
JesiMiranda 6:546e4553cf6d 386 // check time
JesiMiranda 6:546e4553cf6d 387 float periodSeconds = timeBetweenReports / 1000.0;
Jamie Smith 1:aac28ffd63ed 388
JesiMiranda 6:546e4553cf6d 389 if(periodSeconds < getMinPeriod(report)) {
JesiMiranda 6:546e4553cf6d 390 _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 391 static_cast<uint8_t>(report), periodSeconds, getMinPeriod(report));
JesiMiranda 6:546e4553cf6d 392 return;
JesiMiranda 6:546e4553cf6d 393 }
JesiMiranda 6:546e4553cf6d 394 /*
JesiMiranda 6:546e4553cf6d 395 else if(getMaxPeriod(report) > 0 && periodSeconds > getMaxPeriod(report))
JesiMiranda 6:546e4553cf6d 396 {
JesiMiranda 6:546e4553cf6d 397 _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 398 static_cast<uint8_t>(report), periodSeconds, getMaxPeriod(report));
JesiMiranda 6:546e4553cf6d 399 return;
JesiMiranda 6:546e4553cf6d 400 }
JesiMiranda 6:546e4553cf6d 401 */
JesiMiranda 6:546e4553cf6d 402 setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports);
Jamie Smith 1:aac28ffd63ed 403
JesiMiranda 6:546e4553cf6d 404 // 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 405 }
Jamie Smith 1:aac28ffd63ed 406
Jamie Smith 1:aac28ffd63ed 407 void BNO080::disableReport(Report report)
Jamie Smith 1:aac28ffd63ed 408 {
JesiMiranda 6:546e4553cf6d 409 // set the report's polling period to zero to disable it
JesiMiranda 6:546e4553cf6d 410 setFeatureCommand(static_cast<uint8_t>(report), 0);
Jamie Smith 1:aac28ffd63ed 411 }
Jamie Smith 1:aac28ffd63ed 412
Jamie Smith 1:aac28ffd63ed 413 uint32_t BNO080::getSerialNumber()
Jamie Smith 1:aac28ffd63ed 414 {
JesiMiranda 6:546e4553cf6d 415 uint32_t serNoBuffer;
Jamie Smith 1:aac28ffd63ed 416
JesiMiranda 6:546e4553cf6d 417 if(!readFRSRecord(FRS_RECORDID_SERIAL_NUMBER, &serNoBuffer, 1)) {
JesiMiranda 6:546e4553cf6d 418 return 0;
JesiMiranda 6:546e4553cf6d 419 }
Jamie Smith 1:aac28ffd63ed 420
JesiMiranda 6:546e4553cf6d 421 return serNoBuffer;
Jamie Smith 1:aac28ffd63ed 422 }
Jamie Smith 1:aac28ffd63ed 423
Jamie Smith 1:aac28ffd63ed 424 float BNO080::getRange(Report report)
Jamie Smith 1:aac28ffd63ed 425 {
JesiMiranda 6:546e4553cf6d 426 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 427
JesiMiranda 6:546e4553cf6d 428 return qToFloat_dword(metadataRecord[1], getQ1(report));
Jamie Smith 1:aac28ffd63ed 429 }
Jamie Smith 1:aac28ffd63ed 430
Jamie Smith 1:aac28ffd63ed 431
Jamie Smith 1:aac28ffd63ed 432 float BNO080::getResolution(Report report)
Jamie Smith 1:aac28ffd63ed 433 {
JesiMiranda 6:546e4553cf6d 434 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 435
JesiMiranda 6:546e4553cf6d 436 return qToFloat_dword(metadataRecord[2], getQ1(report));
Jamie Smith 1:aac28ffd63ed 437 }
Jamie Smith 1:aac28ffd63ed 438
Jamie Smith 1:aac28ffd63ed 439 float BNO080::getPower(Report report)
Jamie Smith 1:aac28ffd63ed 440 {
JesiMiranda 6:546e4553cf6d 441 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 442
JesiMiranda 6:546e4553cf6d 443 uint16_t powerQ = static_cast<uint16_t>(metadataRecord[3] & 0xFFFF);
Jamie Smith 1:aac28ffd63ed 444
JesiMiranda 6:546e4553cf6d 445 return qToFloat_dword(powerQ, POWER_Q_POINT);
Jamie Smith 1:aac28ffd63ed 446 }
Jamie Smith 1:aac28ffd63ed 447
Jamie Smith 1:aac28ffd63ed 448 float BNO080::getMinPeriod(Report report)
Jamie Smith 1:aac28ffd63ed 449 {
JesiMiranda 6:546e4553cf6d 450 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 451
JesiMiranda 6:546e4553cf6d 452 return metadataRecord[4] / 1e6f; // convert from microseconds to seconds
Jamie Smith 1:aac28ffd63ed 453 }
Jamie Smith 1:aac28ffd63ed 454
Jamie Smith 1:aac28ffd63ed 455 float BNO080::getMaxPeriod(Report report)
Jamie Smith 1:aac28ffd63ed 456 {
JesiMiranda 6:546e4553cf6d 457 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 458
JesiMiranda 6:546e4553cf6d 459 if(getMetaVersion() == 3) {
JesiMiranda 6:546e4553cf6d 460 // no max period entry in this record format
JesiMiranda 6:546e4553cf6d 461 return -1.0f;
JesiMiranda 6:546e4553cf6d 462 }
Jamie Smith 1:aac28ffd63ed 463
JesiMiranda 6:546e4553cf6d 464 return metadataRecord[9] / 1e6f; // convert from microseconds to seconds
Jamie Smith 1:aac28ffd63ed 465 }
Jamie Smith 1:aac28ffd63ed 466
Jamie Smith 1:aac28ffd63ed 467 void BNO080::printMetadataSummary(Report report)
Jamie Smith 1:aac28ffd63ed 468 {
Jamie Smith 1:aac28ffd63ed 469 #if BNO_DEBUG
JesiMiranda 6:546e4553cf6d 470 if(!loadReportMetadata(report)) {
JesiMiranda 6:546e4553cf6d 471 _debugPort->printf("Failed to load report metadata!\n");
JesiMiranda 6:546e4553cf6d 472 }
JesiMiranda 6:546e4553cf6d 473
JesiMiranda 6:546e4553cf6d 474 _debugPort->printf("======= Metadata for report 0x%02hhx =======\n", static_cast<uint8_t>(report));
Jamie Smith 1:aac28ffd63ed 475
JesiMiranda 6:546e4553cf6d 476 _debugPort->printf("Range: +- %.04f units\n", getRange(report));
JesiMiranda 6:546e4553cf6d 477 _debugPort->printf("Resolution: %.04f units\n", getResolution(report));
JesiMiranda 6:546e4553cf6d 478 _debugPort->printf("Power Used: %.03f mA\n", getPower(report));
JesiMiranda 6:546e4553cf6d 479 _debugPort->printf("Min Period: %.06f s\n", getMinPeriod(report));
JesiMiranda 6:546e4553cf6d 480 _debugPort->printf("Max Period: %.06f s\n\n", getMaxPeriod(report));
Jamie Smith 1:aac28ffd63ed 481
Jamie Smith 1:aac28ffd63ed 482 #endif
Jamie Smith 1:aac28ffd63ed 483 }
Jamie Smith 1:aac28ffd63ed 484
Jamie Smith 1:aac28ffd63ed 485 int16_t BNO080::getQ1(Report report)
Jamie Smith 1:aac28ffd63ed 486 {
JesiMiranda 6:546e4553cf6d 487 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 488
JesiMiranda 6:546e4553cf6d 489 return static_cast<int16_t>(metadataRecord[7] & 0xFFFF);
Jamie Smith 1:aac28ffd63ed 490 }
Jamie Smith 1:aac28ffd63ed 491
Jamie Smith 1:aac28ffd63ed 492 int16_t BNO080::getQ2(Report report)
Jamie Smith 1:aac28ffd63ed 493 {
JesiMiranda 6:546e4553cf6d 494 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 495
JesiMiranda 6:546e4553cf6d 496 return static_cast<int16_t>(metadataRecord[7] >> 16);
Jamie Smith 1:aac28ffd63ed 497 }
Jamie Smith 1:aac28ffd63ed 498
Jamie Smith 1:aac28ffd63ed 499 int16_t BNO080::getQ3(Report report)
Jamie Smith 1:aac28ffd63ed 500 {
JesiMiranda 6:546e4553cf6d 501 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 502
JesiMiranda 6:546e4553cf6d 503 return static_cast<int16_t>(metadataRecord[8] >> 16);
Jamie Smith 1:aac28ffd63ed 504 }
Jamie Smith 1:aac28ffd63ed 505
Jamie Smith 1:aac28ffd63ed 506 void BNO080::processPacket()
Jamie Smith 1:aac28ffd63ed 507 {
JesiMiranda 6:546e4553cf6d 508 if(shtpHeader[2] == CHANNEL_CONTROL) {
JesiMiranda 6:546e4553cf6d 509 // currently no command reports are read
JesiMiranda 6:546e4553cf6d 510 } else if(shtpHeader[2] == CHANNEL_EXECUTABLE) {
JesiMiranda 6:546e4553cf6d 511 // currently no executable reports are read
JesiMiranda 6:546e4553cf6d 512 } else if(shtpHeader[2] == CHANNEL_COMMAND) {
Jamie Smith 1:aac28ffd63ed 513
JesiMiranda 6:546e4553cf6d 514 } else if(shtpHeader[2] == CHANNEL_REPORTS || shtpHeader[2] == CHANNEL_WAKE_REPORTS) {
JesiMiranda 6:546e4553cf6d 515 if(shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP) {
JesiMiranda 6:546e4553cf6d 516 parseSensorDataPacket();
JesiMiranda 9:68e4b1932497 517
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 8:92d4109beb3c 760 while(timeoutTimer.read() <= 2*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 }
JesiMiranda 6:546e4553cf6d 775 }
JesiMiranda 6:546e4553cf6d 776 }
Jamie Smith 1:aac28ffd63ed 777
JesiMiranda 6:546e4553cf6d 778 _debugPort->printf("Packet wait timeout.\n");
JesiMiranda 6:546e4553cf6d 779 return false;
Jamie Smith 1:aac28ffd63ed 780 }
Jamie Smith 1:aac28ffd63ed 781
Jamie Smith 1:aac28ffd63ed 782 //Given a register value and a Q point, convert to float
Jamie Smith 1:aac28ffd63ed 783 //See https://en.wikipedia.org/wiki/Q_(number_format)
Jamie Smith 1:aac28ffd63ed 784 float BNO080::qToFloat(int16_t fixedPointValue, uint8_t qPoint)
Jamie Smith 1:aac28ffd63ed 785 {
JesiMiranda 6:546e4553cf6d 786 float qFloat = fixedPointValue;
JesiMiranda 6:546e4553cf6d 787 qFloat *= pow(2.0, qPoint * -1.0);
JesiMiranda 6:546e4553cf6d 788 return (qFloat);
Jamie Smith 1:aac28ffd63ed 789 }
Jamie Smith 1:aac28ffd63ed 790
Jamie Smith 1:aac28ffd63ed 791 float BNO080::qToFloat_dword(uint32_t fixedPointValue, int16_t qPoint)
Jamie Smith 1:aac28ffd63ed 792 {
JesiMiranda 6:546e4553cf6d 793 float qFloat = fixedPointValue;
JesiMiranda 6:546e4553cf6d 794 qFloat *= pow(2.0, qPoint * -1.0);
JesiMiranda 6:546e4553cf6d 795 return (qFloat);
Jamie Smith 1:aac28ffd63ed 796 }
Jamie Smith 1:aac28ffd63ed 797
Jamie Smith 1:aac28ffd63ed 798 //Given a floating point value and a Q point, convert to Q
Jamie Smith 1:aac28ffd63ed 799 //See https://en.wikipedia.org/wiki/Q_(number_format)
Jamie Smith 1:aac28ffd63ed 800 int16_t BNO080::floatToQ(float qFloat, uint8_t qPoint)
Jamie Smith 1:aac28ffd63ed 801 {
JesiMiranda 6:546e4553cf6d 802 int16_t qVal = static_cast<int16_t>(qFloat * pow(2.0, qPoint));
JesiMiranda 6:546e4553cf6d 803 return qVal;
Jamie Smith 1:aac28ffd63ed 804 }
Jamie Smith 1:aac28ffd63ed 805
Jamie Smith 1:aac28ffd63ed 806 //Tell the sensor to do a command
Jamie Smith 1:aac28ffd63ed 807 //See 6.3.8 page 41, Command request
Jamie Smith 1:aac28ffd63ed 808 //The caller is expected to set P0 through P8 prior to calling
Jamie Smith 1:aac28ffd63ed 809 void BNO080::sendCommand(uint8_t command)
Jamie Smith 1:aac28ffd63ed 810 {
JesiMiranda 6:546e4553cf6d 811 shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request
JesiMiranda 6:546e4553cf6d 812 shtpData[1] = commandSequenceNumber++; //Increments automatically each function call
JesiMiranda 6:546e4553cf6d 813 shtpData[2] = command; //Command
Jamie Smith 1:aac28ffd63ed 814
JesiMiranda 6:546e4553cf6d 815 //Caller must set these
JesiMiranda 8:92d4109beb3c 816 shtpData[3] = 0; //P0
JesiMiranda 6:546e4553cf6d 817 shtpData[4] = 0; //P1
JesiMiranda 6:546e4553cf6d 818 shtpData[5] = 0; //P2
JesiMiranda 6:546e4553cf6d 819 shtpData[6] = 0;
JesiMiranda 6:546e4553cf6d 820 shtpData[7] = 0;
JesiMiranda 6:546e4553cf6d 821 shtpData[8] = 0;
JesiMiranda 6:546e4553cf6d 822 shtpData[9] = 0;
JesiMiranda 6:546e4553cf6d 823 shtpData[10] = 0;
JesiMiranda 8:92d4109beb3c 824 shtpData[11] = 0;
JesiMiranda 6:546e4553cf6d 825
JesiMiranda 6:546e4553cf6d 826 //Transmit packet on channel 2, 12 bytes
JesiMiranda 6:546e4553cf6d 827 sendPacket(CHANNEL_CONTROL, 12);
Jamie Smith 1:aac28ffd63ed 828 }
Jamie Smith 1:aac28ffd63ed 829
Jamie Smith 1:aac28ffd63ed 830 //Given a sensor's report ID, this tells the BNO080 to begin reporting the values
Jamie Smith 1:aac28ffd63ed 831 //Also sets the specific config word. Useful for personal activity classifier
Jamie Smith 1:aac28ffd63ed 832 void BNO080::setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig)
Jamie Smith 1:aac28ffd63ed 833 {
JesiMiranda 6:546e4553cf6d 834 uint32_t microsBetweenReports = static_cast<uint32_t>(timeBetweenReports * 1000);
Jamie Smith 1:aac28ffd63ed 835
JesiMiranda 6:546e4553cf6d 836 const uint32_t batchMicros = 0;
Jamie Smith 1:aac28ffd63ed 837
JesiMiranda 6:546e4553cf6d 838 shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55
JesiMiranda 6:546e4553cf6d 839 shtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
JesiMiranda 6:546e4553cf6d 840 shtpData[2] = 0; //Feature flags
JesiMiranda 6:546e4553cf6d 841 shtpData[3] = 0; //Change sensitivity (LSB)
JesiMiranda 6:546e4553cf6d 842 shtpData[4] = 0; //Change sensitivity (MSB)
JesiMiranda 6:546e4553cf6d 843 shtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms
JesiMiranda 6:546e4553cf6d 844 shtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval
JesiMiranda 6:546e4553cf6d 845 shtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval
JesiMiranda 6:546e4553cf6d 846 shtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
JesiMiranda 6:546e4553cf6d 847 shtpData[9] = (batchMicros >> 0) & 0xFF; //Batch Interval (LSB)
JesiMiranda 6:546e4553cf6d 848 shtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval
JesiMiranda 6:546e4553cf6d 849 shtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval
JesiMiranda 6:546e4553cf6d 850 shtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB)
JesiMiranda 6:546e4553cf6d 851 shtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB)
JesiMiranda 6:546e4553cf6d 852 shtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config
JesiMiranda 6:546e4553cf6d 853 shtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config
JesiMiranda 6:546e4553cf6d 854 shtpData[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB)
Jamie Smith 1:aac28ffd63ed 855
JesiMiranda 6:546e4553cf6d 856 //Transmit packet on channel 2, 17 bytes
JesiMiranda 6:546e4553cf6d 857 sendPacket(CHANNEL_CONTROL, 17);
Jamie Smith 1:aac28ffd63ed 858 }
Jamie Smith 1:aac28ffd63ed 859
Jamie Smith 1:aac28ffd63ed 860 bool BNO080::readFRSRecord(uint16_t recordID, uint32_t* readBuffer, uint16_t readLength)
Jamie Smith 1:aac28ffd63ed 861 {
JesiMiranda 6:546e4553cf6d 862 // send initial read request
JesiMiranda 6:546e4553cf6d 863 zeroBuffer();
Jamie Smith 1:aac28ffd63ed 864
JesiMiranda 6:546e4553cf6d 865 shtpData[0] = SHTP_REPORT_FRS_READ_REQUEST;
JesiMiranda 6:546e4553cf6d 866 // read offset of 0 -> start at the start of the record
JesiMiranda 6:546e4553cf6d 867 shtpData[2] = 0;
JesiMiranda 6:546e4553cf6d 868 shtpData[3] = 0;
JesiMiranda 6:546e4553cf6d 869 // record ID
JesiMiranda 6:546e4553cf6d 870 shtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
JesiMiranda 6:546e4553cf6d 871 shtpData[5] = static_cast<uint8_t>(recordID >> 8);
JesiMiranda 6:546e4553cf6d 872 // block size
JesiMiranda 6:546e4553cf6d 873 shtpData[6] = static_cast<uint8_t>(readLength & 0xFF);
JesiMiranda 6:546e4553cf6d 874 shtpData[7] = static_cast<uint8_t>(readLength >> 8);
Jamie Smith 1:aac28ffd63ed 875
JesiMiranda 6:546e4553cf6d 876 sendPacket(CHANNEL_CONTROL, 8);
JesiMiranda 6:546e4553cf6d 877
JesiMiranda 6:546e4553cf6d 878 // now, read back the responses
JesiMiranda 6:546e4553cf6d 879 size_t readOffset = 0;
JesiMiranda 6:546e4553cf6d 880 while(readOffset < readLength) {
JesiMiranda 6:546e4553cf6d 881 if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE)) {
Jamie Smith 1:aac28ffd63ed 882 #if BNO_DEBUG
JesiMiranda 6:546e4553cf6d 883 _debugPort->printf("Error: did not receive FRS read response after sending read request!\n");
Jamie Smith 1:aac28ffd63ed 884 #endif
JesiMiranda 6:546e4553cf6d 885 return false;
JesiMiranda 6:546e4553cf6d 886 }
Jamie Smith 1:aac28ffd63ed 887
JesiMiranda 6:546e4553cf6d 888 uint8_t status = static_cast<uint8_t>(shtpData[1] & 0b1111);
JesiMiranda 6:546e4553cf6d 889 uint8_t dataLength = shtpData[1] >> 4;
Jamie Smith 1:aac28ffd63ed 890
JesiMiranda 6:546e4553cf6d 891 // check status
JesiMiranda 6:546e4553cf6d 892 if(status == 1) {
JesiMiranda 6:546e4553cf6d 893 #if BNO_DEBUG
JesiMiranda 6:546e4553cf6d 894 _debugPort->printf("Error: FRS reports invalid record ID!\n");
JesiMiranda 6:546e4553cf6d 895 #endif
JesiMiranda 6:546e4553cf6d 896 return false;
JesiMiranda 6:546e4553cf6d 897 } else if(status == 2) {
JesiMiranda 6:546e4553cf6d 898 #if BNO_DEBUG
JesiMiranda 6:546e4553cf6d 899 _debugPort->printf("Error: FRS is busy!\n");
JesiMiranda 6:546e4553cf6d 900 #endif
JesiMiranda 6:546e4553cf6d 901 return false;
JesiMiranda 6:546e4553cf6d 902 } else if(status == 4) {
Jamie Smith 3:197ad972fb7c 903 #if BNO_DEBUG
JesiMiranda 6:546e4553cf6d 904 _debugPort->printf("Error: FRS reports offset is out of range!\n");
Jamie Smith 3:197ad972fb7c 905 #endif
JesiMiranda 6:546e4553cf6d 906 return false;
JesiMiranda 6:546e4553cf6d 907 } else if(status == 5) {
Jamie Smith 3:197ad972fb7c 908 #if BNO_DEBUG
JesiMiranda 6:546e4553cf6d 909 _debugPort->printf("Error: FRS reports record %hx is empty!\n", recordID);
Jamie Smith 3:197ad972fb7c 910 #endif
JesiMiranda 6:546e4553cf6d 911 return false;
JesiMiranda 6:546e4553cf6d 912 } else if(status == 8) {
JesiMiranda 6:546e4553cf6d 913 #if BNO_DEBUG
JesiMiranda 6:546e4553cf6d 914 _debugPort->printf("Error: FRS reports flash memory device unavailable!\n");
JesiMiranda 6:546e4553cf6d 915 #endif
JesiMiranda 6:546e4553cf6d 916 return false;
JesiMiranda 6:546e4553cf6d 917 }
Jamie Smith 3:197ad972fb7c 918
JesiMiranda 6:546e4553cf6d 919 // check data length
JesiMiranda 6:546e4553cf6d 920 if(dataLength == 0) {
Jamie Smith 3:197ad972fb7c 921 #if BNO_DEBUG
JesiMiranda 6:546e4553cf6d 922 _debugPort->printf("Error: Received FRS packet with 0 data length!\n");
Jamie Smith 3:197ad972fb7c 923 #endif
JesiMiranda 6:546e4553cf6d 924 return false;
JesiMiranda 6:546e4553cf6d 925 } else if(dataLength == 1) {
JesiMiranda 6:546e4553cf6d 926 if(readOffset + 1 != readLength) {
Jamie Smith 3:197ad972fb7c 927 #if BNO_DEBUG
JesiMiranda 6:546e4553cf6d 928 _debugPort->printf("Error: Received 1 length packet but more than 1 byte remains to be be read!\n");
Jamie Smith 3:197ad972fb7c 929 #endif
JesiMiranda 6:546e4553cf6d 930 return false;
JesiMiranda 6:546e4553cf6d 931 }
JesiMiranda 6:546e4553cf6d 932 }
JesiMiranda 6:546e4553cf6d 933
JesiMiranda 6:546e4553cf6d 934 // now, _finally_, read the dang words
JesiMiranda 6:546e4553cf6d 935 readBuffer[readOffset] = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | (shtpData[4]);
Jamie Smith 3:197ad972fb7c 936
JesiMiranda 6:546e4553cf6d 937 // check if we only wanted the first word
JesiMiranda 6:546e4553cf6d 938 ++readOffset;
JesiMiranda 6:546e4553cf6d 939 if(readOffset == readLength) {
JesiMiranda 6:546e4553cf6d 940 break;
JesiMiranda 6:546e4553cf6d 941 }
Jamie Smith 3:197ad972fb7c 942
JesiMiranda 6:546e4553cf6d 943 readBuffer[readOffset] = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | (shtpData[8]);
JesiMiranda 6:546e4553cf6d 944 readOffset++;
JesiMiranda 6:546e4553cf6d 945 }
Jamie Smith 3:197ad972fb7c 946
JesiMiranda 6:546e4553cf6d 947 // read successful
JesiMiranda 6:546e4553cf6d 948 return true;
JesiMiranda 6:546e4553cf6d 949
Jamie Smith 3:197ad972fb7c 950 }
Jamie Smith 3:197ad972fb7c 951
Jamie Smith 1:aac28ffd63ed 952 //Given the data packet, send the header then the data
Jamie Smith 1:aac28ffd63ed 953 //Returns false if sensor does not ACK
Jamie Smith 1:aac28ffd63ed 954 bool BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength)
Jamie Smith 1:aac28ffd63ed 955 {
JesiMiranda 9:68e4b1932497 956
JesiMiranda 6:546e4553cf6d 957 uint16_t totalLength = dataLength + 4; //Add four bytes for the header
JesiMiranda 6:546e4553cf6d 958 packetLength = dataLength;
Jamie Smith 1:aac28ffd63ed 959
JesiMiranda 6:546e4553cf6d 960 shtpHeader[0] = totalLength & 0xFF;
JesiMiranda 6:546e4553cf6d 961 shtpHeader[1] = totalLength >> 8;
JesiMiranda 6:546e4553cf6d 962 shtpHeader[2] = channelNumber;
JesiMiranda 8:92d4109beb3c 963 shtpHeader[3] = sequenceNumber[channelNumber]++;
JesiMiranda 8:92d4109beb3c 964 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 965
JesiMiranda 6:546e4553cf6d 966 _debugPort->printf("Transmitting packet: ----------------\n");
JesiMiranda 6:546e4553cf6d 967 printPacket();
JesiMiranda 6:546e4553cf6d 968 #endif
JesiMiranda 6:546e4553cf6d 969
JesiMiranda 8:92d4109beb3c 970 readBuffer[0] = shtpHeader[0];
JesiMiranda 8:92d4109beb3c 971 readBuffer[1] = shtpHeader[1];
JesiMiranda 8:92d4109beb3c 972 readBuffer[2] = shtpHeader[2];
JesiMiranda 8:92d4109beb3c 973 readBuffer[3] = shtpHeader[3];
JesiMiranda 8:92d4109beb3c 974
JesiMiranda 8:92d4109beb3c 975 for(size_t index = 0; index < dataLength; ++index)
JesiMiranda 8:92d4109beb3c 976 {
JesiMiranda 8:92d4109beb3c 977 readBuffer[index + 4] = shtpData[index];
JesiMiranda 8:92d4109beb3c 978 }
JesiMiranda 8:92d4109beb3c 979
JesiMiranda 8:92d4109beb3c 980 int writeRetval = _i2cPort.write(
JesiMiranda 8:92d4109beb3c 981 _i2cAddress << 1,
JesiMiranda 8:92d4109beb3c 982 reinterpret_cast<char*>(readBuffer),
JesiMiranda 8:92d4109beb3c 983 totalLength);
JesiMiranda 8:92d4109beb3c 984
JesiMiranda 8:92d4109beb3c 985 if(writeRetval < 0)
JesiMiranda 8:92d4109beb3c 986 {
JesiMiranda 8:92d4109beb3c 987 _debugPort->printf("BNO I2C body write failed!\n");
JesiMiranda 8:92d4109beb3c 988 return false;
JesiMiranda 8:92d4109beb3c 989 }
JesiMiranda 8:92d4109beb3c 990
JesiMiranda 9:68e4b1932497 991
JesiMiranda 8:92d4109beb3c 992
JesiMiranda 6:546e4553cf6d 993 return (true);
Jamie Smith 1:aac28ffd63ed 994 }
Jamie Smith 1:aac28ffd63ed 995
Jamie Smith 1:aac28ffd63ed 996 //Check to see if there is any new data available
Jamie Smith 1:aac28ffd63ed 997 //Read the contents of the incoming packet into the shtpData array
Jamie Smith 1:aac28ffd63ed 998 bool BNO080::receivePacket(float timeout)
Jamie Smith 1:aac28ffd63ed 999 {
JesiMiranda 6:546e4553cf6d 1000 Timer waitStartTime;
JesiMiranda 6:546e4553cf6d 1001 waitStartTime.start();
Jamie Smith 1:aac28ffd63ed 1002
JesiMiranda 6:546e4553cf6d 1003 while(_int.read() != 0) {
JesiMiranda 6:546e4553cf6d 1004 if(waitStartTime.read() > timeout) {
JesiMiranda 6:546e4553cf6d 1005 _debugPort->printf("BNO I2C wait timeout\n");
JesiMiranda 6:546e4553cf6d 1006 return false;
JesiMiranda 6:546e4553cf6d 1007 }
JesiMiranda 6:546e4553cf6d 1008 }
JesiMiranda 8:92d4109beb3c 1009
JesiMiranda 8:92d4109beb3c 1010 const size_t headerLen = 4;
JesiMiranda 8:92d4109beb3c 1011 uint8_t headerData[headerLen];
JesiMiranda 8:92d4109beb3c 1012 int readRetval = _i2cPort.read(
JesiMiranda 8:92d4109beb3c 1013 (_i2cAddress << 1) | 0x1,
JesiMiranda 8:92d4109beb3c 1014 reinterpret_cast<char*>(headerData),
JesiMiranda 8:92d4109beb3c 1015 headerLen);
JesiMiranda 8:92d4109beb3c 1016
JesiMiranda 8:92d4109beb3c 1017 if(readRetval < 0)
JesiMiranda 8:92d4109beb3c 1018 {
JesiMiranda 8:92d4109beb3c 1019 _debugPort->printf("BNO I2C header read failed!\n");
JesiMiranda 8:92d4109beb3c 1020 return false;
JesiMiranda 8:92d4109beb3c 1021 }
Jamie Smith 1:aac28ffd63ed 1022
JesiMiranda 8:92d4109beb3c 1023
JesiMiranda 6:546e4553cf6d 1024 //Get the first four bytes, aka the packet header
JesiMiranda 8:92d4109beb3c 1025 uint8_t packetLSB = headerData[0];
JesiMiranda 8:92d4109beb3c 1026 uint8_t packetMSB = headerData[1];
JesiMiranda 8:92d4109beb3c 1027 uint8_t channelNumber = headerData[2];
JesiMiranda 8:92d4109beb3c 1028 uint8_t sequenceNum = headerData[3]; //Not sure if we need to store this or not
Jamie Smith 1:aac28ffd63ed 1029
JesiMiranda 6:546e4553cf6d 1030 //Store the header info
JesiMiranda 6:546e4553cf6d 1031 shtpHeader[0] = packetLSB;
JesiMiranda 6:546e4553cf6d 1032 shtpHeader[1] = packetMSB;
JesiMiranda 6:546e4553cf6d 1033 shtpHeader[2] = channelNumber;
JesiMiranda 6:546e4553cf6d 1034 shtpHeader[3] = sequenceNum;
JesiMiranda 6:546e4553cf6d 1035
JesiMiranda 6:546e4553cf6d 1036 if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF) {
JesiMiranda 6:546e4553cf6d 1037 // invalid according to BNO080 datasheet section 1.4.1
Jamie Smith 1:aac28ffd63ed 1038
JesiMiranda 6:546e4553cf6d 1039 _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n");
JesiMiranda 6:546e4553cf6d 1040 return false;
JesiMiranda 6:546e4553cf6d 1041 }
Jamie Smith 1:aac28ffd63ed 1042
JesiMiranda 6:546e4553cf6d 1043 //Calculate the number of data bytes in this packet
JesiMiranda 6:546e4553cf6d 1044 packetLength = (static_cast<uint16_t>(packetMSB) << 8 | packetLSB);
Jamie Smith 1:aac28ffd63ed 1045
JesiMiranda 6:546e4553cf6d 1046 // Clear the MSbit.
JesiMiranda 6:546e4553cf6d 1047 // 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 1048 // but we don't actually care about any of the advertisement packets
JesiMiranda 6:546e4553cf6d 1049 // that use this, so we can just cut off the rest of the packet by releasing chip select.
JesiMiranda 6:546e4553cf6d 1050 packetLength &= ~(1 << 15);
Jamie Smith 1:aac28ffd63ed 1051
JesiMiranda 6:546e4553cf6d 1052 if (packetLength == 0) {
JesiMiranda 6:546e4553cf6d 1053 // Packet is empty
JesiMiranda 6:546e4553cf6d 1054 return (false); //All done
JesiMiranda 6:546e4553cf6d 1055 }
JesiMiranda 8:92d4109beb3c 1056 else if(packetLength > READ_BUFFER_SIZE)
JesiMiranda 8:92d4109beb3c 1057 {
JesiMiranda 8:92d4109beb3c 1058 return false; // read buffer too small
JesiMiranda 8:92d4109beb3c 1059 }
Jamie Smith 1:aac28ffd63ed 1060
JesiMiranda 8:92d4109beb3c 1061 packetLength -= headerLen; //Remove the header bytes from the data count
JesiMiranda 8:92d4109beb3c 1062
JesiMiranda 8:92d4109beb3c 1063 readRetval = _i2cPort.read(
JesiMiranda 8:92d4109beb3c 1064 (_i2cAddress << 1) | 0x1,
JesiMiranda 8:92d4109beb3c 1065 reinterpret_cast<char*>(readBuffer),
JesiMiranda 8:92d4109beb3c 1066 packetLength + headerLen,
JesiMiranda 8:92d4109beb3c 1067 false);
JesiMiranda 8:92d4109beb3c 1068
JesiMiranda 8:92d4109beb3c 1069 if(readRetval < 0)
JesiMiranda 8:92d4109beb3c 1070 {
JesiMiranda 8:92d4109beb3c 1071 _debugPort->printf("BNO I2C body read failed!\n");
JesiMiranda 8:92d4109beb3c 1072 return false;
JesiMiranda 8:92d4109beb3c 1073 }
Jamie Smith 1:aac28ffd63ed 1074
JesiMiranda 6:546e4553cf6d 1075 //Read incoming data into the shtpData array
JesiMiranda 6:546e4553cf6d 1076 for (uint16_t dataSpot = 0 ; dataSpot < packetLength ; dataSpot++) {
Jamie Smith 1:aac28ffd63ed 1077
JesiMiranda 6:546e4553cf6d 1078 if (dataSpot < STORED_PACKET_SIZE) //BNO080 can respond with upto 270 bytes, avoid overflow
JesiMiranda 8:92d4109beb3c 1079 shtpData[dataSpot] = readBuffer[dataSpot + headerLen]; //Store data into the shtpData array
JesiMiranda 6:546e4553cf6d 1080 }
JesiMiranda 6:546e4553cf6d 1081
Jamie Smith 1:aac28ffd63ed 1082 #if BNO_DEBUG
JesiMiranda 6:546e4553cf6d 1083 _debugPort->printf("Recieved packet: ----------------\n");
JesiMiranda 6:546e4553cf6d 1084 printPacket(); // note: add 4 for the header length
Jamie Smith 1:aac28ffd63ed 1085 #endif
JesiMiranda 6:546e4553cf6d 1086 return (true); //We're done!
Jamie Smith 1:aac28ffd63ed 1087 }
Jamie Smith 1:aac28ffd63ed 1088
Jamie Smith 1:aac28ffd63ed 1089 //Pretty prints the contents of the current shtp header and data packets
Jamie Smith 1:aac28ffd63ed 1090 void BNO080::printPacket()
Jamie Smith 1:aac28ffd63ed 1091 {
Jamie Smith 1:aac28ffd63ed 1092 #if BNO_DEBUG
JesiMiranda 6:546e4553cf6d 1093 //Print the four byte header
JesiMiranda 6:546e4553cf6d 1094 _debugPort->printf("Header:");
JesiMiranda 6:546e4553cf6d 1095 for (uint8_t x = 0 ; x < 4 ; x++) {
JesiMiranda 6:546e4553cf6d 1096 _debugPort->printf(" ");
JesiMiranda 6:546e4553cf6d 1097 if (shtpHeader[x] < 0x10) _debugPort->printf("0");
JesiMiranda 6:546e4553cf6d 1098 _debugPort->printf("%hhx", shtpHeader[x]);
JesiMiranda 6:546e4553cf6d 1099 }
Jamie Smith 1:aac28ffd63ed 1100
JesiMiranda 6:546e4553cf6d 1101 uint16_t printLength = packetLength;
JesiMiranda 6:546e4553cf6d 1102 if (printLength > 40) printLength = 40; //Artificial limit. We don't want the phone book.
Jamie Smith 1:aac28ffd63ed 1103
JesiMiranda 6:546e4553cf6d 1104 _debugPort->printf(" Body:");
JesiMiranda 6:546e4553cf6d 1105 for (uint16_t x = 0 ; x < printLength ; x++) {
JesiMiranda 6:546e4553cf6d 1106 _debugPort->printf(" ");
JesiMiranda 6:546e4553cf6d 1107 if (shtpData[x] < 0x10) _debugPort->printf("0");
JesiMiranda 6:546e4553cf6d 1108 _debugPort->printf("%hhx", shtpData[x]);
JesiMiranda 6:546e4553cf6d 1109 }
Jamie Smith 1:aac28ffd63ed 1110
JesiMiranda 6:546e4553cf6d 1111 _debugPort->printf(", Length:");
JesiMiranda 6:546e4553cf6d 1112 _debugPort->printf("%hhu", packetLength + SHTP_HEADER_SIZE);
Jamie Smith 1:aac28ffd63ed 1113
JesiMiranda 6:546e4553cf6d 1114 if(shtpHeader[1] >> 7) {
JesiMiranda 6:546e4553cf6d 1115 _debugPort->printf("[C]");
JesiMiranda 6:546e4553cf6d 1116 }
Jamie Smith 1:aac28ffd63ed 1117
JesiMiranda 6:546e4553cf6d 1118 _debugPort->printf(", SeqNum: %hhu", shtpHeader[3]);
Jamie Smith 1:aac28ffd63ed 1119
JesiMiranda 6:546e4553cf6d 1120 _debugPort->printf(", Channel:");
JesiMiranda 6:546e4553cf6d 1121 if (shtpHeader[2] == 0) _debugPort->printf("Command");
JesiMiranda 6:546e4553cf6d 1122 else if (shtpHeader[2] == 1) _debugPort->printf("Executable");
JesiMiranda 6:546e4553cf6d 1123 else if (shtpHeader[2] == 2) _debugPort->printf("Control");
JesiMiranda 6:546e4553cf6d 1124 else if (shtpHeader[2] == 3) _debugPort->printf("Sensor-report");
JesiMiranda 6:546e4553cf6d 1125 else if (shtpHeader[2] == 4) _debugPort->printf("Wake-report");
JesiMiranda 6:546e4553cf6d 1126 else if (shtpHeader[2] == 5) _debugPort->printf("Gyro-vector");
JesiMiranda 6:546e4553cf6d 1127 else _debugPort->printf("%hhu", shtpHeader[2]);
Jamie Smith 1:aac28ffd63ed 1128
JesiMiranda 6:546e4553cf6d 1129 _debugPort->printf("\n");
Jamie Smith 1:aac28ffd63ed 1130 #endif
Jamie Smith 1:aac28ffd63ed 1131 }
Jamie Smith 1:aac28ffd63ed 1132
Jamie Smith 1:aac28ffd63ed 1133
Jamie Smith 1:aac28ffd63ed 1134 void BNO080::zeroBuffer()
Jamie Smith 1:aac28ffd63ed 1135 {
JesiMiranda 6:546e4553cf6d 1136 memset(shtpHeader, 0, SHTP_HEADER_SIZE);
JesiMiranda 6:546e4553cf6d 1137 memset(shtpData, 0, STORED_PACKET_SIZE);
JesiMiranda 6:546e4553cf6d 1138 packetLength = 0;
Jamie Smith 1:aac28ffd63ed 1139 }
Jamie Smith 1:aac28ffd63ed 1140
Jamie Smith 1:aac28ffd63ed 1141 bool BNO080::loadReportMetadata(BNO080::Report report)
Jamie Smith 1:aac28ffd63ed 1142 {
JesiMiranda 6:546e4553cf6d 1143 uint16_t reportMetaRecord;
JesiMiranda 8:92d4109beb3c 1144
JesiMiranda 6:546e4553cf6d 1145 // first, convert the report into the correct FRS record ID for that report's metadata
JesiMiranda 6:546e4553cf6d 1146 // data from SH-2 section 5.1
JesiMiranda 6:546e4553cf6d 1147 switch(report) {
JesiMiranda 6:546e4553cf6d 1148 case TOTAL_ACCELERATION:
JesiMiranda 6:546e4553cf6d 1149 reportMetaRecord = 0xE301;
JesiMiranda 6:546e4553cf6d 1150 break;
JesiMiranda 6:546e4553cf6d 1151 case LINEAR_ACCELERATION:
JesiMiranda 6:546e4553cf6d 1152 reportMetaRecord = 0xE303;
JesiMiranda 6:546e4553cf6d 1153 break;
JesiMiranda 6:546e4553cf6d 1154 case GRAVITY_ACCELERATION:
JesiMiranda 6:546e4553cf6d 1155 reportMetaRecord = 0xE304;
JesiMiranda 6:546e4553cf6d 1156 break;
JesiMiranda 6:546e4553cf6d 1157 case GYROSCOPE:
JesiMiranda 6:546e4553cf6d 1158 reportMetaRecord = 0xE306;
JesiMiranda 6:546e4553cf6d 1159 break;
JesiMiranda 6:546e4553cf6d 1160 case MAG_FIELD:
JesiMiranda 6:546e4553cf6d 1161 reportMetaRecord = 0xE309;
JesiMiranda 6:546e4553cf6d 1162 break;
JesiMiranda 6:546e4553cf6d 1163 case MAG_FIELD_UNCALIBRATED:
JesiMiranda 6:546e4553cf6d 1164 reportMetaRecord = 0xE30A;
JesiMiranda 6:546e4553cf6d 1165 break;
JesiMiranda 6:546e4553cf6d 1166 case ROTATION:
JesiMiranda 6:546e4553cf6d 1167 reportMetaRecord = 0xE30B;
JesiMiranda 6:546e4553cf6d 1168 break;
JesiMiranda 6:546e4553cf6d 1169 case GEOMAGNETIC_ROTATION:
JesiMiranda 6:546e4553cf6d 1170 reportMetaRecord = 0xE30D;
JesiMiranda 6:546e4553cf6d 1171 break;
JesiMiranda 6:546e4553cf6d 1172 case GAME_ROTATION:
JesiMiranda 6:546e4553cf6d 1173 reportMetaRecord = 0xE30C;
JesiMiranda 6:546e4553cf6d 1174 break;
JesiMiranda 6:546e4553cf6d 1175 case TAP_DETECTOR:
JesiMiranda 6:546e4553cf6d 1176 reportMetaRecord = 0xE313;
JesiMiranda 6:546e4553cf6d 1177 break;
JesiMiranda 6:546e4553cf6d 1178 case STABILITY_CLASSIFIER:
JesiMiranda 6:546e4553cf6d 1179 reportMetaRecord = 0xE317;
JesiMiranda 6:546e4553cf6d 1180 break;
JesiMiranda 6:546e4553cf6d 1181 case STEP_DETECTOR:
JesiMiranda 6:546e4553cf6d 1182 reportMetaRecord = 0xE314;
JesiMiranda 6:546e4553cf6d 1183 break;
JesiMiranda 6:546e4553cf6d 1184 case STEP_COUNTER:
JesiMiranda 6:546e4553cf6d 1185 reportMetaRecord = 0xE315;
JesiMiranda 6:546e4553cf6d 1186 break;
JesiMiranda 6:546e4553cf6d 1187 case SIGNIFICANT_MOTION:
JesiMiranda 6:546e4553cf6d 1188 reportMetaRecord = 0xE316;
JesiMiranda 6:546e4553cf6d 1189 break;
JesiMiranda 6:546e4553cf6d 1190 case SHAKE_DETECTOR:
JesiMiranda 6:546e4553cf6d 1191 reportMetaRecord = 0xE318;
JesiMiranda 6:546e4553cf6d 1192 break;
JesiMiranda 6:546e4553cf6d 1193 }
Jamie Smith 1:aac28ffd63ed 1194
JesiMiranda 6:546e4553cf6d 1195 // if we already have that data stored, everything's OK
JesiMiranda 6:546e4553cf6d 1196 if(bufferMetadataRecord == reportMetaRecord) {
JesiMiranda 6:546e4553cf6d 1197 return true;
JesiMiranda 6:546e4553cf6d 1198 }
Jamie Smith 1:aac28ffd63ed 1199
JesiMiranda 6:546e4553cf6d 1200 // now, load the metadata into the buffer
JesiMiranda 6:546e4553cf6d 1201 if(!readFRSRecord(reportMetaRecord, metadataRecord, METADATA_BUFFER_LEN)) {
JesiMiranda 6:546e4553cf6d 1202 // clear this so future calls won't try to use the cached version
JesiMiranda 6:546e4553cf6d 1203 bufferMetadataRecord = 0;
Jamie Smith 1:aac28ffd63ed 1204
JesiMiranda 6:546e4553cf6d 1205 return false;
JesiMiranda 6:546e4553cf6d 1206 }
Jamie Smith 1:aac28ffd63ed 1207
JesiMiranda 6:546e4553cf6d 1208 bufferMetadataRecord = reportMetaRecord;
Jamie Smith 1:aac28ffd63ed 1209
JesiMiranda 6:546e4553cf6d 1210 return true;
Jamie Smith 1:aac28ffd63ed 1211 }