Sheila Pham / BNOWrapper

start the wrapper burrito

BNO080.cpp@5:7e2cd0f351b2, 2019-07-22 (annotated)

Committer:
JesiMiranda
Date:
Mon Jul 22 18:54:25 2019 +0000
Revision:
5:7e2cd0f351b2
Parent:
3:197ad972fb7c
Child:
6:546e4553cf6d
none

Who changed what in which revision?

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