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