BNO080.cpp

00001 //
00002 // USC RPL BNO080 driver.
00003 //
00004 
00005 /*
00006  * Overview of BNO080 Communications
00007  * ===============================================
00008  *
00009  * Hilcrest has developed a protocol called SHTP (Sensor Hub Transport Protocol) for binary communications with
00010  * the BNO080 and the other IMUs it sells.  Over this protocol, SH-2 (Sensor Hub 2) messages are sent to configure
00011  * the chip and read data back.
00012  *
00013  * SHTP messages are divided at two hierarchical levels: first the channel, then the report ID.  Each category
00014  * of messages (system commands, sensor data reports, etc.) has its own channel, and the individual messages
00015  * in each channel are identified by their report id, which is the first byte of the message payload (note that the
00016  * datasheets don't *always* call the first byte the report ID, but that byte does identify the report, so I'm going
00017  * with it).
00018  *
00019  * ===============================================
00020  *
00021  * Information about the BNO080 is split into three datasheets.  Here's the download links and what they cover:
00022  *
00023  * - the BNO080 datasheet: http://www.hillcrestlabs.com/download/5a05f340566d07c196001ec1
00024  * -- Chip pinouts
00025  * -- Example circuits
00026  * -- Physical specifications
00027  * -- Supported reports and configuration settings (at a high level)
00028  * -- List of packets on the SHTP executable channel
00029  *
00030  * - the SHTP protocol: http://www.hillcrestlabs.com/download/59de8f99cd829e94dc0029d7
00031  * -- SHTP transmit and receive protcols (for SPI, I2C, and UART)
00032  * -- SHTP binary format
00033  * -- packet types on the SHTP command channel
00034  *
00035  * - the SH-2 reference: http://www.hillcrestlabs.com/download/59de8f398934bf6faa00293f
00036  * -- list of packets and their formats for all channels other than command and executable
00037  * -- list of FRS (Flash Record System) entries and their formats
00038  *
00039  * ===============================================
00040  *
00041  * Overview of SHTP channels:
00042  *
00043  * 0 -> Command
00044  * -- Used for protocol-global packets, currently only the advertisement packet (which lists all the channels) and error reports
00045  *
00046  * 1 -> Executable
00047  * -- Used for things that control the software on the chip: commands to reset and sleep
00048  * -- Also used by the chip to report when it's done booting up
00049  *
00050  * 2 -> Control
00051  * -- Used to send configuration commands to the IMU and for it to send back responses.
00052  * -- Common report IDs: Command Request (0xF2), Set Feature (0xFD)
00053  *
00054  * 3 -> Sensor Reports
00055  * -- Used for sensors to send back data reports.
00056  * -- AFAIK the only report ID on this channel will be 0xFB (Report Base Timestamp); sensor data is send in a series of structures
00057  *    following an 0xFB
00058  *
00059  * 4 -> Wake Sensor Reports
00060  * -- same as above, but for sensors configured to wake the device
00061  *
00062  * 5 -> Gyro Rotation Vector
00063  * -- a dedicated channel for the Gyro Rotation Vector sensor report
00064  * -- Why does this get its own channel?  I don't know!!!
00065  */
00066 
00067 #include "BNO080.h"
00068 #include "BNO080Constants.h"
00069 
00070 /// Set to 1 to enable debug printouts.  Should be very useful if the chip is giving you trouble.
00071 /// When debugging, it is recommended to use the highest possible serial baudrate so as not to interrupt the timing of operations.
00072 #define BNO_DEBUG 0
00073 
00074 BNO080::BNO080(Serial *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin,
00075                uint8_t i2cAddress, int i2cPortSpeed) :
00076     _debugPort(debugPort),
00077     _i2cPort(user_SDApin, user_SCLpin),
00078     _i2cAddress(i2cAddress),
00079     _int(user_INTPin),
00080     _rst(user_RSTPin, 1),
00081     commandSequenceNumber(0),
00082     stability(UNKNOWN),
00083     stepDetected(false),
00084     stepCount(0),
00085     significantMotionDetected(false),
00086     shakeDetected(false),
00087     xAxisShake(false),
00088     yAxisShake(false),
00089     zAxisShake(false)
00090 {
00091     // zero sequence numbers
00092     memset(sequenceNumber, 0, sizeof(sequenceNumber));
00093 
00094     //Get user settings
00095     _i2cPortSpeed = i2cPortSpeed;
00096     if(_i2cPortSpeed > 4000000) {
00097         _i2cPortSpeed = 4000000; //BNO080 max is 400Khz
00098     }
00099     _i2cPort.frequency(_i2cPortSpeed);
00100 
00101 }
00102 
00103 bool BNO080::begin()
00104 {
00105     //Configure the BNO080 for SPI communication
00106 
00107     _rst = 0; // Reset BNO080
00108     wait(.002f); // Min length not specified in datasheet?
00109     _rst = 1; // Bring out of reset
00110 
00111     // wait for a falling edge (NOT just a low) on the INT pin to denote startup
00112     Timer timeoutTimer;
00113 
00114     bool highDetected = false;
00115     bool lowDetected = false;
00116 
00117     while(true) {
00118         if(timeoutTimer.read() > BNO080_RESET_TIMEOUT) {
00119             _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n");
00120             return false;
00121         }
00122 
00123         // simple edge detector
00124         if(!highDetected) {
00125             if(_int == 1) {
00126                 highDetected = true;
00127             }
00128         } else if(!lowDetected) {
00129             if(_int == 0) {
00130                 lowDetected = true;
00131             }
00132         } else {
00133             // high and low detected
00134             break;
00135         }
00136     }
00137 
00138     _debugPort->printf("BNO080 detected!\n");
00139 
00140     // At system startup, the hub must send its full advertisement message (see SHTP 5.2 and 5.3) to the
00141     // host. It must not send any other data until this step is complete.
00142     // We don't actually care what's in it, we're just using it as a signal to indicate that the reset is complete.
00143     receivePacket();
00144 
00145     // now, after startup, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command
00146     waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET);
00147 
00148     // Next, officially tell it to initialize, and wait for a successful Initialize Response
00149     zeroBuffer();
00150     shtpData[3] = 0;
00151     //changed from sendCommand
00152     sendPacket(COMMAND_INITIALIZE, 3);
00153 
00154 
00155     if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE) || shtpData[2] != COMMAND_INITIALIZE || shtpData[5] != 0) {
00156         _debugPort->printf("BNO080 reports initialization failed.\n");
00157         __enable_irq();
00158         return false;
00159     } else {
00160 #if BNO_DEBUG
00161         _debugPort->printf("BNO080 reports initialization successful!\n");
00162 #endif
00163     }
00164 
00165 
00166     // Finally, we want to interrogate the device about its model and version.
00167     zeroBuffer();
00168     shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info
00169     shtpData[1] = 0; //Reserved
00170     sendPacket(CHANNEL_CONTROL, 2);
00171 
00172     waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5);
00173 
00174     if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) {
00175         majorSoftwareVersion = shtpData[2];
00176         minorSoftwareVersion = shtpData[3];
00177         patchSoftwareVersion = (shtpData[13] << 8) | shtpData[12];
00178         partNumber = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | shtpData[4];
00179         buildNumber = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | shtpData[8];
00180 
00181 #if BNO_DEBUG
00182         _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n",
00183                            majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion,
00184                            buildNumber, partNumber);
00185 #endif
00186 
00187     } else {
00188         _debugPort->printf("Bad response from product ID command.\n");
00189         return false;
00190     }
00191 
00192     // successful init
00193     return true;
00194 
00195 }
00196 
00197 void BNO080::tare(bool zOnly)
00198 {
00199     zeroBuffer();
00200 
00201     // from SH-2 section 6.4.4.1
00202     shtpData[3] = 0; // perform tare now
00203 
00204     if(zOnly) {
00205         shtpData[4] = 0b100; // tare Z axis
00206     } else {
00207         shtpData[4] = 0b111; // tare X, Y, and Z axes
00208     }
00209 
00210     shtpData[5] = 0; // reorient all motion outputs
00211 
00212     sendCommand(COMMAND_TARE);
00213 }
00214 
00215 bool BNO080::enableCalibration(bool calibrateAccel, bool calibrateGyro, bool calibrateMag)
00216 {
00217     // send the Configure ME Calibration command
00218     zeroBuffer();
00219 
00220     shtpData[3] = static_cast<uint8_t>(calibrateAccel ? 1 : 0);
00221     shtpData[4] = static_cast<uint8_t>(calibrateGyro ? 1 : 0);
00222     shtpData[5] = static_cast<uint8_t>(calibrateMag ? 1 : 0);
00223 
00224     shtpData[6] = 0; // Configure ME Calibration command
00225 
00226     shtpData[7] = 0; // planar accelerometer calibration always disabled
00227 
00228     sendCommand(COMMAND_ME_CALIBRATE);
00229 
00230     // now, wait for the response
00231     if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE)) {
00232 #if BNO_DEBUG
00233         _debugPort->printf("Timeout waiting for calibration response!\n");
00234 #endif
00235         return false;
00236     }
00237 
00238     if(shtpData[2] != COMMAND_ME_CALIBRATE) {
00239 #if BNO_DEBUG
00240         _debugPort->printf("Received wrong response to calibration command!\n");
00241 #endif
00242         return false;
00243     }
00244 
00245     if(shtpData[5] != 0) {
00246 #if BNO_DEBUG
00247         _debugPort->printf("IMU reports calibrate command failed!\n");
00248 #endif
00249         return false;
00250     }
00251 
00252     // acknowledge checks out!
00253     return true;
00254 }
00255 
00256 bool BNO080::saveCalibration()
00257 {
00258     zeroBuffer();
00259 
00260     // no arguments
00261     sendCommand(COMMAND_SAVE_DCD);
00262 
00263     // now, wait for the response
00264     if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE)) {
00265 #if BNO_DEBUG
00266         _debugPort->printf("Timeout waiting for calibration response!\n");
00267 #endif
00268         return false;
00269     }
00270 
00271     if(shtpData[2] != COMMAND_SAVE_DCD) {
00272 #if BNO_DEBUG
00273         _debugPort->printf("Received wrong response to calibration command!\n");
00274 #endif
00275         return false;
00276     }
00277 
00278     if(shtpData[5] != 0) {
00279 #if BNO_DEBUG
00280         _debugPort->printf("IMU reports calibrate command failed!\n");
00281 #endif
00282         return false;
00283     }
00284 
00285     // acknowledge checks out!
00286     return true;
00287 }
00288 
00289 void BNO080::setSensorOrientation(Quaternion orientation)
00290 {
00291     zeroBuffer();
00292 
00293     _debugPort->printf("y: %f", orientation.y());
00294 
00295     // convert floats to Q
00296     int16_t Q_x = floatToQ(orientation.x(), ORIENTATION_QUAT_Q_POINT);
00297     int16_t Q_y = floatToQ(orientation.y(), ORIENTATION_QUAT_Q_POINT);
00298     int16_t Q_z = floatToQ(orientation.z(), ORIENTATION_QUAT_Q_POINT);
00299     int16_t Q_w = floatToQ(orientation.w(), ORIENTATION_QUAT_Q_POINT);
00300 
00301     _debugPort->printf("Q_y: %hd", Q_y);
00302 
00303     shtpData[3] = 2; // set reorientation
00304 
00305     shtpData[4] = static_cast<uint8_t>(Q_x & 0xFF); //P1 - X component LSB
00306     shtpData[5] = static_cast<uint8_t>(Q_x >> 8); //P2 - X component MSB
00307 
00308     shtpData[6] = static_cast<uint8_t>(Q_y & 0xFF); //P3 - Y component LSB
00309     shtpData[7] = static_cast<uint8_t>(Q_y >> 8); //P4 - Y component MSB
00310 
00311     shtpData[8] = static_cast<uint8_t>(Q_z & 0xFF); //P5 - Z component LSB
00312     shtpData[9] = static_cast<uint8_t>(Q_z >> 8); //P6 - Z component MSB
00313 
00314     shtpData[10] = static_cast<uint8_t>(Q_w & 0xFF); //P7 - W component LSB
00315     shtpData[11] = static_cast<uint8_t>(Q_w >> 8); //P8 - W component MSB
00316 
00317     //Using this shtpData packet, send a command
00318     sendCommand(COMMAND_TARE); // Send tare command
00319 
00320     // NOTE: unlike literally every other command, a sensor orientation command is never acknowledged in any way.
00321 }
00322 
00323 
00324 bool BNO080::updateData()
00325 {
00326     if(_int.read() != 0) {
00327         // no waiting packets
00328         return false;
00329     }
00330 
00331     while(_int.read() == 0) {
00332         if(!receivePacket()) {
00333             // comms error
00334             return false;
00335         }
00336 
00337         processPacket();
00338     }
00339 
00340     // packets were received, so data may have changed
00341     return true;
00342 }
00343 
00344 uint8_t BNO080::getReportStatus(Report report)
00345 {
00346     uint8_t reportNum = static_cast<uint8_t>(report);
00347     if(reportNum > STATUS_ARRAY_LEN) {
00348         return 0;
00349     }
00350 
00351     return reportStatus[reportNum];
00352 }
00353 
00354 const char* BNO080::getReportStatusString(Report report)
00355 {
00356     switch(getReportStatus(report)) {
00357         case 0:
00358             return "Unreliable";
00359         case 1:
00360             return "Accuracy Low";
00361         case 2:
00362             return "Accuracy Medium";
00363         case 3:
00364             return "Accuracy High";
00365         default:
00366             return "Error";
00367     }
00368 }
00369 
00370 bool BNO080::hasNewData(Report report)
00371 {
00372     uint8_t reportNum = static_cast<uint8_t>(report);
00373     if(reportNum > STATUS_ARRAY_LEN) {
00374         return false;
00375     }
00376 
00377     bool newData = reportHasBeenUpdated[reportNum];
00378     reportHasBeenUpdated[reportNum] = false; // clear flag
00379     return newData;
00380 }
00381 
00382 //Sends the packet to enable the rotation vector
00383 void BNO080::enableReport(Report report, uint16_t timeBetweenReports)
00384 {
00385     // check time
00386     float periodSeconds = timeBetweenReports / 1000.0;
00387 
00388     if(periodSeconds < getMinPeriod(report)) {
00389         _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",
00390                            static_cast<uint8_t>(report), periodSeconds, getMinPeriod(report));
00391         return;
00392     }
00393     /*
00394     else if(getMaxPeriod(report) > 0 && periodSeconds > getMaxPeriod(report))
00395     {
00396         _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",
00397                            static_cast<uint8_t>(report), periodSeconds, getMaxPeriod(report));
00398         return;
00399     }
00400     */
00401     setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports);
00402 
00403     // note: we don't wait for ACKs on these packets because they can take quite a while, like half a second, to come in
00404 }
00405 
00406 void BNO080::disableReport(Report report)
00407 {
00408     // set the report's polling period to zero to disable it
00409     setFeatureCommand(static_cast<uint8_t>(report), 0);
00410 }
00411 
00412 uint32_t BNO080::getSerialNumber()
00413 {
00414     uint32_t serNoBuffer;
00415 
00416     if(!readFRSRecord(FRS_RECORDID_SERIAL_NUMBER, &serNoBuffer, 1)) {
00417         return 0;
00418     }
00419 
00420     return serNoBuffer;
00421 }
00422 
00423 float BNO080::getRange(Report report)
00424 {
00425     loadReportMetadata(report);
00426 
00427     return qToFloat_dword(metadataRecord[1], getQ1(report));
00428 }
00429 
00430 
00431 float BNO080::getResolution(Report report)
00432 {
00433     loadReportMetadata(report);
00434 
00435     return qToFloat_dword(metadataRecord[2], getQ1(report));
00436 }
00437 
00438 float BNO080::getPower(Report report)
00439 {
00440     loadReportMetadata(report);
00441 
00442     uint16_t powerQ = static_cast<uint16_t>(metadataRecord[3] & 0xFFFF);
00443 
00444     return qToFloat_dword(powerQ, POWER_Q_POINT);
00445 }
00446 
00447 float BNO080::getMinPeriod(Report report)
00448 {
00449     loadReportMetadata(report);
00450 
00451     return metadataRecord[4] / 1e6f; // convert from microseconds to seconds
00452 }
00453 
00454 float BNO080::getMaxPeriod(Report report)
00455 {
00456     loadReportMetadata(report);
00457 
00458     if(getMetaVersion() == 3) {
00459         // no max period entry in this record format
00460         return -1.0f;
00461     }
00462 
00463     return metadataRecord[9] / 1e6f; // convert from microseconds to seconds
00464 }
00465 
00466 void BNO080::printMetadataSummary(Report report)
00467 {
00468 #if BNO_DEBUG
00469     if(!loadReportMetadata(report)) {
00470         _debugPort->printf("Failed to load report metadata!\n");
00471     }
00472 
00473     _debugPort->printf("======= Metadata for report 0x%02hhx =======\n", static_cast<uint8_t>(report));
00474 
00475     _debugPort->printf("Range: +- %.04f units\n", getRange(report));
00476     _debugPort->printf("Resolution: %.04f units\n", getResolution(report));
00477     _debugPort->printf("Power Used: %.03f mA\n", getPower(report));
00478     _debugPort->printf("Min Period: %.06f s\n", getMinPeriod(report));
00479     _debugPort->printf("Max Period: %.06f s\n\n", getMaxPeriod(report));
00480 
00481 #endif
00482 }
00483 
00484 int16_t BNO080::getQ1(Report report)
00485 {
00486     loadReportMetadata(report);
00487 
00488     return static_cast<int16_t>(metadataRecord[7] & 0xFFFF);
00489 }
00490 
00491 int16_t BNO080::getQ2(Report report)
00492 {
00493     loadReportMetadata(report);
00494 
00495     return static_cast<int16_t>(metadataRecord[7] >> 16);
00496 }
00497 
00498 int16_t BNO080::getQ3(Report report)
00499 {
00500     loadReportMetadata(report);
00501 
00502     return static_cast<int16_t>(metadataRecord[8] >> 16);
00503 }
00504 
00505 void BNO080::processPacket()
00506 {
00507     if(shtpHeader[2] == CHANNEL_CONTROL) {
00508         // currently no command reports are read
00509     } else if(shtpHeader[2] == CHANNEL_EXECUTABLE) {
00510         // currently no executable reports are read
00511     } else if(shtpHeader[2] == CHANNEL_COMMAND) {
00512 
00513     } else if(shtpHeader[2] == CHANNEL_REPORTS || shtpHeader[2] == CHANNEL_WAKE_REPORTS) {
00514         if(shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP) {
00515             // sensor data packet 
00516             //_debugPort->printf("\r\t\t enter pareseSensorDataPacket \r\n");
00517             parseSensorDataPacket();
00518         }
00519     }
00520 }
00521 
00522 // sizes of various sensor data packet elements
00523 #define SIZEOF_BASE_TIMESTAMP 5
00524 #define SIZEOF_TIMESTAMP_REBASE 5
00525 #define SIZEOF_ACCELEROMETER 10
00526 #define SIZEOF_LINEAR_ACCELERATION 10
00527 #define SIZEOF_GYROSCOPE_CALIBRATED 10
00528 #define SIZEOF_MAGNETIC_FIELD_CALIBRATED 10
00529 #define SIZEOF_MAGNETIC_FIELD_UNCALIBRATED 16
00530 #define SIZEOF_ROTATION_VECTOR 14
00531 #define SIZEOF_GAME_ROTATION_VECTOR 12
00532 #define SIZEOF_GEOMAGNETIC_ROTATION_VECTOR 14
00533 #define SIZEOF_TAP_DETECTOR 5
00534 #define SIZEOF_STABILITY_REPORT 6
00535 #define SIZEOF_STEP_DETECTOR 8
00536 #define SIZEOF_STEP_COUNTER 12
00537 #define SIZEOF_SIGNIFICANT_MOTION 6
00538 #define SIZEOF_SHAKE_DETECTOR 6
00539 
00540 void BNO080::parseSensorDataPacket()
00541 {
00542     size_t currReportOffset = 0;
00543 
00544     // every sensor data report first contains a timestamp offset to show how long it has been between when
00545     // the host interrupt was sent and when the packet was transmitted.
00546     // We don't use interrupts and don't care about times, so we can throw this out.
00547     currReportOffset += SIZEOF_BASE_TIMESTAMP;
00548 
00549     while(currReportOffset < packetLength) {
00550         if(currReportOffset >= STORED_PACKET_SIZE) {
00551             _debugPort->printf("Error: sensor report longer than packet buffer!\n");
00552             return;
00553         }
00554 
00555         // lots of sensor reports use 3 16-bit numbers stored in bytes 4 through 9
00556         // we can save some time by parsing those out here.
00557         uint16_t data1 = (uint16_t)shtpData[currReportOffset + 5] << 8 | shtpData[currReportOffset + 4];
00558         uint16_t data2 = (uint16_t)shtpData[currReportOffset + 7] << 8 | shtpData[currReportOffset + 6];
00559         uint16_t data3 = (uint16_t)shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8];
00560 
00561         uint8_t reportNum = shtpData[currReportOffset];
00562 
00563         if(reportNum != SENSOR_REPORTID_TIMESTAMP_REBASE) {
00564             // set status from byte 2
00565             reportStatus[reportNum] = static_cast<uint8_t>(shtpData[currReportOffset + 2] & 0b11);
00566 
00567             // set updated flag
00568             reportHasBeenUpdated[reportNum] = true;
00569         }
00570 
00571         switch(shtpData[currReportOffset]) {
00572             case SENSOR_REPORTID_TIMESTAMP_REBASE:
00573                 currReportOffset += SIZEOF_TIMESTAMP_REBASE;
00574                 break;
00575 
00576             case SENSOR_REPORTID_ACCELEROMETER:
00577 
00578                 totalAcceleration = TVector3 (
00579                                         qToFloat(data1, ACCELEROMETER_Q_POINT),
00580                                         qToFloat(data2, ACCELEROMETER_Q_POINT),
00581                                         qToFloat(data3, ACCELEROMETER_Q_POINT));
00582 
00583                 currReportOffset += SIZEOF_ACCELEROMETER;
00584                 break;
00585 
00586             case SENSOR_REPORTID_LINEAR_ACCELERATION:
00587 
00588                 linearAcceleration = TVector3 (
00589                                          qToFloat(data1, ACCELEROMETER_Q_POINT),
00590                                          qToFloat(data2, ACCELEROMETER_Q_POINT),
00591                                          qToFloat(data3, ACCELEROMETER_Q_POINT));
00592 
00593                 currReportOffset += SIZEOF_LINEAR_ACCELERATION;
00594                 break;
00595 
00596             case SENSOR_REPORTID_GRAVITY:
00597 
00598                 gravityAcceleration = TVector3 (
00599                                           qToFloat(data1, ACCELEROMETER_Q_POINT),
00600                                           qToFloat(data2, ACCELEROMETER_Q_POINT),
00601                                           qToFloat(data3, ACCELEROMETER_Q_POINT));
00602 
00603                 currReportOffset += SIZEOF_LINEAR_ACCELERATION;
00604                 break;
00605 
00606             case SENSOR_REPORTID_GYROSCOPE_CALIBRATED:
00607 
00608                 gyroRotation = TVector3 (
00609                                    qToFloat(data1, GYRO_Q_POINT),
00610                                    qToFloat(data2, GYRO_Q_POINT),
00611                                    qToFloat(data3, GYRO_Q_POINT));
00612 
00613                 currReportOffset += SIZEOF_GYROSCOPE_CALIBRATED;
00614                 break;
00615 
00616             case SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED:
00617 
00618                 magField = TVector3 (
00619                                qToFloat(data1, MAGNETOMETER_Q_POINT),
00620                                qToFloat(data2, MAGNETOMETER_Q_POINT),
00621                                qToFloat(data3, MAGNETOMETER_Q_POINT));
00622 
00623                 currReportOffset += SIZEOF_MAGNETIC_FIELD_CALIBRATED;
00624                 break;
00625 
00626             case SENSOR_REPORTID_MAGNETIC_FIELD_UNCALIBRATED: {
00627                 magFieldUncalibrated = TVector3 (
00628                                            qToFloat(data1, MAGNETOMETER_Q_POINT),
00629                                            qToFloat(data2, MAGNETOMETER_Q_POINT),
00630                                            qToFloat(data3, MAGNETOMETER_Q_POINT));
00631 
00632                 uint16_t ironOffsetXQ = shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
00633                 uint16_t ironOffsetYQ = shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12];
00634                 uint16_t ironOffsetZQ = shtpData[currReportOffset + 15] << 8 | shtpData[currReportOffset + 14];
00635 
00636                 hardIronOffset = TVector3 (
00637                                      qToFloat(ironOffsetXQ, MAGNETOMETER_Q_POINT),
00638                                      qToFloat(ironOffsetYQ, MAGNETOMETER_Q_POINT),
00639                                      qToFloat(ironOffsetZQ, MAGNETOMETER_Q_POINT));
00640 
00641                 currReportOffset += SIZEOF_MAGNETIC_FIELD_UNCALIBRATED;
00642             }
00643             break;
00644 
00645             case SENSOR_REPORTID_ROTATION_VECTOR: {
00646                 uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
00647                 uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12];
00648 
00649                 rotationVector = TVector4(
00650                                      qToFloat(data1, ROTATION_Q_POINT),
00651                                      qToFloat(data2, ROTATION_Q_POINT),
00652                                      qToFloat(data3, ROTATION_Q_POINT),
00653                                      qToFloat(realPartQ, ROTATION_Q_POINT));
00654 
00655                 rotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT);
00656 
00657                 currReportOffset += SIZEOF_ROTATION_VECTOR;
00658             }
00659             break;
00660 
00661             case SENSOR_REPORTID_GAME_ROTATION_VECTOR: {
00662                 uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
00663 
00664                 gameRotationVector = TVector4(
00665                                          qToFloat(data1, ROTATION_Q_POINT),
00666                                          qToFloat(data2, ROTATION_Q_POINT),
00667                                          qToFloat(data3, ROTATION_Q_POINT),
00668                                          qToFloat(realPartQ, ROTATION_Q_POINT));
00669 
00670                 currReportOffset += SIZEOF_GAME_ROTATION_VECTOR;
00671             }
00672             break;
00673 
00674             case SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR: {
00675                 uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10];
00676                 uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12];
00677 
00678                 geomagneticRotationVector = TVector4(
00679                                                 qToFloat(data1, ROTATION_Q_POINT),
00680                                                 qToFloat(data2, ROTATION_Q_POINT),
00681                                                 qToFloat(data3, ROTATION_Q_POINT),
00682                                                 qToFloat(realPartQ, ROTATION_Q_POINT));
00683 
00684                 geomagneticRotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT);
00685 
00686                 currReportOffset += SIZEOF_GEOMAGNETIC_ROTATION_VECTOR;
00687             }
00688             break;
00689 
00690             case SENSOR_REPORTID_TAP_DETECTOR:
00691 
00692                 // since we got the report, a tap was detected
00693                 tapDetected = true;
00694 
00695                 doubleTap = (shtpData[currReportOffset + 4] & (1 << 6)) != 0;
00696 
00697                 currReportOffset += SIZEOF_TAP_DETECTOR;
00698                 break;
00699 
00700             case SENSOR_REPORTID_STABILITY_CLASSIFIER: {
00701                 uint8_t classificationNumber = shtpData[currReportOffset + 4];
00702 
00703                 if(classificationNumber > 4) {
00704                     classificationNumber = 0;
00705                 }
00706 
00707                 stability = static_cast<Stability>(classificationNumber);
00708 
00709                 currReportOffset += SIZEOF_STABILITY_REPORT;
00710             }
00711             break;
00712 
00713             case SENSOR_REPORTID_STEP_DETECTOR:
00714 
00715                 // the fact that we got the report means that a step was detected
00716                 stepDetected = true;
00717 
00718                 currReportOffset += SIZEOF_STEP_DETECTOR;
00719 
00720                 break;
00721 
00722             case SENSOR_REPORTID_STEP_COUNTER:
00723 
00724                 stepCount = shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8];
00725 
00726                 currReportOffset += SIZEOF_STEP_COUNTER;
00727 
00728                 break;
00729 
00730             case SENSOR_REPORTID_SIGNIFICANT_MOTION:
00731 
00732                 // the fact that we got the report means that significant motion was detected
00733                 significantMotionDetected = true;
00734 
00735                 currReportOffset += SIZEOF_SIGNIFICANT_MOTION;
00736 
00737             case SENSOR_REPORTID_SHAKE_DETECTOR:
00738 
00739                 shakeDetected = true;
00740 
00741                 xAxisShake = (shtpData[currReportOffset + 4] & 1) != 0;
00742                 yAxisShake = (shtpData[currReportOffset + 4] & (1 << 1)) != 0;
00743                 zAxisShake = (shtpData[currReportOffset + 4] & (1 << 2)) != 0;
00744 
00745                 currReportOffset += SIZEOF_SHAKE_DETECTOR;
00746 
00747             default:
00748                 _debugPort->printf("Error: unrecognized report ID in sensor report: %hhx.  Byte %u, length %hu\n", shtpData[currReportOffset], currReportOffset, packetLength);
00749                 return;
00750         }
00751     }
00752 
00753 }
00754 
00755 bool BNO080::waitForPacket(int channel, uint8_t reportID, float timeout)
00756 {
00757     Timer timeoutTimer;
00758     timeoutTimer.start();
00759 
00760     while(timeoutTimer.read() <= timeout) {
00761         if(_int.read() == 0) {
00762             if(!receivePacket(timeout)) {
00763                 return false;
00764             }
00765 
00766             if(channel == shtpHeader[2] && reportID == shtpData[0]) {
00767                 // found correct packet!
00768                 _debugPort->printf("\r\t found the correct packet \r\n");
00769                 return true;
00770             } else {
00771                 // other data packet, send to proper channels
00772                 _debugPort->printf("\r\t other data packets, sending to proper channel\r\n");
00773                 processPacket();
00774                 //return false;
00775             }
00776         }
00777     }
00778 
00779     _debugPort->printf("Packet wait timeout.\n");
00780     return false;
00781 }
00782 
00783 //Given a register value and a Q point, convert to float
00784 //See https://en.wikipedia.org/wiki/Q_(number_format)
00785 float BNO080::qToFloat(int16_t fixedPointValue, uint8_t qPoint)
00786 {
00787     float qFloat = fixedPointValue;
00788     qFloat *= pow(2.0, qPoint * -1.0);
00789     return (qFloat);
00790 }
00791 
00792 float BNO080::qToFloat_dword(uint32_t fixedPointValue, int16_t qPoint)
00793 {
00794     float qFloat = fixedPointValue;
00795     qFloat *= pow(2.0, qPoint * -1.0);
00796     return (qFloat);
00797 }
00798 
00799 //Given a floating point value and a Q point, convert to Q
00800 //See https://en.wikipedia.org/wiki/Q_(number_format)
00801 int16_t BNO080::floatToQ(float qFloat, uint8_t qPoint)
00802 {
00803     int16_t qVal = static_cast<int16_t>(qFloat * pow(2.0, qPoint));
00804     return qVal;
00805 }
00806 
00807 //Tell the sensor to do a command
00808 //See 6.3.8 page 41, Command request
00809 //The caller is expected to set P0 through P8 prior to calling
00810 void BNO080::sendCommand(uint8_t command)
00811 {
00812     shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request
00813     shtpData[1] = commandSequenceNumber++; //Increments automatically each function call
00814     shtpData[2] = command; //Command
00815 
00816     //Caller must set these
00817     /*shtpData[3] = 0; //P0
00818         shtpData[4] = 0; //P1
00819         shtpData[5] = 0; //P2
00820         shtpData[6] = 0;
00821         shtpData[7] = 0;
00822         shtpData[8] = 0;
00823         shtpData[9] = 0;
00824         shtpData[10] = 0;
00825         shtpData[11] = 0;*/
00826 
00827     //Transmit packet on channel 2, 12 bytes
00828     sendPacket(CHANNEL_CONTROL, 12);
00829 }
00830 
00831 //Given a sensor's report ID, this tells the BNO080 to begin reporting the values
00832 //Also sets the specific config word. Useful for personal activity classifier
00833 void BNO080::setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig)
00834 {
00835     uint32_t microsBetweenReports = static_cast<uint32_t>(timeBetweenReports * 1000);
00836 
00837     const uint32_t batchMicros = 0;
00838 
00839     shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55
00840     shtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
00841     shtpData[2] = 0; //Feature flags
00842     shtpData[3] = 0; //Change sensitivity (LSB)
00843     shtpData[4] = 0; //Change sensitivity (MSB)
00844     shtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms
00845     shtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval
00846     shtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval
00847     shtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
00848     shtpData[9] = (batchMicros >> 0) & 0xFF;  //Batch Interval (LSB)
00849     shtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval
00850     shtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval
00851     shtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB)
00852     shtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB)
00853     shtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config
00854     shtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config
00855     shtpData[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB)
00856 
00857     //Transmit packet on channel 2, 17 bytes
00858     sendPacket(CHANNEL_CONTROL, 17);
00859 }
00860 
00861 bool BNO080::readFRSRecord(uint16_t recordID, uint32_t* readBuffer, uint16_t readLength)
00862 {
00863     // send initial read request
00864     zeroBuffer();
00865 
00866     shtpData[0] = SHTP_REPORT_FRS_READ_REQUEST;
00867     // read offset of 0 -> start at the start of the record
00868     shtpData[2] = 0;
00869     shtpData[3] = 0;
00870     // record ID
00871     shtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
00872     shtpData[5] = static_cast<uint8_t>(recordID >> 8);
00873     // block size
00874     shtpData[6] = static_cast<uint8_t>(readLength & 0xFF);
00875     shtpData[7] = static_cast<uint8_t>(readLength >> 8);
00876 
00877     sendPacket(CHANNEL_CONTROL, 8);
00878 
00879     // now, read back the responses
00880     size_t readOffset = 0;
00881     while(readOffset < readLength) {
00882         if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE)) {
00883 #if BNO_DEBUG
00884             _debugPort->printf("Error: did not receive FRS read response after sending read request!\n");
00885 #endif
00886             return false;
00887         }
00888 
00889         uint8_t status = static_cast<uint8_t>(shtpData[1] & 0b1111);
00890         uint8_t dataLength = shtpData[1] >> 4;
00891 
00892         // check status
00893         if(status == 1) {
00894 #if BNO_DEBUG
00895             _debugPort->printf("Error: FRS reports invalid record ID!\n");
00896 #endif
00897             return false;
00898         } else if(status == 2) {
00899 #if BNO_DEBUG
00900             _debugPort->printf("Error: FRS is busy!\n");
00901 #endif
00902             return false;
00903         } else if(status == 4) {
00904 #if BNO_DEBUG
00905             _debugPort->printf("Error: FRS reports offset is out of range!\n");
00906 #endif
00907             return false;
00908         } else if(status == 5) {
00909 #if BNO_DEBUG
00910             _debugPort->printf("Error: FRS reports record %hx is empty!\n", recordID);
00911 #endif
00912             return false;
00913         } else if(status == 8) {
00914 #if BNO_DEBUG
00915             _debugPort->printf("Error: FRS reports flash memory device unavailable!\n");
00916 #endif
00917             return false;
00918         }
00919 
00920         // check data length
00921         if(dataLength == 0) {
00922 #if BNO_DEBUG
00923             _debugPort->printf("Error: Received FRS packet with 0 data length!\n");
00924 #endif
00925             return false;
00926         } else if(dataLength == 1) {
00927             if(readOffset + 1 != readLength) {
00928 #if BNO_DEBUG
00929                 _debugPort->printf("Error: Received 1 length packet but more than 1 byte remains to be be read!\n");
00930 #endif
00931                 return false;
00932             }
00933         }
00934 
00935         // now, _finally_, read the dang words
00936         readBuffer[readOffset] = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | (shtpData[4]);
00937 
00938         // check if we only wanted the first word
00939         ++readOffset;
00940         if(readOffset == readLength) {
00941             break;
00942         }
00943 
00944         readBuffer[readOffset] = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | (shtpData[8]);
00945         readOffset++;
00946     }
00947 
00948     // read successful
00949     return true;
00950 
00951 }
00952 
00953 //Given the data packet, send the header then the data
00954 //Returns false if sensor does not ACK
00955 bool BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength)
00956 {
00957     // start the transaction and contact the IMU
00958     _i2cPort.start();
00959 
00960     // to indicate an i2c read, shift the 7 bit address up 1 bit and keep bit 0 as a 0
00961     int writeResult = _i2cPort.write(_i2cAddress << 1);
00962 
00963     if(writeResult != 1) {
00964         _debugPort->printf("BNO I2C write failed!\n");
00965         _i2cPort.stop();
00966         return false;
00967     }
00968 
00969 
00970     uint16_t totalLength = dataLength + 4; //Add four bytes for the header
00971     packetLength = dataLength;
00972 
00973 #if BNO_DEBUG
00974     shtpHeader[0] = totalLength & 0xFF;
00975     shtpHeader[1] = totalLength >> 8;
00976     shtpHeader[2] = channelNumber;
00977     shtpHeader[3] = sequenceNumber[channelNumber];
00978 
00979     _debugPort->printf("Transmitting packet: ----------------\n");
00980     printPacket();
00981 #endif
00982     
00983     //Send the 4 byte packet header
00984     _i2cPort.write(totalLength & 0xFF); //Packet length LSB
00985     _i2cPort.write(totalLength >> 8); //Packet length MSB
00986     _i2cPort.write(channelNumber); //Channel number
00987     _i2cPort.write(sequenceNumber[channelNumber]++); //Send the sequence number, increments with each packet sent, different counter for each channel
00988     
00989     //Send the user's data packet
00990     for (uint8_t i = 0 ; i < dataLength ; i++) {
00991         _i2cPort.write(shtpData[i]);
00992     }
00993     _i2cPort.stop();
00994 
00995     return (true);
00996 }
00997 
00998 //Check to see if there is any new data available
00999 //Read the contents of the incoming packet into the shtpData array
01000 bool BNO080::receivePacket(float timeout)
01001 {
01002     Timer waitStartTime;
01003     waitStartTime.start();
01004 
01005     while(_int.read() != 0) {
01006         if(waitStartTime.read() > timeout) {
01007             _debugPort->printf("BNO I2C wait timeout\n");
01008             return false;
01009         }
01010     }
01011 
01012     // start the transaction and contact the IMU
01013     _i2cPort.start();
01014 
01015     // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1
01016     int writeResult = _i2cPort.write((_i2cAddress << 1) | 0x1);
01017 
01018     if(writeResult != 1) {
01019         _debugPort->printf("BNO I2C read failed!\n");
01020         return false;
01021     }
01022 
01023     //Get the first four bytes, aka the packet header
01024     uint8_t packetLSB = static_cast<uint8_t>(_i2cPort.read(true));
01025     uint8_t packetMSB = static_cast<uint8_t>(_i2cPort.read(true));
01026     uint8_t channelNumber = static_cast<uint8_t>(_i2cPort.read(true));
01027     uint8_t sequenceNum = static_cast<uint8_t>(_i2cPort.read(true)); //Not sure if we need to store this or not
01028 
01029     //Store the header info
01030     shtpHeader[0] = packetLSB;
01031     shtpHeader[1] = packetMSB;
01032     shtpHeader[2] = channelNumber;
01033     shtpHeader[3] = sequenceNum;
01034 
01035     if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF) {
01036         // invalid according to BNO080 datasheet section 1.4.1
01037 
01038 #if BNO_DEBUG
01039         _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n");
01040 #endif
01041         _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n");
01042         return false;
01043     }
01044 
01045     //Calculate the number of data bytes in this packet
01046     packetLength = (static_cast<uint16_t>(packetMSB) << 8 | packetLSB);
01047 
01048     // Clear the MSbit.
01049     // 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)
01050     // but we don't actually care about any of the advertisement packets
01051     // that use this, so we can just cut off the rest of the packet by releasing chip select.
01052     packetLength &= ~(1 << 15);
01053 
01054     if (packetLength == 0) {
01055         // Packet is empty
01056         return (false); //All done
01057     }
01058 
01059     packetLength -= 4; //Remove the header bytes from the data count
01060 
01061     //Read incoming data into the shtpData array
01062     for (uint16_t dataSpot = 0 ; dataSpot < packetLength ; dataSpot++) {
01063         bool sendACK = dataSpot < packetLength - 1;
01064 
01065         // per the datasheet, 0xFF is used as filler for the receiver to transmit back
01066         uint8_t incoming = static_cast<uint8_t>(_i2cPort.read(sendACK));
01067         if (dataSpot < STORED_PACKET_SIZE) //BNO080 can respond with upto 270 bytes, avoid overflow
01068             shtpData[dataSpot] = incoming; //Store data into the shtpData array
01069     }
01070 
01071     _i2cPort.stop();
01072 
01073 #if BNO_DEBUG
01074     _debugPort->printf("Recieved packet: ----------------\n");
01075     printPacket(); // note: add 4 for the header length
01076 #endif
01077     //_debugPort->printf("\r\t\t\t We're done!\r\n");
01078     return (true); //We're done!
01079 }
01080 
01081 //Pretty prints the contents of the current shtp header and data packets
01082 void BNO080::printPacket()
01083 {
01084 #if BNO_DEBUG
01085     //Print the four byte header
01086     _debugPort->printf("Header:");
01087     for (uint8_t x = 0 ; x < 4 ; x++) {
01088         _debugPort->printf(" ");
01089         if (shtpHeader[x] < 0x10) _debugPort->printf("0");
01090         _debugPort->printf("%hhx", shtpHeader[x]);
01091     }
01092 
01093     uint16_t printLength = packetLength;
01094     if (printLength > 40) printLength = 40; //Artificial limit. We don't want the phone book.
01095 
01096     _debugPort->printf(" Body:");
01097     for (uint16_t x = 0 ; x < printLength ; x++) {
01098         _debugPort->printf(" ");
01099         if (shtpData[x] < 0x10) _debugPort->printf("0");
01100         _debugPort->printf("%hhx", shtpData[x]);
01101     }
01102 
01103     _debugPort->printf(", Length:");
01104     _debugPort->printf("%hhu", packetLength + SHTP_HEADER_SIZE);
01105 
01106     if(shtpHeader[1] >> 7) {
01107         _debugPort->printf("[C]");
01108     }
01109 
01110     _debugPort->printf(", SeqNum: %hhu", shtpHeader[3]);
01111 
01112     _debugPort->printf(", Channel:");
01113     if (shtpHeader[2] == 0) _debugPort->printf("Command");
01114     else if (shtpHeader[2] == 1) _debugPort->printf("Executable");
01115     else if (shtpHeader[2] == 2) _debugPort->printf("Control");
01116     else if (shtpHeader[2] == 3) _debugPort->printf("Sensor-report");
01117     else if (shtpHeader[2] == 4) _debugPort->printf("Wake-report");
01118     else if (shtpHeader[2] == 5) _debugPort->printf("Gyro-vector");
01119     else _debugPort->printf("%hhu", shtpHeader[2]);
01120 
01121     _debugPort->printf("\n");
01122 #endif
01123 }
01124 
01125 
01126 void BNO080::zeroBuffer()
01127 {
01128     memset(shtpHeader, 0, SHTP_HEADER_SIZE);
01129     memset(shtpData, 0, STORED_PACKET_SIZE);
01130     packetLength = 0;
01131 }
01132 
01133 bool BNO080::loadReportMetadata(BNO080::Report report)
01134 {
01135     uint16_t reportMetaRecord;
01136 
01137     // first, convert the report into the correct FRS record ID for that report's metadata
01138     // data from SH-2 section 5.1
01139     switch(report) {
01140         case TOTAL_ACCELERATION:
01141             reportMetaRecord = 0xE301;
01142             break;
01143         case LINEAR_ACCELERATION:
01144             reportMetaRecord = 0xE303;
01145             break;
01146         case GRAVITY_ACCELERATION:
01147             reportMetaRecord = 0xE304;
01148             break;
01149         case GYROSCOPE:
01150             reportMetaRecord = 0xE306;
01151             break;
01152         case MAG_FIELD:
01153             reportMetaRecord = 0xE309;
01154             break;
01155         case MAG_FIELD_UNCALIBRATED:
01156             reportMetaRecord = 0xE30A;
01157             break;
01158         case ROTATION:
01159             reportMetaRecord = 0xE30B;
01160             break;
01161         case GEOMAGNETIC_ROTATION:
01162             reportMetaRecord = 0xE30D;
01163             break;
01164         case GAME_ROTATION:
01165             reportMetaRecord = 0xE30C;
01166             break;
01167         case TAP_DETECTOR:
01168             reportMetaRecord = 0xE313;
01169             break;
01170         case STABILITY_CLASSIFIER:
01171             reportMetaRecord = 0xE317;
01172             break;
01173         case STEP_DETECTOR:
01174             reportMetaRecord = 0xE314;
01175             break;
01176         case STEP_COUNTER:
01177             reportMetaRecord = 0xE315;
01178             break;
01179         case SIGNIFICANT_MOTION:
01180             reportMetaRecord = 0xE316;
01181             break;
01182         case SHAKE_DETECTOR:
01183             reportMetaRecord = 0xE318;
01184             break;
01185     }
01186 
01187     // if we already have that data stored, everything's OK
01188     if(bufferMetadataRecord == reportMetaRecord) {
01189         return true;
01190     }
01191 
01192     // now, load the metadata into the buffer
01193     if(!readFRSRecord(reportMetaRecord, metadataRecord, METADATA_BUFFER_LEN)) {
01194         // clear this so future calls won't try to use the cached version
01195         bufferMetadataRecord = 0;
01196 
01197         return false;
01198     }
01199 
01200     bufferMetadataRecord = reportMetaRecord;
01201 
01202     return true;
01203 }