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 sent 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 #include <cinttypes>
00070 #include <algorithm>
00071 
00072 /// Set to 1 to enable debug printouts.  Should be very useful if the chip is giving you trouble.
00073 /// When debugging, it is recommended to use the highest possible serial baudrate so as not to interrupt the timing of operations.
00074 #define BNO_DEBUG 0
00075 
00076 BNO080Base::BNO080Base(Stream *debugPort, PinName user_INTPin, PinName user_RSTPin) :
00077         _debugPort(debugPort),
00078         _int(user_INTPin),
00079         _rst(user_RSTPin, 1),
00080         commandSequenceNumber(0),
00081         stability(UNKNOWN),
00082         stepDetected(false),
00083         stepCount(0),
00084         significantMotionDetected(false),
00085         shakeDetected(false),
00086         xAxisShake(false),
00087         yAxisShake(false),
00088         zAxisShake(false)
00089 {
00090     // zero sequence numbers
00091     memset(sequenceNumber, 0, sizeof(sequenceNumber));
00092 }
00093 
00094 bool BNO080Base::begin()
00095 {
00096     //Configure the BNO080
00097 
00098     _rst = 0; // Reset BNO080
00099     ThisThread::sleep_for(1ms); // Min length not specified in datasheet?
00100     _rst = 1; // Bring out of reset
00101 
00102     // wait for a falling edge (NOT just a low) on the INT pin to denote startup
00103     Timer timeoutTimer;
00104     timeoutTimer.start();                  
00105 
00106     bool highDetected = false;
00107     bool lowDetected = false;
00108 
00109     while(true)
00110     {
00111         if(timeoutTimer.elapsed_time() > BNO080_RESET_TIMEOUT)
00112         {
00113             _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n");
00114             return false;
00115         }
00116 
00117         // simple edge detector
00118         if(!highDetected)
00119         {
00120             if(_int == 1)
00121             {
00122                 highDetected = true;
00123             }
00124         }
00125         else if(!lowDetected)
00126         {
00127             if(_int == 0)
00128             {
00129                 lowDetected = true;
00130             }
00131         }
00132         else
00133         {
00134             // high and low detected
00135             break;
00136         }
00137     }
00138 
00139 #if BNO_DEBUG
00140     _debugPort->printf("BNO080 detected!\r\n");
00141 #endif
00142 
00143     // At system startup, the hub must send its full advertisement message (see SHTP 5.2 and 5.3) to the
00144     // host. It must not send any other data until this step is complete.
00145     // We don't actually care what's in it, we're just using it as a signal to indicate that the reset is complete.
00146     receivePacket();
00147 
00148     // now, after startup, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command
00149     if(!waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET))
00150     {
00151         _debugPort->printf("No initialization report from BNO080.\n");
00152         return false;
00153     }
00154     else
00155     {
00156 #if BNO_DEBUG
00157         _debugPort->printf("BNO080 reports initialization successful!\n");
00158 #endif
00159     }
00160 
00161     // Finally, we want to interrogate the device about its model and version.
00162     clearSendBuffer();
00163     txShtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info
00164     txShtpData[1] = 0; //Reserved
00165     sendPacket(CHANNEL_CONTROL, 2);
00166 
00167     waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5ms);
00168 
00169     if (rxShtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE)
00170     {
00171         majorSoftwareVersion = rxShtpData[2];
00172         minorSoftwareVersion = rxShtpData[3];
00173         patchSoftwareVersion = (rxShtpData[13] << 8) | rxShtpData[12];
00174         partNumber = (rxShtpData[7] << 24) | (rxShtpData[6] << 16) | (rxShtpData[5] << 8) | rxShtpData[4];
00175         buildNumber = (rxShtpData[11] << 24) | (rxShtpData[10] << 16) | (rxShtpData[9] << 8) | rxShtpData[8];
00176 
00177 #if BNO_DEBUG
00178         _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n",
00179                            majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion,
00180                            buildNumber, partNumber);
00181 #endif
00182 
00183     }
00184     else
00185     {
00186         _debugPort->printf("Bad response from product ID command.\n");
00187         return false;
00188     }
00189 
00190     // successful init
00191     return true;
00192 
00193 }
00194 
00195 void BNO080Base::tare(bool zOnly)
00196 {
00197     clearSendBuffer();
00198 
00199     // from SH-2 section 6.4.4.1
00200     txShtpData[3] = 0; // perform tare now
00201 
00202     if(zOnly)
00203     {
00204         txShtpData[4] = 0b100; // tare Z axis
00205     }
00206     else
00207     {
00208         txShtpData[4] = 0b111; // tare X, Y, and Z axes
00209     }
00210 
00211     txShtpData[5] = 0; // reorient all motion outputs
00212 
00213     sendCommand(COMMAND_TARE);
00214 }
00215 
00216 bool BNO080Base::enableCalibration(bool calibrateAccel, bool calibrateGyro, bool calibrateMag)
00217 {
00218     // send the Configure ME Calibration command
00219     clearSendBuffer();
00220 
00221     txShtpData[3] = static_cast<uint8_t>(calibrateAccel ? 1 : 0);
00222     txShtpData[4] = static_cast<uint8_t>(calibrateGyro ? 1 : 0);
00223     txShtpData[5] = static_cast<uint8_t>(calibrateMag ? 1 : 0);
00224 
00225     txShtpData[6] = 0; // Configure ME Calibration command
00226 
00227     txShtpData[7] = 0; // planar accelerometer calibration always disabled
00228 
00229     sendCommand(COMMAND_ME_CALIBRATE);
00230 
00231     // now, wait for the response
00232     if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE))
00233     {
00234 #if BNO_DEBUG
00235         _debugPort->printf("Timeout waiting for calibration response!\n");
00236 #endif
00237         return false;
00238     }
00239 
00240     if(rxShtpData[2] != COMMAND_ME_CALIBRATE)
00241     {
00242 #if BNO_DEBUG
00243         _debugPort->printf("Received wrong response to calibration command!\n");
00244 #endif
00245         return false;
00246     }
00247 
00248     if(rxShtpData[5] != 0)
00249     {
00250 #if BNO_DEBUG
00251         _debugPort->printf("IMU reports calibrate command failed!\n");
00252 #endif
00253         return false;
00254     }
00255 
00256     // acknowledge checks out!
00257     return true;
00258 }
00259 
00260 bool BNO080Base::saveCalibration()
00261 {
00262     clearSendBuffer();
00263 
00264     // no arguments
00265     sendCommand(COMMAND_SAVE_DCD);
00266 
00267     // now, wait for the response
00268     if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE))
00269     {
00270 #if BNO_DEBUG
00271         _debugPort->printf("Timeout waiting for calibration response!\n");
00272 #endif
00273         return false;
00274     }
00275 
00276     if(rxShtpData[2] != COMMAND_SAVE_DCD)
00277     {
00278 #if BNO_DEBUG
00279         _debugPort->printf("Received wrong response to calibration command!\n");
00280 #endif
00281         return false;
00282     }
00283 
00284     if(rxShtpData[5] != 0)
00285     {
00286 #if BNO_DEBUG
00287         _debugPort->printf("IMU reports calibrate command failed!\n");
00288 #endif
00289         return false;
00290     }
00291 
00292     // acknowledge checks out!
00293     return true;
00294 }
00295 
00296 void BNO080Base::setSensorOrientation(Quaternion orientation)
00297 {
00298     clearSendBuffer();
00299 
00300     // convert floats to Q
00301     int16_t Q_x = floatToQ(orientation.x(), ORIENTATION_QUAT_Q_POINT);
00302     int16_t Q_y = floatToQ(orientation.y(), ORIENTATION_QUAT_Q_POINT);
00303     int16_t Q_z = floatToQ(orientation.z(), ORIENTATION_QUAT_Q_POINT);
00304     int16_t Q_w = floatToQ(orientation.w(), ORIENTATION_QUAT_Q_POINT);
00305 
00306     txShtpData[3] = 2; // set reorientation
00307 
00308     txShtpData[4] = static_cast<uint8_t>(Q_x & 0xFF); //P1 - X component LSB
00309     txShtpData[5] = static_cast<uint8_t>(Q_x >> 8); //P2 - X component MSB
00310 
00311     txShtpData[6] = static_cast<uint8_t>(Q_y & 0xFF); //P3 - Y component LSB
00312     txShtpData[7] = static_cast<uint8_t>(Q_y >> 8); //P4 - Y component MSB
00313 
00314     txShtpData[8] = static_cast<uint8_t>(Q_z & 0xFF); //P5 - Z component LSB
00315     txShtpData[9] = static_cast<uint8_t>(Q_z >> 8); //P6 - Z component MSB
00316 
00317     txShtpData[10] = static_cast<uint8_t>(Q_w & 0xFF); //P7 - W component LSB
00318     txShtpData[11] = static_cast<uint8_t>(Q_w >> 8); //P8 - W component MSB
00319 
00320     //Using this shtpData packet, send a command
00321     sendCommand(COMMAND_TARE); // Send tare command
00322 
00323     // NOTE: unlike literally every other command, a sensor orientation command is never acknowledged in any way.
00324 }
00325 
00326 #define ORIENTATION_RECORD_LEN 4
00327 
00328 bool BNO080Base::setPermanentOrientation(Quaternion orientation)
00329 {
00330     uint32_t orientationRecord[ORIENTATION_RECORD_LEN];
00331 
00332     // each word is one element of the quaternion
00333     orientationRecord[0] = static_cast<uint32_t>(floatToQ_dword(orientation.x(), FRS_ORIENTATION_Q_POINT));
00334     orientationRecord[1] = static_cast<uint32_t>(floatToQ_dword(orientation.y(), FRS_ORIENTATION_Q_POINT));
00335     orientationRecord[2] = static_cast<uint32_t>(floatToQ_dword(orientation.z(), FRS_ORIENTATION_Q_POINT));
00336     orientationRecord[3] = static_cast<uint32_t>(floatToQ_dword(orientation.w(), FRS_ORIENTATION_Q_POINT));
00337 
00338     return writeFRSRecord(FRS_RECORDID_SYSTEM_ORIENTATION, orientationRecord, ORIENTATION_RECORD_LEN);
00339 }                                                                                                       
00340 
00341 bool BNO080Base::updateData()
00342 {
00343     if(_int.read() != 0)
00344     {
00345         // no waiting packets
00346         return false;
00347     }
00348 
00349     while(_int.read() == 0)
00350     {
00351         if(!receivePacket())
00352         {
00353             // comms error
00354             return false;
00355         }
00356 
00357         processPacket();
00358 
00359         // Allow time for the IMU to ready another packet if it has one
00360         wait_us(150); // time between packets measured with a logic analyzer
00361     }
00362 
00363     // packets were received, so data may have changed
00364     return true;
00365 }
00366 
00367 uint8_t BNO080Base::getReportStatus(Report report)
00368 {
00369     uint8_t reportNum = static_cast<uint8_t>(report);
00370     if(reportNum > STATUS_ARRAY_LEN)
00371     {
00372         return 0;
00373     }
00374 
00375     return reportStatus[reportNum];
00376 }
00377 
00378 const char* BNO080Base::getReportStatusString(Report report)
00379 {
00380     switch(getReportStatus(report))
00381     {
00382         case 0:
00383             return "Unreliable";
00384         case 1:
00385             return "Accuracy Low";
00386         case 2:
00387             return "Accuracy Medium";
00388         case 3:
00389             return "Accuracy High";
00390         default:
00391             return "Error";
00392     }
00393 }
00394 
00395 bool BNO080Base::hasNewData(Report report)
00396 {
00397     uint8_t reportNum = static_cast<uint8_t>(report);
00398     if(reportNum > STATUS_ARRAY_LEN)
00399     {
00400         return false;
00401     }
00402 
00403     bool newData = reportHasBeenUpdated[reportNum];
00404     reportHasBeenUpdated[reportNum] = false; // clear flag
00405     return newData;
00406 }
00407 
00408 //Sends the packet to enable the rotation vector
00409 void BNO080Base::enableReport(Report report, uint16_t timeBetweenReports)
00410 {
00411 #if BNO_DEBUG
00412     // check time is valid
00413     float periodSeconds = static_cast<float>(timeBetweenReports / 1000.0);
00414 
00415     if(periodSeconds < getMinPeriod(report))
00416     {
00417         _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",
00418                            static_cast<uint8_t>(report), periodSeconds, getMinPeriod(report));
00419         return;
00420     }
00421 
00422 #endif
00423     setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports);
00424 
00425     // note: we don't wait for ACKs on these packets because they can take quite a while, like half a second, to come in
00426 }
00427 
00428 void BNO080Base::disableReport(Report report)
00429 {
00430     // set the report's polling period to zero to disable it
00431     setFeatureCommand(static_cast<uint8_t>(report), 0);
00432 }
00433 
00434 uint32_t BNO080Base::getSerialNumber()
00435 {
00436     uint32_t serNoBuffer;
00437 
00438     if(!readFRSRecord(FRS_RECORDID_SERIAL_NUMBER, &serNoBuffer, 1))
00439     {
00440         return 0;
00441     }
00442 
00443     return serNoBuffer;
00444 }
00445 
00446 float BNO080Base::getRange(Report report)
00447 {
00448     loadReportMetadata(report);
00449 
00450     return qToFloat_dword(metadataRecord[1], getQ1(report));
00451 }
00452 
00453 
00454 float BNO080Base::getResolution(Report report)
00455 {
00456     loadReportMetadata(report);
00457 
00458     return qToFloat_dword(metadataRecord[2], getQ1(report));
00459 }
00460 
00461 float BNO080Base::getPower(Report report)
00462 {
00463     loadReportMetadata(report);
00464 
00465     uint16_t powerQ = static_cast<uint16_t>(metadataRecord[3] & 0xFFFF);
00466 
00467     return qToFloat_dword(powerQ, POWER_Q_POINT);
00468 }
00469 
00470 float BNO080Base::getMinPeriod(Report report)
00471 {
00472     loadReportMetadata(report);
00473 
00474     return metadataRecord[4] / 1e6f; // convert from microseconds to seconds
00475 }
00476 
00477 float BNO080Base::getMaxPeriod(Report report)
00478 {
00479     loadReportMetadata(report);
00480 
00481     if(getMetaVersion() == 3)
00482     {
00483         // no max period entry in this record format
00484         return -1.0f;
00485     }
00486 
00487     return metadataRecord[9] / 1e6f; // convert from microseconds to seconds
00488 }
00489 
00490 void BNO080Base::printMetadataSummary(Report report)
00491 {
00492 #if BNO_DEBUG
00493     if(!loadReportMetadata(report))
00494     {
00495         _debugPort->printf("Failed to load report metadata!\n");
00496     }
00497 
00498     _debugPort->printf("======= Metadata for report 0x%02hhx =======\n", static_cast<uint8_t>(report));
00499 
00500     _debugPort->printf("Range: +- %.04f units\n", getRange(report));
00501     _debugPort->printf("Resolution: %.04f units\n", getResolution(report));
00502     _debugPort->printf("Power Used: %.03f mA\n", getPower(report));
00503     _debugPort->printf("Min Period: %.06f s\n", getMinPeriod(report));
00504     _debugPort->printf("Max Period: %.06f s\n\n", getMaxPeriod(report));
00505 
00506 #endif
00507 }
00508 
00509 int16_t BNO080Base::getQ1(Report report)
00510 {
00511     loadReportMetadata(report);
00512 
00513     return static_cast<int16_t>(metadataRecord[7] & 0xFFFF);
00514 }
00515 
00516 int16_t BNO080Base::getQ2(Report report)
00517 {
00518     loadReportMetadata(report);
00519 
00520     return static_cast<int16_t>(metadataRecord[7] >> 16);
00521 }
00522 
00523 int16_t BNO080Base::getQ3(Report report)
00524 {
00525     loadReportMetadata(report);
00526 
00527     return static_cast<int16_t>(metadataRecord[8] >> 16);
00528 }
00529 
00530 void BNO080Base::processPacket()
00531 {
00532     if(rxShtpHeader[2] == CHANNEL_CONTROL)
00533     {
00534         // currently no command reports are read
00535     }
00536     else if(rxShtpHeader[2] == CHANNEL_EXECUTABLE)
00537     {
00538         // currently no executable reports are read
00539     }
00540     else if(rxShtpHeader[2] == CHANNEL_COMMAND)
00541     {
00542 
00543     }
00544     else if(rxShtpHeader[2] == CHANNEL_REPORTS || rxShtpHeader[2] == CHANNEL_WAKE_REPORTS)
00545     {
00546         if(rxShtpData[0] == SHTP_REPORT_BASE_TIMESTAMP)
00547         {
00548             // sensor data packet
00549             parseSensorDataPacket();
00550         }
00551     }
00552 }
00553 
00554 // sizes of various sensor data packet elements
00555 #define SIZEOF_BASE_TIMESTAMP 5
00556 #define SIZEOF_TIMESTAMP_REBASE 5
00557 #define SIZEOF_ACCELEROMETER 10
00558 #define SIZEOF_LINEAR_ACCELERATION 10
00559 #define SIZEOF_GYROSCOPE_CALIBRATED 10
00560 #define SIZEOF_MAGNETIC_FIELD_CALIBRATED 10
00561 #define SIZEOF_MAGNETIC_FIELD_UNCALIBRATED 16
00562 #define SIZEOF_ROTATION_VECTOR 14
00563 #define SIZEOF_GAME_ROTATION_VECTOR 12
00564 #define SIZEOF_GEOMAGNETIC_ROTATION_VECTOR 14
00565 #define SIZEOF_TAP_DETECTOR 5
00566 #define SIZEOF_STABILITY_REPORT 6
00567 #define SIZEOF_STEP_DETECTOR 8
00568 #define SIZEOF_STEP_COUNTER 12
00569 #define SIZEOF_SIGNIFICANT_MOTION 6
00570 #define SIZEOF_SHAKE_DETECTOR 6
00571 
00572 void BNO080Base::parseSensorDataPacket()
00573 {
00574     size_t currReportOffset = 0;
00575 
00576     // every sensor data report first contains a timestamp offset to show how long it has been between when
00577     // the host interrupt was sent and when the packet was transmitted.
00578     // We don't use interrupts and don't care about times, so we can throw this out.
00579     currReportOffset += SIZEOF_BASE_TIMESTAMP;
00580 
00581     while(currReportOffset < rxPacketLength)
00582     {
00583         // lots of sensor reports use 3 16-bit numbers stored in bytes 4 through 9
00584         // we can save some time by parsing those out here.
00585         uint16_t data1 = (uint16_t)rxShtpData[currReportOffset + 5] << 8 | rxShtpData[currReportOffset + 4];
00586         uint16_t data2 = (uint16_t)rxShtpData[currReportOffset + 7] << 8 | rxShtpData[currReportOffset + 6];
00587         uint16_t data3 = (uint16_t)rxShtpData[currReportOffset + 9] << 8 | rxShtpData[currReportOffset + 8];
00588 
00589         uint8_t reportNum = rxShtpData[currReportOffset];
00590 
00591         if(reportNum != SENSOR_REPORTID_TIMESTAMP_REBASE)
00592         {
00593             // set status from byte 2
00594             reportStatus[reportNum] = static_cast<uint8_t>(rxShtpData[currReportOffset + 2] & 0b11);
00595 
00596             // set updated flag
00597             reportHasBeenUpdated[reportNum] = true;
00598         }
00599 
00600         switch(rxShtpData[currReportOffset])
00601         {
00602             case SENSOR_REPORTID_TIMESTAMP_REBASE:
00603                 currReportOffset += SIZEOF_TIMESTAMP_REBASE;
00604                 break;
00605 
00606             case SENSOR_REPORTID_ACCELEROMETER:
00607 
00608                 totalAcceleration = TVector3 (
00609                         qToFloat(data1, ACCELEROMETER_Q_POINT),
00610                         qToFloat(data2, ACCELEROMETER_Q_POINT),
00611                         qToFloat(data3, ACCELEROMETER_Q_POINT));
00612 
00613                 currReportOffset += SIZEOF_ACCELEROMETER;
00614                 break;
00615 
00616             case SENSOR_REPORTID_LINEAR_ACCELERATION:
00617 
00618                 linearAcceleration = TVector3 (
00619                         qToFloat(data1, ACCELEROMETER_Q_POINT),
00620                         qToFloat(data2, ACCELEROMETER_Q_POINT),
00621                         qToFloat(data3, ACCELEROMETER_Q_POINT));
00622 
00623                 currReportOffset += SIZEOF_LINEAR_ACCELERATION;
00624                 break;
00625 
00626             case SENSOR_REPORTID_GRAVITY:
00627 
00628                 gravityAcceleration = TVector3 (
00629                         qToFloat(data1, ACCELEROMETER_Q_POINT),
00630                         qToFloat(data2, ACCELEROMETER_Q_POINT),
00631                         qToFloat(data3, ACCELEROMETER_Q_POINT));
00632 
00633                 currReportOffset += SIZEOF_LINEAR_ACCELERATION;
00634                 break;
00635 
00636             case SENSOR_REPORTID_GYROSCOPE_CALIBRATED:
00637 
00638                 gyroRotation = TVector3 (
00639                         qToFloat(data1, GYRO_Q_POINT),
00640                         qToFloat(data2, GYRO_Q_POINT),
00641                         qToFloat(data3, GYRO_Q_POINT));
00642 
00643                 currReportOffset += SIZEOF_GYROSCOPE_CALIBRATED;
00644                 break;
00645 
00646             case SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED:
00647 
00648                 magField = TVector3 (
00649                         qToFloat(data1, MAGNETOMETER_Q_POINT),
00650                         qToFloat(data2, MAGNETOMETER_Q_POINT),
00651                         qToFloat(data3, MAGNETOMETER_Q_POINT));
00652 
00653                 currReportOffset += SIZEOF_MAGNETIC_FIELD_CALIBRATED;
00654                 break;
00655 
00656             case SENSOR_REPORTID_MAGNETIC_FIELD_UNCALIBRATED:
00657             {
00658                 magFieldUncalibrated = TVector3 (
00659                         qToFloat(data1, MAGNETOMETER_Q_POINT),
00660                         qToFloat(data2, MAGNETOMETER_Q_POINT),
00661                         qToFloat(data3, MAGNETOMETER_Q_POINT));
00662 
00663                 uint16_t ironOffsetXQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
00664                 uint16_t ironOffsetYQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12];
00665                 uint16_t ironOffsetZQ = (uint16_t) rxShtpData[currReportOffset + 15] << 8 | rxShtpData[currReportOffset + 14];
00666 
00667                 hardIronOffset = TVector3 (
00668                         qToFloat(ironOffsetXQ, MAGNETOMETER_Q_POINT),
00669                         qToFloat(ironOffsetYQ, MAGNETOMETER_Q_POINT),
00670                         qToFloat(ironOffsetZQ, MAGNETOMETER_Q_POINT));
00671 
00672                 currReportOffset += SIZEOF_MAGNETIC_FIELD_UNCALIBRATED;
00673             }
00674                 break;
00675 
00676             case SENSOR_REPORTID_ROTATION_VECTOR:
00677             {
00678                 uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
00679                 uint16_t accuracyQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12];
00680 
00681                 rotationVector = TVector4(
00682                         qToFloat(data1, ROTATION_Q_POINT),
00683                         qToFloat(data2, ROTATION_Q_POINT),
00684                         qToFloat(data3, ROTATION_Q_POINT),
00685                         qToFloat(realPartQ, ROTATION_Q_POINT));
00686 
00687                 rotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT);
00688 
00689                 currReportOffset += SIZEOF_ROTATION_VECTOR;
00690             }
00691                 break;
00692 
00693             case SENSOR_REPORTID_GAME_ROTATION_VECTOR:
00694             {
00695                 uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
00696 
00697                 gameRotationVector = TVector4(
00698                         qToFloat(data1, ROTATION_Q_POINT),
00699                         qToFloat(data2, ROTATION_Q_POINT),
00700                         qToFloat(data3, ROTATION_Q_POINT),
00701                         qToFloat(realPartQ, ROTATION_Q_POINT));
00702 
00703                 currReportOffset += SIZEOF_GAME_ROTATION_VECTOR;
00704             }
00705                 break;
00706 
00707             case SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR:
00708             {
00709                 uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
00710                 uint16_t accuracyQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12];
00711 
00712                 geomagneticRotationVector = TVector4(
00713                         qToFloat(data1, ROTATION_Q_POINT),
00714                         qToFloat(data2, ROTATION_Q_POINT),
00715                         qToFloat(data3, ROTATION_Q_POINT),
00716                         qToFloat(realPartQ, ROTATION_Q_POINT));
00717 
00718                 geomagneticRotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT);
00719 
00720                 currReportOffset += SIZEOF_GEOMAGNETIC_ROTATION_VECTOR;
00721             }
00722                 break;
00723 
00724             case SENSOR_REPORTID_TAP_DETECTOR:
00725 
00726                 // since we got the report, a tap was detected
00727                 tapDetected = true;
00728 
00729                 doubleTap = (rxShtpData[currReportOffset + 4] & (1 << 6)) != 0;
00730 
00731                 currReportOffset += SIZEOF_TAP_DETECTOR;
00732                 break;
00733 
00734             case SENSOR_REPORTID_STABILITY_CLASSIFIER:
00735             {
00736                 uint8_t classificationNumber = rxShtpData[currReportOffset + 4];
00737 
00738                 if(classificationNumber > 4)
00739                 {
00740                     classificationNumber = 0;
00741                 }
00742 
00743                 stability = static_cast<Stability>(classificationNumber);
00744 
00745                 currReportOffset += SIZEOF_STABILITY_REPORT;
00746             }
00747                 break;
00748 
00749             case SENSOR_REPORTID_STEP_DETECTOR:
00750 
00751                 // the fact that we got the report means that a step was detected
00752                 stepDetected = true;
00753 
00754                 currReportOffset += SIZEOF_STEP_DETECTOR;
00755 
00756                 break;
00757 
00758             case SENSOR_REPORTID_STEP_COUNTER:
00759 
00760                 stepCount = rxShtpData[currReportOffset + 9] << 8 | rxShtpData[currReportOffset + 8];
00761 
00762                 currReportOffset += SIZEOF_STEP_COUNTER;
00763 
00764                 break;
00765 
00766             case SENSOR_REPORTID_SIGNIFICANT_MOTION:
00767 
00768                 // the fact that we got the report means that significant motion was detected
00769                 significantMotionDetected = true;
00770 
00771                 currReportOffset += SIZEOF_SIGNIFICANT_MOTION;
00772                 
00773                 break;
00774 
00775             case SENSOR_REPORTID_SHAKE_DETECTOR:
00776 
00777                 shakeDetected = true;
00778 
00779                 xAxisShake = (rxShtpData[currReportOffset + 4] & 1) != 0;
00780                 yAxisShake = (rxShtpData[currReportOffset + 4] & (1 << 1)) != 0;
00781                 zAxisShake = (rxShtpData[currReportOffset + 4] & (1 << 2)) != 0;
00782 
00783                 currReportOffset += SIZEOF_SHAKE_DETECTOR;
00784 
00785                 break;
00786                 
00787             default:
00788                 _debugPort->printf("Error: unrecognized report ID in sensor report: %hhx.  Byte %u, length %hu\n", rxShtpData[currReportOffset], currReportOffset, rxPacketLength);
00789                 return;
00790         }
00791 
00792         if(currReportOffset >= SHTP_RX_PACKET_SIZE)
00793         {
00794             _debugPort->printf("Error: sensor report longer than packet buffer! Some data was not read! Increase buffer size or decrease number of reports!\r\n");
00795             return;
00796         }
00797     }
00798 
00799 }
00800 
00801 bool BNO080Base::waitForPacket(int channel, uint8_t reportID, std::chrono::milliseconds timeout)
00802 {
00803     Timer timeoutTimer;
00804     timeoutTimer.start();
00805 
00806     while(timeoutTimer.elapsed_time() <= timeout)
00807     {
00808         if(_int.read() == 0)
00809         {
00810             if(!receivePacket(timeout))
00811             {
00812                 return false;
00813             }
00814 
00815             if(channel == rxShtpHeader[2] && reportID == rxShtpData[0])
00816             {
00817                 // found correct packet!
00818                 return true;
00819             }
00820             else
00821             {
00822                 // other data packet, send to proper channels
00823                 processPacket();
00824             }
00825         }
00826     }
00827 
00828     _debugPort->printf("Packet wait timeout.\n");
00829     return false;
00830 }
00831 
00832 //Given a register value and a Q point, convert to float
00833 //See https://en.wikipedia.org/wiki/Q_(number_format)
00834 float BNO080Base::qToFloat(int16_t fixedPointValue, uint8_t qPoint)
00835 {
00836     float qFloat = fixedPointValue;
00837     qFloat *= pow(2.0f, qPoint * -1);
00838     return (qFloat);
00839 }
00840 
00841 float BNO080Base::qToFloat_dword(uint32_t fixedPointValue, int16_t qPoint)
00842 {
00843     float qFloat = fixedPointValue;
00844     qFloat *= pow(2.0f, qPoint * -1);
00845     return (qFloat);
00846 }
00847 
00848 //Given a floating point value and a Q point, convert to Q
00849 //See https://en.wikipedia.org/wiki/Q_(number_format)
00850 int16_t BNO080Base::floatToQ(float qFloat, uint8_t qPoint)
00851 {
00852     int16_t qVal = static_cast<int16_t>(qFloat * pow(2.0f, qPoint));
00853     return qVal;
00854 }
00855 
00856 int32_t BNO080Base::floatToQ_dword(float qFloat, uint16_t qPoint)
00857 {
00858     int32_t qVal = static_cast<int32_t>(qFloat * pow(2.0f, qPoint));
00859     return qVal;
00860 }
00861 //Tell the sensor to do a command
00862 //See 6.3.8 page 41, Command request
00863 //The caller is expected to set P0 through P8 prior to calling
00864 void BNO080Base::sendCommand(uint8_t command)
00865 {
00866     txShtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request
00867     txShtpData[1] = commandSequenceNumber++; //Increments automatically each function call
00868     txShtpData[2] = command; //Command
00869 
00870     //Caller must set these
00871     /*shtpData[3] = 0; //P0
00872         shtpData[4] = 0; //P1
00873         shtpData[5] = 0; //P2
00874         shtpData[6] = 0;
00875         shtpData[7] = 0;
00876         shtpData[8] = 0;
00877         shtpData[9] = 0;
00878         shtpData[10] = 0;
00879         shtpData[11] = 0;*/
00880 
00881     //Transmit packet on channel 2, 12 bytes
00882     sendPacket(CHANNEL_CONTROL, 12);
00883 }
00884 
00885 //Given a sensor's report ID, this tells the BNO080 to begin reporting the values
00886 //Also sets the specific config word. Useful for personal activity classifier
00887 void BNO080Base::setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig)
00888 {
00889     uint32_t microsBetweenReports = static_cast<uint32_t>(timeBetweenReports * 1000);
00890 
00891     const uint32_t batchMicros = 0;
00892 
00893     txShtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55
00894     txShtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
00895     txShtpData[2] = 0; //Feature flags
00896     txShtpData[3] = 0; //Change sensitivity (LSB)
00897     txShtpData[4] = 0; //Change sensitivity (MSB)
00898     txShtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms
00899     txShtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval
00900     txShtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval
00901     txShtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
00902     txShtpData[9] = (batchMicros >> 0) & 0xFF;  //Batch Interval (LSB)
00903     txShtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval
00904     txShtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval
00905     txShtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB)
00906     txShtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB)
00907     txShtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config
00908     txShtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config
00909     txShtpData[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB)
00910 
00911     //Transmit packet on channel 2, 17 bytes
00912     sendPacket(CHANNEL_CONTROL, 17);
00913 }
00914 
00915 bool BNO080Base::readFRSRecord(uint16_t recordID, uint32_t* readBuffer, uint16_t readLength)
00916 {
00917     // send initial read request
00918     clearSendBuffer();
00919 
00920     txShtpData[0] = SHTP_REPORT_FRS_READ_REQUEST;
00921     // read offset of 0 -> start at the start of the record
00922     txShtpData[2] = 0;
00923     txShtpData[3] = 0;
00924     // record ID
00925     txShtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
00926     txShtpData[5] = static_cast<uint8_t>(recordID >> 8);
00927     // block size
00928     txShtpData[6] = static_cast<uint8_t>(readLength & 0xFF);
00929     txShtpData[7] = static_cast<uint8_t>(readLength >> 8);
00930 
00931     sendPacket(CHANNEL_CONTROL, 8);
00932 
00933     // now, read back the responses
00934     size_t readOffset = 0;
00935     while(readOffset < readLength)
00936     {
00937         // it seems like it can take quite a long time for FRS data to be read, so we have to increase the timeout
00938         if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE, 300ms))
00939         {
00940 #if BNO_DEBUG
00941             _debugPort->printf("Error: did not receive FRS read response after sending read request!\n");
00942 #endif
00943             return false;
00944         }
00945 
00946         uint8_t status = static_cast<uint8_t>(rxShtpData[1] & 0b1111);
00947         uint8_t dataLength = rxShtpData[1] >> 4;
00948 
00949         // check status
00950         if(status == 1)
00951         {
00952 #if BNO_DEBUG
00953             _debugPort->printf("Error: FRS reports invalid record ID!\n");
00954 #endif
00955             return false;
00956         }
00957         else if(status == 2)
00958         {
00959 #if BNO_DEBUG
00960             _debugPort->printf("Error: FRS is busy!\n");
00961 #endif
00962             return false;
00963         }
00964         else if(status == 4)
00965         {
00966 #if BNO_DEBUG
00967             _debugPort->printf("Error: FRS reports offset is out of range!\n");
00968 #endif
00969             return false;
00970         }
00971         else if(status == 5)
00972         {
00973 #if BNO_DEBUG
00974             _debugPort->printf("Error: FRS reports record %hx is empty!\n", recordID);
00975 #endif
00976             return false;
00977         }
00978         else if(status == 8)
00979         {
00980 #if BNO_DEBUG
00981             _debugPort->printf("Error: FRS reports flash memory device unavailable!\n");
00982 #endif
00983             return false;
00984         }
00985 
00986         // check data length
00987         if(dataLength == 0)
00988         {
00989 #if BNO_DEBUG
00990             _debugPort->printf("Error: Received FRS packet with 0 data length!\n");
00991 #endif
00992             return false;
00993         }
00994         else if(dataLength == 1)
00995         {
00996             if(readOffset + 1 != readLength)
00997             {
00998 #if BNO_DEBUG
00999                 _debugPort->printf("Error: Received 1 length packet but more than 1 byte remains to be be read!\n");
01000 #endif
01001                 return false;
01002             }
01003         }
01004 
01005         // now, _finally_, read the dang words
01006         readBuffer[readOffset] = (rxShtpData[7] << 24) | (rxShtpData[6] << 16) | (rxShtpData[5] << 8) | (rxShtpData[4]);
01007 
01008         // check if we only wanted the first word
01009         ++readOffset;
01010         if(readOffset == readLength)
01011         {
01012             break;
01013         }
01014 
01015         readBuffer[readOffset] = (rxShtpData[11] << 24) | (rxShtpData[10] << 16) | (rxShtpData[9] << 8) | (rxShtpData[8]);
01016         readOffset++;
01017     }
01018 
01019     // read successful
01020     return true;
01021 
01022 }
01023 
01024 bool BNO080Base::writeFRSRecord(uint16_t recordID, uint32_t* buffer, uint16_t length)
01025 {
01026     // send initial write request, which tells the chip where we're writing
01027     clearSendBuffer();
01028 
01029     txShtpData[0] = SHTP_REPORT_FRS_WRITE_REQUEST;
01030     // length to write (must be <= record length)
01031     txShtpData[2] = static_cast<uint8_t>(length & 0xFF);
01032     txShtpData[3] = static_cast<uint8_t>(length >> 8);
01033     // record ID
01034     txShtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
01035     txShtpData[5] = static_cast<uint8_t>(recordID >> 8);
01036 
01037     sendPacket(CHANNEL_CONTROL, 6);
01038 
01039     // wait for FRS to become ready
01040     if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, 300ms))
01041     {
01042 #if BNO_DEBUG
01043         _debugPort->printf("Error: did not receive FRS write ready response after sending write request!\r\n");
01044 #endif
01045         return false;
01046     }
01047 
01048     if(rxShtpData[1] != 4)
01049     {
01050 #if BNO_DEBUG
01051         _debugPort->printf("Error: FRS reports error initiating write operation: %hhu!\r\n", rxShtpData[1]);
01052 #endif
01053         return false;
01054     }
01055 
01056     // now, send the actual data
01057     for(uint16_t wordIndex = 0; wordIndex < length; wordIndex += 2)
01058     {
01059         // send packet containing 2 words
01060         clearSendBuffer();
01061         txShtpData[0] = SHTP_REPORT_FRS_WRITE_DATA;
01062 
01063         // offset to write at
01064         txShtpData[2] = static_cast<uint8_t>(wordIndex & 0xFF);
01065         txShtpData[3] = static_cast<uint8_t>(wordIndex >> 8);
01066 
01067         // data 0
01068         *reinterpret_cast<uint32_t*>(txShtpData + 4) = buffer[wordIndex];
01069 
01070         // data 1, if it exists
01071         if(wordIndex != length - 1)
01072         {
01073             *reinterpret_cast<uint32_t*>(txShtpData + 8) = buffer[wordIndex + 1];
01074         }
01075 
01076         sendPacket(CHANNEL_CONTROL, 12);
01077 
01078         // wait for acknowledge
01079         if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, 300ms))
01080         {
01081 #if BNO_DEBUG
01082             _debugPort->printf("Error: did not receive FRS write response after sending write data!\r\n");
01083 #endif
01084             return false;
01085         }
01086 
01087         uint8_t status = rxShtpData[1];
01088         
01089         switch(status)
01090         {
01091             case 0:
01092                 if(length - wordIndex >= 2)
01093                 {
01094                     // status OK, write still in progress
01095                 }
01096                 else
01097                 {
01098 #if BNO_DEBUG
01099                     _debugPort->printf("Error: FRS reports write in progress when it should be complete!\r\n");
01100 #endif
01101                     return false;
01102                 }
01103                 break;
01104             case 3:
01105             case 8:
01106                 if(length - wordIndex <= 2)
01107                 {
01108                     // status OK, write complete
01109                 }
01110                 else
01111                 {
01112 #if BNO_DEBUG
01113                     _debugPort->printf("Error: FRS reports write complete when it should be still going!\n");
01114 #endif
01115                     return false;
01116                 }
01117                 break;
01118             case 1:
01119 #if BNO_DEBUG
01120                 _debugPort->printf("Error: FRS reports invalid record ID!\n");
01121 #endif
01122                 return false;
01123             case 2:
01124 #if BNO_DEBUG
01125                 _debugPort->printf("Error: FRS is busy!\n");
01126 #endif
01127                 return false;
01128             case 5:
01129 #if BNO_DEBUG
01130                 _debugPort->printf("Error: FRS reports write failed!\n");
01131 #endif
01132                 return false;
01133             case 6:
01134 #if BNO_DEBUG
01135                 _debugPort->printf("Error: FRS reports data received while not in write mode!\n");
01136 #endif
01137                 return false;
01138             case 7:
01139 #if BNO_DEBUG
01140                 _debugPort->printf("Error: FRS reports invalid length!\n");
01141 #endif
01142                 return false;
01143             case 9:
01144 #if BNO_DEBUG
01145                 _debugPort->printf("Error: FRS reports invalid data for this record!\n");
01146 #endif
01147                 return false;
01148 
01149             case 10:
01150 #if BNO_DEBUG
01151                 _debugPort->printf("Error: FRS reports flash device unavailable!\n");
01152 #endif
01153                 return false;
01154 
01155             case 11:
01156 #if BNO_DEBUG
01157                 _debugPort->printf("Error: FRS reports record is read-only!\n");
01158 #endif
01159                 return false;
01160             default:
01161 #if BNO_DEBUG
01162                 _debugPort->printf("Error: FRS reports unknown result code %hhu!\n", status);
01163 #endif
01164                 break;
01165 
01166         }
01167     }
01168 
01169     // write complete
01170     return true; 
01171 }
01172 
01173 //Pretty prints the contents of a packet in the given buffer
01174 void BNO080Base::printPacket(uint8_t * buffer)
01175 {
01176 #if BNO_DEBUG
01177     //Print the four byte header
01178     _debugPort->printf("Header:");
01179     for (uint8_t x = 0 ; x < 4 ; x++)
01180     {
01181         _debugPort->printf(" ");
01182         _debugPort->printf("%02hhx", buffer[x]);
01183     }
01184 
01185     //Calculate the number of data bytes in this packet
01186     uint16_t totalLength = (static_cast<uint16_t>(buffer[1]) << 8 | buffer[0]);
01187     totalLength &= ~(1 << 15);
01188     uint16_t packetLength = totalLength - 4; //Remove the header bytes from the data count
01189 
01190     uint16_t printLength = std::min(packetLength, static_cast<uint16_t>(40)); //Artificial limit. We don't want the phone book.
01191 
01192     _debugPort->printf(" Body:");
01193     for (uint16_t x = 0 ; x < printLength ; x++)
01194     {
01195         _debugPort->printf(" ");
01196         _debugPort->printf("%02hhx", buffer[x + SHTP_HEADER_SIZE]);
01197     }
01198 
01199     _debugPort->printf(", Length:");
01200     _debugPort->printf("%d", packetLength + SHTP_HEADER_SIZE);
01201     _debugPort->printf(", SeqNum: %hhu", buffer[3]);
01202 
01203     _debugPort->printf(", Channel:");
01204     if (buffer[2] == 0) _debugPort->printf("Command");
01205     else if (buffer[2] == 1) _debugPort->printf("Executable");
01206     else if (buffer[2] == 2) _debugPort->printf("Control");
01207     else if (buffer[2] == 3) _debugPort->printf("Sensor-report");
01208     else if (buffer[2] == 4) _debugPort->printf("Wake-report");
01209     else if (buffer[2] == 5) _debugPort->printf("Gyro-vector");
01210     else _debugPort->printf("%hhu", buffer[2]);
01211 
01212     _debugPort->printf("\n");
01213 #endif
01214 }
01215 
01216 
01217 void BNO080Base::clearSendBuffer()
01218 {
01219     memset(txPacketBuffer, 0, SHTP_HEADER_SIZE + SHTP_MAX_TX_PACKET_SIZE);
01220 }
01221 
01222 bool BNO080Base::loadReportMetadata(BNO080Base::Report report)
01223 {
01224     uint16_t reportMetaRecord = 0;
01225 
01226     // first, convert the report into the correct FRS record ID for that report's metadata
01227     // data from SH-2 section 5.1
01228     switch(report)
01229     {
01230         case TOTAL_ACCELERATION:
01231             reportMetaRecord = 0xE301;
01232             break;
01233         case LINEAR_ACCELERATION:
01234             reportMetaRecord = 0xE303;
01235             break;
01236         case GRAVITY_ACCELERATION:
01237             reportMetaRecord = 0xE304;
01238             break;
01239         case GYROSCOPE:
01240             reportMetaRecord = 0xE306;
01241             break;
01242         case MAG_FIELD:
01243             reportMetaRecord = 0xE309;
01244             break;
01245         case MAG_FIELD_UNCALIBRATED:
01246             reportMetaRecord = 0xE30A;
01247             break;
01248         case ROTATION:
01249             reportMetaRecord = 0xE30B;
01250             break;
01251         case GEOMAGNETIC_ROTATION:
01252             reportMetaRecord = 0xE30D;
01253             break;
01254         case GAME_ROTATION:
01255             reportMetaRecord = 0xE30C;
01256             break;
01257         case TAP_DETECTOR:
01258             reportMetaRecord = 0xE313;
01259             break;
01260         case STABILITY_CLASSIFIER:
01261             reportMetaRecord = 0xE317;
01262             break;
01263         case STEP_DETECTOR:
01264             reportMetaRecord = 0xE314;
01265             break;
01266         case STEP_COUNTER:
01267             reportMetaRecord = 0xE315;
01268             break;
01269         case SIGNIFICANT_MOTION:
01270             reportMetaRecord = 0xE316;
01271             break;
01272         case SHAKE_DETECTOR:
01273             reportMetaRecord = 0xE318;
01274             break;
01275     }
01276 
01277     // if we already have that data stored, everything's OK
01278     if(bufferMetadataRecord == reportMetaRecord)
01279     {
01280         return true;
01281     }
01282 
01283     // now, load the metadata into the buffer
01284     if(!readFRSRecord(reportMetaRecord, metadataRecord, METADATA_BUFFER_LEN))
01285     {
01286         // clear this so future calls won't try to use the cached version
01287         bufferMetadataRecord = 0;
01288 
01289         return false;
01290     }
01291 
01292     bufferMetadataRecord = reportMetaRecord;
01293 
01294     return true;
01295 }
01296 
01297 BNO080I2C::BNO080I2C(Stream *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin,
01298                        uint8_t i2cAddress, int i2cPortSpeed) :
01299        BNO080Base(debugPort, user_INTPin, user_RSTPin),
01300         _i2cPort(user_SDApin, user_SCLpin),
01301         _i2cAddress(i2cAddress)
01302 {
01303 
01304     //Get user settings
01305     _i2cPortSpeed = i2cPortSpeed;
01306     if(_i2cPortSpeed > 4000000)
01307     {
01308         _i2cPortSpeed = 4000000; //BNO080 max is 400Khz
01309     }
01310     _i2cPort.frequency(_i2cPortSpeed);
01311 }
01312 
01313 
01314 //Given the data packet, send the header then the data
01315 //Returns false if sensor does not ACK
01316 bool BNO080I2C::sendPacket(uint8_t channelNumber, uint8_t dataLength)
01317 {
01318 
01319     uint16_t totalLength = dataLength + 4; //Add four bytes for the header
01320 
01321     txShtpHeader[0] = totalLength & 0xFF;
01322     txShtpHeader[1] = totalLength >> 8;
01323     txShtpHeader[2] = channelNumber;
01324     txShtpHeader[3] = sequenceNumber[channelNumber];
01325 
01326 #if BNO_DEBUG
01327     _debugPort->printf("Transmitting packet: ----------------\n");
01328     printPacket(txPacketBuffer);
01329 #endif
01330 
01331     // send packet to IMU
01332     int writeResult = _i2cPort.write(_i2cAddress << 1, reinterpret_cast<char*>(txPacketBuffer), totalLength);
01333 
01334     if(writeResult != 0)
01335     {
01336         _debugPort->printf("BNO I2C write failed!\n");
01337         return false;
01338     }
01339     return true;
01340 }
01341 
01342 //Check to see if there is any new data available
01343 //Read the contents of the incoming packet into the shtpData array
01344 bool BNO080I2C::receivePacket(std::chrono::milliseconds timeout)
01345 {
01346     Timer waitStartTime;
01347     waitStartTime.start();
01348 
01349     while(_int.read() != 0)
01350     {
01351         if(waitStartTime.elapsed_time() > timeout)
01352         {
01353             _debugPort->printf("BNO I2C header wait timeout\n");
01354             return false;
01355         }
01356 
01357     }
01358 
01359     // start the transaction and contact the IMU
01360     _i2cPort.start();
01361 
01362     // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1
01363     int writeResult = _i2cPort.write((_i2cAddress << 1) | 0x1);
01364 
01365     if(writeResult != 1)
01366     {
01367         _debugPort->printf("BNO I2C read failed!\n");
01368         return false;
01369     }
01370 
01371     //Get the first four bytes, aka the packet header
01372     uint8_t packetLSB = static_cast<uint8_t>(_i2cPort.read(true));
01373     uint8_t packetMSB = static_cast<uint8_t>(_i2cPort.read(true));
01374     uint8_t channelNumber = static_cast<uint8_t>(_i2cPort.read(true));
01375     uint8_t sequenceNum = static_cast<uint8_t>(_i2cPort.read(true)); //Not sure if we need to store this or not
01376 
01377     //Store the header info
01378     rxShtpHeader[0] = packetLSB;
01379     rxShtpHeader[1] = packetMSB;
01380     rxShtpHeader[2] = channelNumber;
01381     rxShtpHeader[3] = sequenceNum;
01382 
01383     if(rxShtpHeader[0] == 0xFF && rxShtpHeader[1] == 0xFF)
01384     {
01385         // invalid according to BNO080 datasheet section 1.4.1
01386 
01387 #if BNO_DEBUG
01388         _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n");
01389 #endif
01390         return false;
01391     }
01392 
01393     //Calculate the number of data bytes in this packet
01394     rxPacketLength = (static_cast<uint16_t>(packetMSB) << 8 | packetLSB);
01395 
01396     // Clear the MSbit.
01397     // 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)
01398     // but we don't actually care about any of the advertisement packets
01399     // that use this, so we can just cut off the rest of the packet by releasing chip select.
01400     rxPacketLength &= ~(1 << 15);
01401 
01402     if (rxPacketLength == 0)
01403     {
01404         // Packet is empty
01405         return (false); //All done
01406     }
01407 
01408     rxPacketLength -= 4; //Remove the header bytes from the data count
01409 
01410     //Read incoming data into the shtpData array
01411     for (uint16_t dataSpot = 0 ; dataSpot < rxPacketLength ; dataSpot++)
01412     {
01413         bool sendACK = dataSpot < rxPacketLength - 1;
01414 
01415         // per the datasheet, 0xFF is used as filler for the receiver to transmit back
01416         uint8_t incoming = static_cast<uint8_t>(_i2cPort.read(sendACK));
01417         if (dataSpot < (sizeof(rxPacketBuffer) - SHTP_HEADER_SIZE)) //BNO080 can respond with upto 270 bytes, avoid overflow
01418             rxShtpData[dataSpot] = incoming; //Store data into the shtpData array
01419     }
01420 
01421     _i2cPort.stop();
01422 
01423     /*
01424     // first read the header to get the length of the packet
01425     int i2cResult;
01426     i2cResult = _i2cPort.read(_i2cAddress << 1, reinterpret_cast<char*>(packetBuffer), SHTP_HEADER_SIZE, false);
01427     if(i2cResult != 0)
01428     {
01429         _debugPort->printf("BNO I2C length read failed!\n");
01430         return false;
01431     }
01432 
01433     if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF)
01434     {
01435         // invalid according to BNO080 datasheet section 1.4.1
01436 #if BNO_DEBUG
01437         _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n");
01438 #endif
01439         return false;
01440     }
01441 
01442     //Calculate the number of data bytes in this packet
01443     uint16_t totalLength = (static_cast<uint16_t>(shtpHeader[1]) << 8 | shtpHeader[0]);
01444 
01445     // Clear the MSbit.
01446     // 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)
01447     // but we don't actually care about any of the advertisement packets
01448     // that use this, so we can just cut off the rest of the packet by releasing chip select.
01449     totalLength &= ~(1 << 15);
01450 
01451     if (totalLength == 0)
01452     {
01453         // Packet is empty
01454         return (false); //All done
01455     }
01456 
01457     // only receive as many bytes as we can fit
01458     if(totalLength > sizeof(packetBuffer))
01459     {
01460         totalLength = sizeof(packetBuffer);
01461     }
01462 
01463     packetLength = totalLength - 4; //Remove the header bytes from the data count
01464 
01465     waitStartTime.reset();
01466 
01467     while(_int.read() != 0)
01468     {
01469         if(waitStartTime.elapsed_time() > timeout)
01470         {
01471             _debugPort->printf("BNO I2C wait timeout\n");
01472             return false;
01473         }
01474 
01475     }
01476 
01477     //Read the actual packet bytes
01478     _debugPort->printf("Attempting to read %" PRIu16 " bytes\n", totalLength);
01479     i2cResult = _i2cPort.read(_i2cAddress << 1, reinterpret_cast<char*>(packetBuffer), totalLength);
01480     if(i2cResult != 0)
01481     {
01482         _debugPort->printf("BNO I2C read failed!\n");
01483         return false;
01484     }
01485     */
01486 
01487 #if BNO_DEBUG
01488     _debugPort->printf("Recieved packet: ----------------\n");
01489     printPacket(rxPacketBuffer); // note: add 4 for the header length
01490 #endif
01491 
01492     return (true); //We're done!
01493 }
01494 
01495 BNO080SPI::BNO080SPI(Stream *debugPort, PinName rstPin, PinName intPin, PinName wakePin, PinName misoPin,
01496                      PinName mosiPin, PinName sclkPin, PinName csPin, int spiSpeed):
01497 BNO080Base(debugPort, intPin, rstPin),
01498 _spiPort(mosiPin, misoPin, sclkPin, csPin, use_gpio_ssel),
01499 _wakePin(wakePin, 1), // wake pin needs to be 1 on reset so that the BNO boots up into SPI mode
01500 _spiSpeed(spiSpeed)
01501 {
01502     _spiPort.frequency(spiSpeed);
01503     _spiPort.format(8, 3);
01504     _spiPort.set_default_write_value(0x0);
01505 }
01506 
01507 bool BNO080SPI::sendPacket(uint8_t channelNumber, uint8_t dataLength)
01508 {
01509     if(_int.read() == 0)
01510     {
01511         // The BNO is already awake because it has a packet it wants to send to us
01512     }
01513     else
01514     {
01515         // assert WAKE to tell the BNO to prepare for a transfer
01516         _wakePin = 0;
01517 
01518         Timer waitStartTime;
01519         waitStartTime.start();
01520         const std::chrono::milliseconds timeout = 10ms;
01521 
01522         while(_int.read() != 0)
01523         {
01524             if(waitStartTime.elapsed_time() > timeout)
01525             {
01526                 _debugPort->printf("BNO SPI wake wait timeout\n");
01527                 _wakePin = 1;
01528                 return false;
01529             }
01530         }
01531         _wakePin = 1;
01532     }
01533 
01534     uint16_t totalLength = dataLength + 4; //Add four bytes for the header
01535 
01536     txShtpHeader[0] = totalLength & 0xFF;
01537     txShtpHeader[1] = totalLength >> 8;
01538     txShtpHeader[2] = channelNumber;
01539     txShtpHeader[3] = sequenceNumber[channelNumber];
01540 
01541 #if BNO_DEBUG
01542     _debugPort->printf("Transmitting packet: ----------------\n");
01543     printPacket(txPacketBuffer);
01544 #endif
01545 
01546     // wipe out any existing RX packet header so we know if we received a packet
01547     memset(rxPacketBuffer, 0, SHTP_HEADER_SIZE);
01548 
01549     // send packet to IMU.
01550     // This also might receive the first part of another packet, which there is no way to avoid.
01551     spiTransferAndWait(txPacketBuffer, totalLength, rxPacketBuffer, totalLength);
01552 
01553     if(rxShtpHeader[0] == 0 && rxShtpHeader[0] == 0)
01554     {
01555         // no header data so no packet received
01556         return true;
01557     }
01558     else
01559     {
01560         // received first part of data packet while writing
01561         if(receiveCompletePacket(totalLength))
01562         {
01563             // received data packet, send to proper channels
01564             processPacket();
01565             return true;
01566         }
01567 
01568         // receive failed
01569         return false;
01570     }
01571 }
01572 
01573 //Check to see if there is any new data available
01574 //Read the contents of the incoming packet into the shtpData array
01575 bool BNO080SPI::receivePacket(std::chrono::milliseconds timeout)
01576 {
01577     Timer waitStartTime;
01578     waitStartTime.start();
01579 
01580     while (_int.read() != 0)
01581     {
01582         if (waitStartTime.elapsed_time() > timeout)
01583         {
01584             _debugPort->printf("BNO SPI header wait timeout\n");
01585             return false;
01586         }
01587 
01588     }
01589 
01590     // read the header bytes first.
01591     spiTransferAndWait(nullptr, 0, rxPacketBuffer, SHTP_HEADER_SIZE);
01592 
01593     // now read the data
01594     return receiveCompletePacket(SHTP_HEADER_SIZE, timeout);
01595 }
01596 
01597 bool BNO080SPI::receiveCompletePacket(size_t bytesRead, std::chrono::milliseconds timeout)
01598 {
01599     // Process header bytes
01600     // ------------------------------------------------------------------------
01601     if (rxShtpHeader[0] == 0xFF && rxShtpHeader[1] == 0xFF)
01602     {
01603         // invalid according to BNO080 datasheet section 1.4.1
01604 
01605 #if BNO_DEBUG
01606         _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n");
01607 #endif
01608         return false;
01609     }
01610 
01611     //Calculate the number of data bytes in this packet
01612     uint16_t totalLength = (static_cast<uint16_t>(rxShtpHeader[1]) << 8 | rxShtpHeader[0]);
01613 
01614     // Clear the MSbit.
01615     totalLength &= ~(1 << 15);
01616 
01617     if (totalLength == 0)
01618     {
01619         // Packet is empty
01620         return (false); //All done
01621     }
01622 
01623     rxPacketLength = totalLength - SHTP_HEADER_SIZE;
01624 
01625     if(totalLength <= bytesRead)
01626     {
01627         // the original transaction already read the completed packet!  We're done.
01628         return true;
01629     }
01630 
01631     // Receive data
01632     // ------------------------------------------------------------------------
01633 
01634     // Wait for it to be ready to talk to us again.
01635     // Note: in my testing this takes about 200ms
01636     Timer waitStartTime;
01637     waitStartTime.start();
01638     while (_int.read() != 0)
01639     {
01640         if (waitStartTime.elapsed_time() > timeout)
01641         {
01642             _debugPort->printf("BNO SPI continued packet header wait timeout\n");
01643             return false;
01644         }
01645 
01646     }
01647 
01648     if(rxPacketLength > SHTP_RX_PACKET_SIZE)
01649     {
01650         _debugPort->printf("Packet too long (%" PRIu16 " bytes), increase SHTP_RX_PACKET_SIZE\n", rxPacketLength);
01651         _debugPort->printf("Packet dropped, expect subsequent driver errors.\n");
01652         return false;
01653     }
01654 
01655     if(bytesRead == SHTP_HEADER_SIZE)
01656     {
01657         // just read the entire packet into the buffer
01658         spiTransferAndWait(nullptr, 0, rxPacketBuffer, totalLength);
01659     }
01660     else
01661     {
01662         // we want to receive a new header, plus the remaining data bytes that haven't been read.
01663         size_t receiveLength = SHTP_HEADER_SIZE + (totalLength - bytesRead);
01664 
01665         // read remaining bytes into the data buffer starting at the next byte
01666         spiTransferAndWait(nullptr, 0, rxPacketBuffer + bytesRead, receiveLength);
01667 
01668         // erase the new header we just read, leaving only the data as a contiguous block
01669         std::memmove(rxPacketBuffer + bytesRead, rxPacketBuffer + bytesRead + SHTP_HEADER_SIZE, receiveLength - SHTP_HEADER_SIZE);
01670     }
01671 
01672 #if BNO_DEBUG
01673     _debugPort->printf("Recieved packet: ----------------\n");
01674     printPacket(rxPacketBuffer);
01675 #endif
01676 
01677     //ThisThread::sleep_for(4ms);
01678 
01679     // done!
01680     return true;
01681 }
01682 
01683 #if USE_ASYNC_SPI
01684 
01685 void BNO080SPI::spiTransferAndWait(const uint8_t *tx_buffer, int tx_length, uint8_t *rx_buffer, int rx_length)
01686 {
01687     _spiPort.transfer(tx_buffer, tx_length, rx_buffer, rx_length,
01688                       callback(this, &BNO080SPI::onSPITransferComplete),
01689                       SPI_EVENT_COMPLETE | SPI_EVENT_ERROR);
01690 
01691     uint32_t waitResult = spiCompleteFlag.wait_any(SPI_EVENT_ALL);
01692     if(!(waitResult & SPI_EVENT_COMPLETE))
01693     {
01694         // at least let the user know the error happened...
01695         _debugPort->printf("BNO Async SPI Error %" PRIu32 "\n", waitResult);
01696     }
01697 
01698 }
01699 
01700 void BNO080SPI::onSPITransferComplete(int event)
01701 {
01702     spiCompleteFlag.set(event);
01703 }
01704 
01705 #endif