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@9:430f5302f9e1, 2020-11-24 (annotated)
- Committer:
- Jamie Smith
- Date:
- Tue Nov 24 15:06:05 2020 -0800
- Revision:
- 9:430f5302f9e1
- Parent:
- 8:199c7fad233d
Implement BNO080Async
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 |
3:197ad972fb7c | 56 | * -- AFAIK the only report ID on this channel will be 0xFB (Report Base Timestamp); sensor data is sent in a series of structures |
Jamie Smith |
1:aac28ffd63ed | 57 | * following an 0xFB |
Jamie Smith |
1:aac28ffd63ed | 58 | * |
Jamie Smith |
1:aac28ffd63ed | 59 | * 4 -> Wake Sensor Reports |
Jamie Smith |
1:aac28ffd63ed | 60 | * -- same as above, but for sensors configured to wake the device |
Jamie Smith |
1:aac28ffd63ed | 61 | * |
Jamie Smith |
1:aac28ffd63ed | 62 | * 5 -> Gyro Rotation Vector |
Jamie Smith |
1:aac28ffd63ed | 63 | * -- a dedicated channel for the Gyro Rotation Vector sensor report |
Jamie Smith |
1:aac28ffd63ed | 64 | * -- Why does this get its own channel? I don't know!!! |
Jamie Smith |
1:aac28ffd63ed | 65 | */ |
Jamie Smith |
1:aac28ffd63ed | 66 | |
Jamie Smith |
1:aac28ffd63ed | 67 | #include "BNO080.h" |
Jamie Smith |
1:aac28ffd63ed | 68 | #include "BNO080Constants.h" |
Jamie Smith |
8:199c7fad233d | 69 | #include <cinttypes> |
Jamie Smith |
8:199c7fad233d | 70 | #include <algorithm> |
Jamie Smith |
1:aac28ffd63ed | 71 | |
Jamie Smith |
1:aac28ffd63ed | 72 | /// Set to 1 to enable debug printouts. Should be very useful if the chip is giving you trouble. |
Jamie Smith |
1:aac28ffd63ed | 73 | /// 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 | 74 | #define BNO_DEBUG 0 |
Jamie Smith |
1:aac28ffd63ed | 75 | |
Jamie Smith |
8:199c7fad233d | 76 | BNO080Base::BNO080Base(Stream *debugPort, PinName user_INTPin, PinName user_RSTPin) : |
Jamie Smith |
1:aac28ffd63ed | 77 | _debugPort(debugPort), |
Jamie Smith |
1:aac28ffd63ed | 78 | _int(user_INTPin), |
Jamie Smith |
2:2269b723d16a | 79 | _rst(user_RSTPin, 1), |
Jamie Smith |
2:2269b723d16a | 80 | commandSequenceNumber(0), |
Jamie Smith |
2:2269b723d16a | 81 | stability(UNKNOWN), |
Jamie Smith |
2:2269b723d16a | 82 | stepDetected(false), |
Jamie Smith |
2:2269b723d16a | 83 | stepCount(0), |
Jamie Smith |
2:2269b723d16a | 84 | significantMotionDetected(false), |
Jamie Smith |
2:2269b723d16a | 85 | shakeDetected(false), |
Jamie Smith |
2:2269b723d16a | 86 | xAxisShake(false), |
Jamie Smith |
2:2269b723d16a | 87 | yAxisShake(false), |
Jamie Smith |
2:2269b723d16a | 88 | zAxisShake(false) |
Jamie Smith |
1:aac28ffd63ed | 89 | { |
Jamie Smith |
2:2269b723d16a | 90 | // zero sequence numbers |
Jamie Smith |
2:2269b723d16a | 91 | memset(sequenceNumber, 0, sizeof(sequenceNumber)); |
Jamie Smith |
1:aac28ffd63ed | 92 | } |
Jamie Smith |
1:aac28ffd63ed | 93 | |
Jamie Smith |
8:199c7fad233d | 94 | bool BNO080Base::begin() |
Jamie Smith |
1:aac28ffd63ed | 95 | { |
Jamie Smith |
9:430f5302f9e1 | 96 | //Configure the BNO080 |
Jamie Smith |
1:aac28ffd63ed | 97 | |
Jamie Smith |
1:aac28ffd63ed | 98 | _rst = 0; // Reset BNO080 |
Jamie Smith |
6:5ba996be5312 | 99 | ThisThread::sleep_for(1ms); // Min length not specified in datasheet? |
Jamie Smith |
1:aac28ffd63ed | 100 | _rst = 1; // Bring out of reset |
Jamie Smith |
1:aac28ffd63ed | 101 | |
Jamie Smith |
1:aac28ffd63ed | 102 | // wait for a falling edge (NOT just a low) on the INT pin to denote startup |
Jamie Smith |
1:aac28ffd63ed | 103 | Timer timeoutTimer; |
Jamie Smith |
3:197ad972fb7c | 104 | timeoutTimer.start(); |
Jamie Smith |
1:aac28ffd63ed | 105 | |
Jamie Smith |
1:aac28ffd63ed | 106 | bool highDetected = false; |
Jamie Smith |
1:aac28ffd63ed | 107 | bool lowDetected = false; |
Jamie Smith |
1:aac28ffd63ed | 108 | |
Jamie Smith |
1:aac28ffd63ed | 109 | while(true) |
Jamie Smith |
1:aac28ffd63ed | 110 | { |
Jamie Smith |
8:199c7fad233d | 111 | if(timeoutTimer.elapsed_time() > BNO080_RESET_TIMEOUT) |
Jamie Smith |
1:aac28ffd63ed | 112 | { |
Jamie Smith |
1:aac28ffd63ed | 113 | _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n"); |
Jamie Smith |
1:aac28ffd63ed | 114 | return false; |
Jamie Smith |
1:aac28ffd63ed | 115 | } |
Jamie Smith |
1:aac28ffd63ed | 116 | |
Jamie Smith |
1:aac28ffd63ed | 117 | // simple edge detector |
Jamie Smith |
1:aac28ffd63ed | 118 | if(!highDetected) |
Jamie Smith |
1:aac28ffd63ed | 119 | { |
Jamie Smith |
1:aac28ffd63ed | 120 | if(_int == 1) |
Jamie Smith |
1:aac28ffd63ed | 121 | { |
Jamie Smith |
1:aac28ffd63ed | 122 | highDetected = true; |
Jamie Smith |
1:aac28ffd63ed | 123 | } |
Jamie Smith |
1:aac28ffd63ed | 124 | } |
Jamie Smith |
1:aac28ffd63ed | 125 | else if(!lowDetected) |
Jamie Smith |
1:aac28ffd63ed | 126 | { |
Jamie Smith |
1:aac28ffd63ed | 127 | if(_int == 0) |
Jamie Smith |
1:aac28ffd63ed | 128 | { |
Jamie Smith |
1:aac28ffd63ed | 129 | lowDetected = true; |
Jamie Smith |
1:aac28ffd63ed | 130 | } |
Jamie Smith |
1:aac28ffd63ed | 131 | } |
Jamie Smith |
1:aac28ffd63ed | 132 | else |
Jamie Smith |
1:aac28ffd63ed | 133 | { |
Jamie Smith |
1:aac28ffd63ed | 134 | // high and low detected |
Jamie Smith |
1:aac28ffd63ed | 135 | break; |
Jamie Smith |
1:aac28ffd63ed | 136 | } |
Jamie Smith |
1:aac28ffd63ed | 137 | } |
Jamie Smith |
1:aac28ffd63ed | 138 | |
Jamie Smith |
3:197ad972fb7c | 139 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 140 | _debugPort->printf("BNO080 detected!\r\n"); |
Jamie Smith |
3:197ad972fb7c | 141 | #endif |
Jamie Smith |
1:aac28ffd63ed | 142 | |
Jamie Smith |
1:aac28ffd63ed | 143 | // At system startup, the hub must send its full advertisement message (see SHTP 5.2 and 5.3) to the |
Jamie Smith |
1:aac28ffd63ed | 144 | // host. It must not send any other data until this step is complete. |
Jamie Smith |
1:aac28ffd63ed | 145 | // 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 | 146 | receivePacket(); |
Jamie Smith |
1:aac28ffd63ed | 147 | |
Jamie Smith |
1:aac28ffd63ed | 148 | // 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 |
8:199c7fad233d | 149 | if(!waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET)) |
Jamie Smith |
1:aac28ffd63ed | 150 | { |
Jamie Smith |
8:199c7fad233d | 151 | _debugPort->printf("No initialization report from BNO080.\n"); |
Jamie Smith |
1:aac28ffd63ed | 152 | return false; |
Jamie Smith |
1:aac28ffd63ed | 153 | } |
Jamie Smith |
1:aac28ffd63ed | 154 | else |
Jamie Smith |
1:aac28ffd63ed | 155 | { |
Jamie Smith |
1:aac28ffd63ed | 156 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 157 | _debugPort->printf("BNO080 reports initialization successful!\n"); |
Jamie Smith |
1:aac28ffd63ed | 158 | #endif |
Jamie Smith |
1:aac28ffd63ed | 159 | } |
Jamie Smith |
1:aac28ffd63ed | 160 | |
Jamie Smith |
1:aac28ffd63ed | 161 | // Finally, we want to interrogate the device about its model and version. |
Jamie Smith |
9:430f5302f9e1 | 162 | clearSendBuffer(); |
Jamie Smith |
8:199c7fad233d | 163 | txShtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info |
Jamie Smith |
8:199c7fad233d | 164 | txShtpData[1] = 0; //Reserved |
Jamie Smith |
1:aac28ffd63ed | 165 | sendPacket(CHANNEL_CONTROL, 2); |
Jamie Smith |
1:aac28ffd63ed | 166 | |
Jamie Smith |
8:199c7fad233d | 167 | waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5ms); |
Jamie Smith |
1:aac28ffd63ed | 168 | |
Jamie Smith |
8:199c7fad233d | 169 | if (rxShtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) |
Jamie Smith |
1:aac28ffd63ed | 170 | { |
Jamie Smith |
8:199c7fad233d | 171 | majorSoftwareVersion = rxShtpData[2]; |
Jamie Smith |
8:199c7fad233d | 172 | minorSoftwareVersion = rxShtpData[3]; |
Jamie Smith |
8:199c7fad233d | 173 | patchSoftwareVersion = (rxShtpData[13] << 8) | rxShtpData[12]; |
Jamie Smith |
8:199c7fad233d | 174 | partNumber = (rxShtpData[7] << 24) | (rxShtpData[6] << 16) | (rxShtpData[5] << 8) | rxShtpData[4]; |
Jamie Smith |
8:199c7fad233d | 175 | buildNumber = (rxShtpData[11] << 24) | (rxShtpData[10] << 16) | (rxShtpData[9] << 8) | rxShtpData[8]; |
Jamie Smith |
1:aac28ffd63ed | 176 | |
Jamie Smith |
1:aac28ffd63ed | 177 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 178 | _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n", |
Jamie Smith |
1:aac28ffd63ed | 179 | majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion, |
Jamie Smith |
1:aac28ffd63ed | 180 | buildNumber, partNumber); |
Jamie Smith |
1:aac28ffd63ed | 181 | #endif |
Jamie Smith |
1:aac28ffd63ed | 182 | |
Jamie Smith |
1:aac28ffd63ed | 183 | } |
Jamie Smith |
1:aac28ffd63ed | 184 | else |
Jamie Smith |
1:aac28ffd63ed | 185 | { |
Jamie Smith |
1:aac28ffd63ed | 186 | _debugPort->printf("Bad response from product ID command.\n"); |
Jamie Smith |
1:aac28ffd63ed | 187 | return false; |
Jamie Smith |
1:aac28ffd63ed | 188 | } |
Jamie Smith |
1:aac28ffd63ed | 189 | |
Jamie Smith |
1:aac28ffd63ed | 190 | // successful init |
Jamie Smith |
1:aac28ffd63ed | 191 | return true; |
Jamie Smith |
1:aac28ffd63ed | 192 | |
Jamie Smith |
1:aac28ffd63ed | 193 | } |
Jamie Smith |
1:aac28ffd63ed | 194 | |
Jamie Smith |
8:199c7fad233d | 195 | void BNO080Base::tare(bool zOnly) |
Jamie Smith |
1:aac28ffd63ed | 196 | { |
Jamie Smith |
9:430f5302f9e1 | 197 | clearSendBuffer(); |
Jamie Smith |
1:aac28ffd63ed | 198 | |
Jamie Smith |
1:aac28ffd63ed | 199 | // from SH-2 section 6.4.4.1 |
Jamie Smith |
8:199c7fad233d | 200 | txShtpData[3] = 0; // perform tare now |
Jamie Smith |
1:aac28ffd63ed | 201 | |
Jamie Smith |
1:aac28ffd63ed | 202 | if(zOnly) |
Jamie Smith |
1:aac28ffd63ed | 203 | { |
Jamie Smith |
8:199c7fad233d | 204 | txShtpData[4] = 0b100; // tare Z axis |
Jamie Smith |
1:aac28ffd63ed | 205 | } |
Jamie Smith |
1:aac28ffd63ed | 206 | else |
Jamie Smith |
1:aac28ffd63ed | 207 | { |
Jamie Smith |
8:199c7fad233d | 208 | txShtpData[4] = 0b111; // tare X, Y, and Z axes |
Jamie Smith |
1:aac28ffd63ed | 209 | } |
Jamie Smith |
1:aac28ffd63ed | 210 | |
Jamie Smith |
8:199c7fad233d | 211 | txShtpData[5] = 0; // reorient all motion outputs |
Jamie Smith |
1:aac28ffd63ed | 212 | |
Jamie Smith |
1:aac28ffd63ed | 213 | sendCommand(COMMAND_TARE); |
Jamie Smith |
1:aac28ffd63ed | 214 | } |
Jamie Smith |
1:aac28ffd63ed | 215 | |
Jamie Smith |
8:199c7fad233d | 216 | bool BNO080Base::enableCalibration(bool calibrateAccel, bool calibrateGyro, bool calibrateMag) |
Jamie Smith |
1:aac28ffd63ed | 217 | { |
Jamie Smith |
1:aac28ffd63ed | 218 | // send the Configure ME Calibration command |
Jamie Smith |
9:430f5302f9e1 | 219 | clearSendBuffer(); |
Jamie Smith |
1:aac28ffd63ed | 220 | |
Jamie Smith |
8:199c7fad233d | 221 | txShtpData[3] = static_cast<uint8_t>(calibrateAccel ? 1 : 0); |
Jamie Smith |
8:199c7fad233d | 222 | txShtpData[4] = static_cast<uint8_t>(calibrateGyro ? 1 : 0); |
Jamie Smith |
8:199c7fad233d | 223 | txShtpData[5] = static_cast<uint8_t>(calibrateMag ? 1 : 0); |
Jamie Smith |
1:aac28ffd63ed | 224 | |
Jamie Smith |
8:199c7fad233d | 225 | txShtpData[6] = 0; // Configure ME Calibration command |
Jamie Smith |
1:aac28ffd63ed | 226 | |
Jamie Smith |
8:199c7fad233d | 227 | txShtpData[7] = 0; // planar accelerometer calibration always disabled |
Jamie Smith |
1:aac28ffd63ed | 228 | |
Jamie Smith |
1:aac28ffd63ed | 229 | sendCommand(COMMAND_ME_CALIBRATE); |
Jamie Smith |
1:aac28ffd63ed | 230 | |
Jamie Smith |
1:aac28ffd63ed | 231 | // now, wait for the response |
Jamie Smith |
1:aac28ffd63ed | 232 | if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE)) |
Jamie Smith |
1:aac28ffd63ed | 233 | { |
Jamie Smith |
1:aac28ffd63ed | 234 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 235 | _debugPort->printf("Timeout waiting for calibration response!\n"); |
Jamie Smith |
1:aac28ffd63ed | 236 | #endif |
Jamie Smith |
1:aac28ffd63ed | 237 | return false; |
Jamie Smith |
1:aac28ffd63ed | 238 | } |
Jamie Smith |
1:aac28ffd63ed | 239 | |
Jamie Smith |
8:199c7fad233d | 240 | if(rxShtpData[2] != COMMAND_ME_CALIBRATE) |
Jamie Smith |
1:aac28ffd63ed | 241 | { |
Jamie Smith |
1:aac28ffd63ed | 242 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 243 | _debugPort->printf("Received wrong response to calibration command!\n"); |
Jamie Smith |
1:aac28ffd63ed | 244 | #endif |
Jamie Smith |
1:aac28ffd63ed | 245 | return false; |
Jamie Smith |
1:aac28ffd63ed | 246 | } |
Jamie Smith |
1:aac28ffd63ed | 247 | |
Jamie Smith |
8:199c7fad233d | 248 | if(rxShtpData[5] != 0) |
Jamie Smith |
1:aac28ffd63ed | 249 | { |
Jamie Smith |
1:aac28ffd63ed | 250 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 251 | _debugPort->printf("IMU reports calibrate command failed!\n"); |
Jamie Smith |
1:aac28ffd63ed | 252 | #endif |
Jamie Smith |
1:aac28ffd63ed | 253 | return false; |
Jamie Smith |
1:aac28ffd63ed | 254 | } |
Jamie Smith |
1:aac28ffd63ed | 255 | |
Jamie Smith |
1:aac28ffd63ed | 256 | // acknowledge checks out! |
Jamie Smith |
1:aac28ffd63ed | 257 | return true; |
Jamie Smith |
1:aac28ffd63ed | 258 | } |
Jamie Smith |
1:aac28ffd63ed | 259 | |
Jamie Smith |
8:199c7fad233d | 260 | bool BNO080Base::saveCalibration() |
Jamie Smith |
1:aac28ffd63ed | 261 | { |
Jamie Smith |
9:430f5302f9e1 | 262 | clearSendBuffer(); |
Jamie Smith |
1:aac28ffd63ed | 263 | |
Jamie Smith |
1:aac28ffd63ed | 264 | // no arguments |
Jamie Smith |
1:aac28ffd63ed | 265 | sendCommand(COMMAND_SAVE_DCD); |
Jamie Smith |
1:aac28ffd63ed | 266 | |
Jamie Smith |
1:aac28ffd63ed | 267 | // now, wait for the response |
Jamie Smith |
1:aac28ffd63ed | 268 | if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE)) |
Jamie Smith |
1:aac28ffd63ed | 269 | { |
Jamie Smith |
1:aac28ffd63ed | 270 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 271 | _debugPort->printf("Timeout waiting for calibration response!\n"); |
Jamie Smith |
1:aac28ffd63ed | 272 | #endif |
Jamie Smith |
1:aac28ffd63ed | 273 | return false; |
Jamie Smith |
1:aac28ffd63ed | 274 | } |
Jamie Smith |
1:aac28ffd63ed | 275 | |
Jamie Smith |
8:199c7fad233d | 276 | if(rxShtpData[2] != COMMAND_SAVE_DCD) |
Jamie Smith |
1:aac28ffd63ed | 277 | { |
Jamie Smith |
1:aac28ffd63ed | 278 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 279 | _debugPort->printf("Received wrong response to calibration command!\n"); |
Jamie Smith |
1:aac28ffd63ed | 280 | #endif |
Jamie Smith |
1:aac28ffd63ed | 281 | return false; |
Jamie Smith |
1:aac28ffd63ed | 282 | } |
Jamie Smith |
1:aac28ffd63ed | 283 | |
Jamie Smith |
8:199c7fad233d | 284 | if(rxShtpData[5] != 0) |
Jamie Smith |
1:aac28ffd63ed | 285 | { |
Jamie Smith |
1:aac28ffd63ed | 286 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 287 | _debugPort->printf("IMU reports calibrate command failed!\n"); |
Jamie Smith |
1:aac28ffd63ed | 288 | #endif |
Jamie Smith |
1:aac28ffd63ed | 289 | return false; |
Jamie Smith |
1:aac28ffd63ed | 290 | } |
Jamie Smith |
1:aac28ffd63ed | 291 | |
Jamie Smith |
1:aac28ffd63ed | 292 | // acknowledge checks out! |
Jamie Smith |
1:aac28ffd63ed | 293 | return true; |
Jamie Smith |
1:aac28ffd63ed | 294 | } |
Jamie Smith |
1:aac28ffd63ed | 295 | |
Jamie Smith |
8:199c7fad233d | 296 | void BNO080Base::setSensorOrientation(Quaternion orientation) |
Jamie Smith |
1:aac28ffd63ed | 297 | { |
Jamie Smith |
9:430f5302f9e1 | 298 | clearSendBuffer(); |
Jamie Smith |
1:aac28ffd63ed | 299 | |
Jamie Smith |
1:aac28ffd63ed | 300 | // convert floats to Q |
Jamie Smith |
1:aac28ffd63ed | 301 | int16_t Q_x = floatToQ(orientation.x(), ORIENTATION_QUAT_Q_POINT); |
Jamie Smith |
1:aac28ffd63ed | 302 | int16_t Q_y = floatToQ(orientation.y(), ORIENTATION_QUAT_Q_POINT); |
Jamie Smith |
1:aac28ffd63ed | 303 | int16_t Q_z = floatToQ(orientation.z(), ORIENTATION_QUAT_Q_POINT); |
Jamie Smith |
1:aac28ffd63ed | 304 | int16_t Q_w = floatToQ(orientation.w(), ORIENTATION_QUAT_Q_POINT); |
Jamie Smith |
1:aac28ffd63ed | 305 | |
Jamie Smith |
8:199c7fad233d | 306 | txShtpData[3] = 2; // set reorientation |
Jamie Smith |
1:aac28ffd63ed | 307 | |
Jamie Smith |
8:199c7fad233d | 308 | txShtpData[4] = static_cast<uint8_t>(Q_x & 0xFF); //P1 - X component LSB |
Jamie Smith |
8:199c7fad233d | 309 | txShtpData[5] = static_cast<uint8_t>(Q_x >> 8); //P2 - X component MSB |
Jamie Smith |
1:aac28ffd63ed | 310 | |
Jamie Smith |
8:199c7fad233d | 311 | txShtpData[6] = static_cast<uint8_t>(Q_y & 0xFF); //P3 - Y component LSB |
Jamie Smith |
8:199c7fad233d | 312 | txShtpData[7] = static_cast<uint8_t>(Q_y >> 8); //P4 - Y component MSB |
Jamie Smith |
1:aac28ffd63ed | 313 | |
Jamie Smith |
8:199c7fad233d | 314 | txShtpData[8] = static_cast<uint8_t>(Q_z & 0xFF); //P5 - Z component LSB |
Jamie Smith |
8:199c7fad233d | 315 | txShtpData[9] = static_cast<uint8_t>(Q_z >> 8); //P6 - Z component MSB |
Jamie Smith |
1:aac28ffd63ed | 316 | |
Jamie Smith |
8:199c7fad233d | 317 | txShtpData[10] = static_cast<uint8_t>(Q_w & 0xFF); //P7 - W component LSB |
Jamie Smith |
8:199c7fad233d | 318 | txShtpData[11] = static_cast<uint8_t>(Q_w >> 8); //P8 - W component MSB |
Jamie Smith |
1:aac28ffd63ed | 319 | |
Jamie Smith |
1:aac28ffd63ed | 320 | //Using this shtpData packet, send a command |
Jamie Smith |
1:aac28ffd63ed | 321 | sendCommand(COMMAND_TARE); // Send tare command |
Jamie Smith |
1:aac28ffd63ed | 322 | |
Jamie Smith |
1:aac28ffd63ed | 323 | // NOTE: unlike literally every other command, a sensor orientation command is never acknowledged in any way. |
Jamie Smith |
1:aac28ffd63ed | 324 | } |
Jamie Smith |
1:aac28ffd63ed | 325 | |
Jamie Smith |
3:197ad972fb7c | 326 | #define ORIENTATION_RECORD_LEN 4 |
Jamie Smith |
3:197ad972fb7c | 327 | |
Jamie Smith |
8:199c7fad233d | 328 | bool BNO080Base::setPermanentOrientation(Quaternion orientation) |
Jamie Smith |
3:197ad972fb7c | 329 | { |
Jamie Smith |
3:197ad972fb7c | 330 | uint32_t orientationRecord[ORIENTATION_RECORD_LEN]; |
Jamie Smith |
3:197ad972fb7c | 331 | |
Jamie Smith |
3:197ad972fb7c | 332 | // each word is one element of the quaternion |
Jamie Smith |
3:197ad972fb7c | 333 | orientationRecord[0] = static_cast<uint32_t>(floatToQ_dword(orientation.x(), FRS_ORIENTATION_Q_POINT)); |
Jamie Smith |
3:197ad972fb7c | 334 | orientationRecord[1] = static_cast<uint32_t>(floatToQ_dword(orientation.y(), FRS_ORIENTATION_Q_POINT)); |
Jamie Smith |
3:197ad972fb7c | 335 | orientationRecord[2] = static_cast<uint32_t>(floatToQ_dword(orientation.z(), FRS_ORIENTATION_Q_POINT)); |
Jamie Smith |
3:197ad972fb7c | 336 | orientationRecord[3] = static_cast<uint32_t>(floatToQ_dword(orientation.w(), FRS_ORIENTATION_Q_POINT)); |
Jamie Smith |
3:197ad972fb7c | 337 | |
Jamie Smith |
3:197ad972fb7c | 338 | return writeFRSRecord(FRS_RECORDID_SYSTEM_ORIENTATION, orientationRecord, ORIENTATION_RECORD_LEN); |
Jamie Smith |
3:197ad972fb7c | 339 | } |
Jamie Smith |
1:aac28ffd63ed | 340 | |
Jamie Smith |
8:199c7fad233d | 341 | bool BNO080Base::updateData() |
Jamie Smith |
1:aac28ffd63ed | 342 | { |
Jamie Smith |
1:aac28ffd63ed | 343 | if(_int.read() != 0) |
Jamie Smith |
1:aac28ffd63ed | 344 | { |
Jamie Smith |
1:aac28ffd63ed | 345 | // no waiting packets |
Jamie Smith |
1:aac28ffd63ed | 346 | return false; |
Jamie Smith |
1:aac28ffd63ed | 347 | } |
Jamie Smith |
1:aac28ffd63ed | 348 | |
Jamie Smith |
1:aac28ffd63ed | 349 | while(_int.read() == 0) |
Jamie Smith |
1:aac28ffd63ed | 350 | { |
Jamie Smith |
1:aac28ffd63ed | 351 | if(!receivePacket()) |
Jamie Smith |
1:aac28ffd63ed | 352 | { |
Jamie Smith |
1:aac28ffd63ed | 353 | // comms error |
Jamie Smith |
1:aac28ffd63ed | 354 | return false; |
Jamie Smith |
1:aac28ffd63ed | 355 | } |
Jamie Smith |
1:aac28ffd63ed | 356 | |
Jamie Smith |
1:aac28ffd63ed | 357 | processPacket(); |
Jamie Smith |
8:199c7fad233d | 358 | |
Jamie Smith |
8:199c7fad233d | 359 | // Allow time for the IMU to ready another packet if it has one |
Jamie Smith |
8:199c7fad233d | 360 | wait_us(150); // time between packets measured with a logic analyzer |
Jamie Smith |
1:aac28ffd63ed | 361 | } |
Jamie Smith |
1:aac28ffd63ed | 362 | |
Jamie Smith |
1:aac28ffd63ed | 363 | // packets were received, so data may have changed |
Jamie Smith |
1:aac28ffd63ed | 364 | return true; |
Jamie Smith |
1:aac28ffd63ed | 365 | } |
Jamie Smith |
1:aac28ffd63ed | 366 | |
Jamie Smith |
8:199c7fad233d | 367 | uint8_t BNO080Base::getReportStatus(Report report) |
Jamie Smith |
1:aac28ffd63ed | 368 | { |
Jamie Smith |
1:aac28ffd63ed | 369 | uint8_t reportNum = static_cast<uint8_t>(report); |
Jamie Smith |
1:aac28ffd63ed | 370 | if(reportNum > STATUS_ARRAY_LEN) |
Jamie Smith |
1:aac28ffd63ed | 371 | { |
Jamie Smith |
1:aac28ffd63ed | 372 | return 0; |
Jamie Smith |
1:aac28ffd63ed | 373 | } |
Jamie Smith |
1:aac28ffd63ed | 374 | |
Jamie Smith |
1:aac28ffd63ed | 375 | return reportStatus[reportNum]; |
Jamie Smith |
1:aac28ffd63ed | 376 | } |
Jamie Smith |
1:aac28ffd63ed | 377 | |
Jamie Smith |
8:199c7fad233d | 378 | const char* BNO080Base::getReportStatusString(Report report) |
Jamie Smith |
1:aac28ffd63ed | 379 | { |
Jamie Smith |
1:aac28ffd63ed | 380 | switch(getReportStatus(report)) |
Jamie Smith |
1:aac28ffd63ed | 381 | { |
Jamie Smith |
1:aac28ffd63ed | 382 | case 0: |
Jamie Smith |
1:aac28ffd63ed | 383 | return "Unreliable"; |
Jamie Smith |
1:aac28ffd63ed | 384 | case 1: |
Jamie Smith |
1:aac28ffd63ed | 385 | return "Accuracy Low"; |
Jamie Smith |
1:aac28ffd63ed | 386 | case 2: |
Jamie Smith |
1:aac28ffd63ed | 387 | return "Accuracy Medium"; |
Jamie Smith |
1:aac28ffd63ed | 388 | case 3: |
Jamie Smith |
1:aac28ffd63ed | 389 | return "Accuracy High"; |
Jamie Smith |
1:aac28ffd63ed | 390 | default: |
Jamie Smith |
1:aac28ffd63ed | 391 | return "Error"; |
Jamie Smith |
1:aac28ffd63ed | 392 | } |
Jamie Smith |
1:aac28ffd63ed | 393 | } |
Jamie Smith |
1:aac28ffd63ed | 394 | |
Jamie Smith |
8:199c7fad233d | 395 | bool BNO080Base::hasNewData(Report report) |
Jamie Smith |
1:aac28ffd63ed | 396 | { |
Jamie Smith |
1:aac28ffd63ed | 397 | uint8_t reportNum = static_cast<uint8_t>(report); |
Jamie Smith |
1:aac28ffd63ed | 398 | if(reportNum > STATUS_ARRAY_LEN) |
Jamie Smith |
1:aac28ffd63ed | 399 | { |
Jamie Smith |
1:aac28ffd63ed | 400 | return false; |
Jamie Smith |
1:aac28ffd63ed | 401 | } |
Jamie Smith |
1:aac28ffd63ed | 402 | |
Jamie Smith |
1:aac28ffd63ed | 403 | bool newData = reportHasBeenUpdated[reportNum]; |
Jamie Smith |
1:aac28ffd63ed | 404 | reportHasBeenUpdated[reportNum] = false; // clear flag |
Jamie Smith |
1:aac28ffd63ed | 405 | return newData; |
Jamie Smith |
1:aac28ffd63ed | 406 | } |
Jamie Smith |
1:aac28ffd63ed | 407 | |
Jamie Smith |
1:aac28ffd63ed | 408 | //Sends the packet to enable the rotation vector |
Jamie Smith |
8:199c7fad233d | 409 | void BNO080Base::enableReport(Report report, uint16_t timeBetweenReports) |
Jamie Smith |
1:aac28ffd63ed | 410 | { |
Jamie Smith |
3:197ad972fb7c | 411 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 412 | // check time is valid |
Jamie Smith |
3:197ad972fb7c | 413 | float periodSeconds = static_cast<float>(timeBetweenReports / 1000.0); |
Jamie Smith |
1:aac28ffd63ed | 414 | |
Jamie Smith |
1:aac28ffd63ed | 415 | if(periodSeconds < getMinPeriod(report)) |
Jamie Smith |
1:aac28ffd63ed | 416 | { |
Jamie Smith |
3:197ad972fb7c | 417 | _debugPort->printf("Error: attempt made to set report 0x%02hhx to period of %.06f s, which is smaller than its min period of %.06f s.\r\n", |
Jamie Smith |
1:aac28ffd63ed | 418 | static_cast<uint8_t>(report), periodSeconds, getMinPeriod(report)); |
Jamie Smith |
1:aac28ffd63ed | 419 | return; |
Jamie Smith |
1:aac28ffd63ed | 420 | } |
Jamie Smith |
8:199c7fad233d | 421 | |
Jamie Smith |
3:197ad972fb7c | 422 | #endif |
Jamie Smith |
1:aac28ffd63ed | 423 | setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports); |
Jamie Smith |
1:aac28ffd63ed | 424 | |
Jamie Smith |
1:aac28ffd63ed | 425 | // 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 | 426 | } |
Jamie Smith |
1:aac28ffd63ed | 427 | |
Jamie Smith |
8:199c7fad233d | 428 | void BNO080Base::disableReport(Report report) |
Jamie Smith |
1:aac28ffd63ed | 429 | { |
Jamie Smith |
1:aac28ffd63ed | 430 | // set the report's polling period to zero to disable it |
Jamie Smith |
1:aac28ffd63ed | 431 | setFeatureCommand(static_cast<uint8_t>(report), 0); |
Jamie Smith |
1:aac28ffd63ed | 432 | } |
Jamie Smith |
1:aac28ffd63ed | 433 | |
Jamie Smith |
8:199c7fad233d | 434 | uint32_t BNO080Base::getSerialNumber() |
Jamie Smith |
1:aac28ffd63ed | 435 | { |
Jamie Smith |
1:aac28ffd63ed | 436 | uint32_t serNoBuffer; |
Jamie Smith |
1:aac28ffd63ed | 437 | |
Jamie Smith |
1:aac28ffd63ed | 438 | if(!readFRSRecord(FRS_RECORDID_SERIAL_NUMBER, &serNoBuffer, 1)) |
Jamie Smith |
1:aac28ffd63ed | 439 | { |
Jamie Smith |
1:aac28ffd63ed | 440 | return 0; |
Jamie Smith |
1:aac28ffd63ed | 441 | } |
Jamie Smith |
1:aac28ffd63ed | 442 | |
Jamie Smith |
1:aac28ffd63ed | 443 | return serNoBuffer; |
Jamie Smith |
1:aac28ffd63ed | 444 | } |
Jamie Smith |
1:aac28ffd63ed | 445 | |
Jamie Smith |
8:199c7fad233d | 446 | float BNO080Base::getRange(Report report) |
Jamie Smith |
1:aac28ffd63ed | 447 | { |
Jamie Smith |
1:aac28ffd63ed | 448 | loadReportMetadata(report); |
Jamie Smith |
1:aac28ffd63ed | 449 | |
Jamie Smith |
1:aac28ffd63ed | 450 | return qToFloat_dword(metadataRecord[1], getQ1(report)); |
Jamie Smith |
1:aac28ffd63ed | 451 | } |
Jamie Smith |
1:aac28ffd63ed | 452 | |
Jamie Smith |
1:aac28ffd63ed | 453 | |
Jamie Smith |
8:199c7fad233d | 454 | float BNO080Base::getResolution(Report report) |
Jamie Smith |
1:aac28ffd63ed | 455 | { |
Jamie Smith |
1:aac28ffd63ed | 456 | loadReportMetadata(report); |
Jamie Smith |
1:aac28ffd63ed | 457 | |
Jamie Smith |
1:aac28ffd63ed | 458 | return qToFloat_dword(metadataRecord[2], getQ1(report)); |
Jamie Smith |
1:aac28ffd63ed | 459 | } |
Jamie Smith |
1:aac28ffd63ed | 460 | |
Jamie Smith |
8:199c7fad233d | 461 | float BNO080Base::getPower(Report report) |
Jamie Smith |
1:aac28ffd63ed | 462 | { |
Jamie Smith |
1:aac28ffd63ed | 463 | loadReportMetadata(report); |
Jamie Smith |
1:aac28ffd63ed | 464 | |
Jamie Smith |
1:aac28ffd63ed | 465 | uint16_t powerQ = static_cast<uint16_t>(metadataRecord[3] & 0xFFFF); |
Jamie Smith |
1:aac28ffd63ed | 466 | |
Jamie Smith |
1:aac28ffd63ed | 467 | return qToFloat_dword(powerQ, POWER_Q_POINT); |
Jamie Smith |
1:aac28ffd63ed | 468 | } |
Jamie Smith |
1:aac28ffd63ed | 469 | |
Jamie Smith |
8:199c7fad233d | 470 | float BNO080Base::getMinPeriod(Report report) |
Jamie Smith |
1:aac28ffd63ed | 471 | { |
Jamie Smith |
1:aac28ffd63ed | 472 | loadReportMetadata(report); |
Jamie Smith |
1:aac28ffd63ed | 473 | |
Jamie Smith |
1:aac28ffd63ed | 474 | return metadataRecord[4] / 1e6f; // convert from microseconds to seconds |
Jamie Smith |
1:aac28ffd63ed | 475 | } |
Jamie Smith |
1:aac28ffd63ed | 476 | |
Jamie Smith |
8:199c7fad233d | 477 | float BNO080Base::getMaxPeriod(Report report) |
Jamie Smith |
1:aac28ffd63ed | 478 | { |
Jamie Smith |
1:aac28ffd63ed | 479 | loadReportMetadata(report); |
Jamie Smith |
1:aac28ffd63ed | 480 | |
Jamie Smith |
1:aac28ffd63ed | 481 | if(getMetaVersion() == 3) |
Jamie Smith |
1:aac28ffd63ed | 482 | { |
Jamie Smith |
1:aac28ffd63ed | 483 | // no max period entry in this record format |
Jamie Smith |
1:aac28ffd63ed | 484 | return -1.0f; |
Jamie Smith |
1:aac28ffd63ed | 485 | } |
Jamie Smith |
1:aac28ffd63ed | 486 | |
Jamie Smith |
1:aac28ffd63ed | 487 | return metadataRecord[9] / 1e6f; // convert from microseconds to seconds |
Jamie Smith |
1:aac28ffd63ed | 488 | } |
Jamie Smith |
1:aac28ffd63ed | 489 | |
Jamie Smith |
8:199c7fad233d | 490 | void BNO080Base::printMetadataSummary(Report report) |
Jamie Smith |
1:aac28ffd63ed | 491 | { |
Jamie Smith |
1:aac28ffd63ed | 492 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 493 | if(!loadReportMetadata(report)) |
Jamie Smith |
1:aac28ffd63ed | 494 | { |
Jamie Smith |
1:aac28ffd63ed | 495 | _debugPort->printf("Failed to load report metadata!\n"); |
Jamie Smith |
1:aac28ffd63ed | 496 | } |
Jamie Smith |
1:aac28ffd63ed | 497 | |
Jamie Smith |
1:aac28ffd63ed | 498 | _debugPort->printf("======= Metadata for report 0x%02hhx =======\n", static_cast<uint8_t>(report)); |
Jamie Smith |
1:aac28ffd63ed | 499 | |
Jamie Smith |
1:aac28ffd63ed | 500 | _debugPort->printf("Range: +- %.04f units\n", getRange(report)); |
Jamie Smith |
1:aac28ffd63ed | 501 | _debugPort->printf("Resolution: %.04f units\n", getResolution(report)); |
Jamie Smith |
1:aac28ffd63ed | 502 | _debugPort->printf("Power Used: %.03f mA\n", getPower(report)); |
Jamie Smith |
1:aac28ffd63ed | 503 | _debugPort->printf("Min Period: %.06f s\n", getMinPeriod(report)); |
Jamie Smith |
1:aac28ffd63ed | 504 | _debugPort->printf("Max Period: %.06f s\n\n", getMaxPeriod(report)); |
Jamie Smith |
1:aac28ffd63ed | 505 | |
Jamie Smith |
1:aac28ffd63ed | 506 | #endif |
Jamie Smith |
1:aac28ffd63ed | 507 | } |
Jamie Smith |
1:aac28ffd63ed | 508 | |
Jamie Smith |
8:199c7fad233d | 509 | int16_t BNO080Base::getQ1(Report report) |
Jamie Smith |
1:aac28ffd63ed | 510 | { |
Jamie Smith |
1:aac28ffd63ed | 511 | loadReportMetadata(report); |
Jamie Smith |
1:aac28ffd63ed | 512 | |
Jamie Smith |
1:aac28ffd63ed | 513 | return static_cast<int16_t>(metadataRecord[7] & 0xFFFF); |
Jamie Smith |
1:aac28ffd63ed | 514 | } |
Jamie Smith |
1:aac28ffd63ed | 515 | |
Jamie Smith |
8:199c7fad233d | 516 | int16_t BNO080Base::getQ2(Report report) |
Jamie Smith |
1:aac28ffd63ed | 517 | { |
Jamie Smith |
1:aac28ffd63ed | 518 | loadReportMetadata(report); |
Jamie Smith |
1:aac28ffd63ed | 519 | |
Jamie Smith |
1:aac28ffd63ed | 520 | return static_cast<int16_t>(metadataRecord[7] >> 16); |
Jamie Smith |
1:aac28ffd63ed | 521 | } |
Jamie Smith |
1:aac28ffd63ed | 522 | |
Jamie Smith |
8:199c7fad233d | 523 | int16_t BNO080Base::getQ3(Report report) |
Jamie Smith |
1:aac28ffd63ed | 524 | { |
Jamie Smith |
1:aac28ffd63ed | 525 | loadReportMetadata(report); |
Jamie Smith |
1:aac28ffd63ed | 526 | |
Jamie Smith |
1:aac28ffd63ed | 527 | return static_cast<int16_t>(metadataRecord[8] >> 16); |
Jamie Smith |
1:aac28ffd63ed | 528 | } |
Jamie Smith |
1:aac28ffd63ed | 529 | |
Jamie Smith |
8:199c7fad233d | 530 | void BNO080Base::processPacket() |
Jamie Smith |
1:aac28ffd63ed | 531 | { |
Jamie Smith |
8:199c7fad233d | 532 | if(rxShtpHeader[2] == CHANNEL_CONTROL) |
Jamie Smith |
1:aac28ffd63ed | 533 | { |
Jamie Smith |
1:aac28ffd63ed | 534 | // currently no command reports are read |
Jamie Smith |
1:aac28ffd63ed | 535 | } |
Jamie Smith |
8:199c7fad233d | 536 | else if(rxShtpHeader[2] == CHANNEL_EXECUTABLE) |
Jamie Smith |
1:aac28ffd63ed | 537 | { |
Jamie Smith |
1:aac28ffd63ed | 538 | // currently no executable reports are read |
Jamie Smith |
1:aac28ffd63ed | 539 | } |
Jamie Smith |
8:199c7fad233d | 540 | else if(rxShtpHeader[2] == CHANNEL_COMMAND) |
Jamie Smith |
1:aac28ffd63ed | 541 | { |
Jamie Smith |
1:aac28ffd63ed | 542 | |
Jamie Smith |
1:aac28ffd63ed | 543 | } |
Jamie Smith |
8:199c7fad233d | 544 | else if(rxShtpHeader[2] == CHANNEL_REPORTS || rxShtpHeader[2] == CHANNEL_WAKE_REPORTS) |
Jamie Smith |
1:aac28ffd63ed | 545 | { |
Jamie Smith |
8:199c7fad233d | 546 | if(rxShtpData[0] == SHTP_REPORT_BASE_TIMESTAMP) |
Jamie Smith |
1:aac28ffd63ed | 547 | { |
Jamie Smith |
1:aac28ffd63ed | 548 | // sensor data packet |
Jamie Smith |
1:aac28ffd63ed | 549 | parseSensorDataPacket(); |
Jamie Smith |
1:aac28ffd63ed | 550 | } |
Jamie Smith |
1:aac28ffd63ed | 551 | } |
Jamie Smith |
1:aac28ffd63ed | 552 | } |
Jamie Smith |
1:aac28ffd63ed | 553 | |
Jamie Smith |
1:aac28ffd63ed | 554 | // sizes of various sensor data packet elements |
Jamie Smith |
1:aac28ffd63ed | 555 | #define SIZEOF_BASE_TIMESTAMP 5 |
Jamie Smith |
1:aac28ffd63ed | 556 | #define SIZEOF_TIMESTAMP_REBASE 5 |
Jamie Smith |
1:aac28ffd63ed | 557 | #define SIZEOF_ACCELEROMETER 10 |
Jamie Smith |
1:aac28ffd63ed | 558 | #define SIZEOF_LINEAR_ACCELERATION 10 |
Jamie Smith |
1:aac28ffd63ed | 559 | #define SIZEOF_GYROSCOPE_CALIBRATED 10 |
Jamie Smith |
1:aac28ffd63ed | 560 | #define SIZEOF_MAGNETIC_FIELD_CALIBRATED 10 |
Jamie Smith |
1:aac28ffd63ed | 561 | #define SIZEOF_MAGNETIC_FIELD_UNCALIBRATED 16 |
Jamie Smith |
1:aac28ffd63ed | 562 | #define SIZEOF_ROTATION_VECTOR 14 |
Jamie Smith |
1:aac28ffd63ed | 563 | #define SIZEOF_GAME_ROTATION_VECTOR 12 |
Jamie Smith |
1:aac28ffd63ed | 564 | #define SIZEOF_GEOMAGNETIC_ROTATION_VECTOR 14 |
Jamie Smith |
1:aac28ffd63ed | 565 | #define SIZEOF_TAP_DETECTOR 5 |
Jamie Smith |
1:aac28ffd63ed | 566 | #define SIZEOF_STABILITY_REPORT 6 |
Jamie Smith |
1:aac28ffd63ed | 567 | #define SIZEOF_STEP_DETECTOR 8 |
Jamie Smith |
1:aac28ffd63ed | 568 | #define SIZEOF_STEP_COUNTER 12 |
Jamie Smith |
1:aac28ffd63ed | 569 | #define SIZEOF_SIGNIFICANT_MOTION 6 |
Jamie Smith |
1:aac28ffd63ed | 570 | #define SIZEOF_SHAKE_DETECTOR 6 |
Jamie Smith |
1:aac28ffd63ed | 571 | |
Jamie Smith |
8:199c7fad233d | 572 | void BNO080Base::parseSensorDataPacket() |
Jamie Smith |
1:aac28ffd63ed | 573 | { |
Jamie Smith |
1:aac28ffd63ed | 574 | size_t currReportOffset = 0; |
Jamie Smith |
1:aac28ffd63ed | 575 | |
Jamie Smith |
1:aac28ffd63ed | 576 | // every sensor data report first contains a timestamp offset to show how long it has been between when |
Jamie Smith |
1:aac28ffd63ed | 577 | // the host interrupt was sent and when the packet was transmitted. |
Jamie Smith |
1:aac28ffd63ed | 578 | // We don't use interrupts and don't care about times, so we can throw this out. |
Jamie Smith |
1:aac28ffd63ed | 579 | currReportOffset += SIZEOF_BASE_TIMESTAMP; |
Jamie Smith |
1:aac28ffd63ed | 580 | |
Jamie Smith |
8:199c7fad233d | 581 | while(currReportOffset < rxPacketLength) |
Jamie Smith |
1:aac28ffd63ed | 582 | { |
Jamie Smith |
1:aac28ffd63ed | 583 | // lots of sensor reports use 3 16-bit numbers stored in bytes 4 through 9 |
Jamie Smith |
1:aac28ffd63ed | 584 | // we can save some time by parsing those out here. |
Jamie Smith |
8:199c7fad233d | 585 | uint16_t data1 = (uint16_t)rxShtpData[currReportOffset + 5] << 8 | rxShtpData[currReportOffset + 4]; |
Jamie Smith |
8:199c7fad233d | 586 | uint16_t data2 = (uint16_t)rxShtpData[currReportOffset + 7] << 8 | rxShtpData[currReportOffset + 6]; |
Jamie Smith |
8:199c7fad233d | 587 | uint16_t data3 = (uint16_t)rxShtpData[currReportOffset + 9] << 8 | rxShtpData[currReportOffset + 8]; |
Jamie Smith |
1:aac28ffd63ed | 588 | |
Jamie Smith |
8:199c7fad233d | 589 | uint8_t reportNum = rxShtpData[currReportOffset]; |
Jamie Smith |
1:aac28ffd63ed | 590 | |
Jamie Smith |
1:aac28ffd63ed | 591 | if(reportNum != SENSOR_REPORTID_TIMESTAMP_REBASE) |
Jamie Smith |
1:aac28ffd63ed | 592 | { |
Jamie Smith |
1:aac28ffd63ed | 593 | // set status from byte 2 |
Jamie Smith |
8:199c7fad233d | 594 | reportStatus[reportNum] = static_cast<uint8_t>(rxShtpData[currReportOffset + 2] & 0b11); |
Jamie Smith |
1:aac28ffd63ed | 595 | |
Jamie Smith |
1:aac28ffd63ed | 596 | // set updated flag |
Jamie Smith |
1:aac28ffd63ed | 597 | reportHasBeenUpdated[reportNum] = true; |
Jamie Smith |
1:aac28ffd63ed | 598 | } |
Jamie Smith |
1:aac28ffd63ed | 599 | |
Jamie Smith |
8:199c7fad233d | 600 | switch(rxShtpData[currReportOffset]) |
Jamie Smith |
1:aac28ffd63ed | 601 | { |
Jamie Smith |
1:aac28ffd63ed | 602 | case SENSOR_REPORTID_TIMESTAMP_REBASE: |
Jamie Smith |
1:aac28ffd63ed | 603 | currReportOffset += SIZEOF_TIMESTAMP_REBASE; |
Jamie Smith |
1:aac28ffd63ed | 604 | break; |
Jamie Smith |
1:aac28ffd63ed | 605 | |
Jamie Smith |
1:aac28ffd63ed | 606 | case SENSOR_REPORTID_ACCELEROMETER: |
Jamie Smith |
1:aac28ffd63ed | 607 | |
Jamie Smith |
1:aac28ffd63ed | 608 | totalAcceleration = TVector3( |
Jamie Smith |
1:aac28ffd63ed | 609 | qToFloat(data1, ACCELEROMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 610 | qToFloat(data2, ACCELEROMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 611 | qToFloat(data3, ACCELEROMETER_Q_POINT)); |
Jamie Smith |
1:aac28ffd63ed | 612 | |
Jamie Smith |
1:aac28ffd63ed | 613 | currReportOffset += SIZEOF_ACCELEROMETER; |
Jamie Smith |
1:aac28ffd63ed | 614 | break; |
Jamie Smith |
1:aac28ffd63ed | 615 | |
Jamie Smith |
1:aac28ffd63ed | 616 | case SENSOR_REPORTID_LINEAR_ACCELERATION: |
Jamie Smith |
1:aac28ffd63ed | 617 | |
Jamie Smith |
1:aac28ffd63ed | 618 | linearAcceleration = TVector3( |
Jamie Smith |
1:aac28ffd63ed | 619 | qToFloat(data1, ACCELEROMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 620 | qToFloat(data2, ACCELEROMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 621 | qToFloat(data3, ACCELEROMETER_Q_POINT)); |
Jamie Smith |
1:aac28ffd63ed | 622 | |
Jamie Smith |
1:aac28ffd63ed | 623 | currReportOffset += SIZEOF_LINEAR_ACCELERATION; |
Jamie Smith |
1:aac28ffd63ed | 624 | break; |
Jamie Smith |
1:aac28ffd63ed | 625 | |
Jamie Smith |
1:aac28ffd63ed | 626 | case SENSOR_REPORTID_GRAVITY: |
Jamie Smith |
1:aac28ffd63ed | 627 | |
Jamie Smith |
1:aac28ffd63ed | 628 | gravityAcceleration = TVector3( |
Jamie Smith |
1:aac28ffd63ed | 629 | qToFloat(data1, ACCELEROMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 630 | qToFloat(data2, ACCELEROMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 631 | qToFloat(data3, ACCELEROMETER_Q_POINT)); |
Jamie Smith |
1:aac28ffd63ed | 632 | |
Jamie Smith |
1:aac28ffd63ed | 633 | currReportOffset += SIZEOF_LINEAR_ACCELERATION; |
Jamie Smith |
1:aac28ffd63ed | 634 | break; |
Jamie Smith |
1:aac28ffd63ed | 635 | |
Jamie Smith |
1:aac28ffd63ed | 636 | case SENSOR_REPORTID_GYROSCOPE_CALIBRATED: |
Jamie Smith |
1:aac28ffd63ed | 637 | |
Jamie Smith |
1:aac28ffd63ed | 638 | gyroRotation = TVector3( |
Jamie Smith |
1:aac28ffd63ed | 639 | qToFloat(data1, GYRO_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 640 | qToFloat(data2, GYRO_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 641 | qToFloat(data3, GYRO_Q_POINT)); |
Jamie Smith |
1:aac28ffd63ed | 642 | |
Jamie Smith |
1:aac28ffd63ed | 643 | currReportOffset += SIZEOF_GYROSCOPE_CALIBRATED; |
Jamie Smith |
1:aac28ffd63ed | 644 | break; |
Jamie Smith |
1:aac28ffd63ed | 645 | |
Jamie Smith |
1:aac28ffd63ed | 646 | case SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED: |
Jamie Smith |
1:aac28ffd63ed | 647 | |
Jamie Smith |
1:aac28ffd63ed | 648 | magField = TVector3( |
Jamie Smith |
1:aac28ffd63ed | 649 | qToFloat(data1, MAGNETOMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 650 | qToFloat(data2, MAGNETOMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 651 | qToFloat(data3, MAGNETOMETER_Q_POINT)); |
Jamie Smith |
1:aac28ffd63ed | 652 | |
Jamie Smith |
1:aac28ffd63ed | 653 | currReportOffset += SIZEOF_MAGNETIC_FIELD_CALIBRATED; |
Jamie Smith |
1:aac28ffd63ed | 654 | break; |
Jamie Smith |
1:aac28ffd63ed | 655 | |
Jamie Smith |
1:aac28ffd63ed | 656 | case SENSOR_REPORTID_MAGNETIC_FIELD_UNCALIBRATED: |
Jamie Smith |
1:aac28ffd63ed | 657 | { |
Jamie Smith |
1:aac28ffd63ed | 658 | magFieldUncalibrated = TVector3( |
Jamie Smith |
1:aac28ffd63ed | 659 | qToFloat(data1, MAGNETOMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 660 | qToFloat(data2, MAGNETOMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 661 | qToFloat(data3, MAGNETOMETER_Q_POINT)); |
Jamie Smith |
1:aac28ffd63ed | 662 | |
Jamie Smith |
8:199c7fad233d | 663 | uint16_t ironOffsetXQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10]; |
Jamie Smith |
8:199c7fad233d | 664 | uint16_t ironOffsetYQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12]; |
Jamie Smith |
8:199c7fad233d | 665 | uint16_t ironOffsetZQ = (uint16_t) rxShtpData[currReportOffset + 15] << 8 | rxShtpData[currReportOffset + 14]; |
Jamie Smith |
1:aac28ffd63ed | 666 | |
Jamie Smith |
1:aac28ffd63ed | 667 | hardIronOffset = TVector3( |
Jamie Smith |
1:aac28ffd63ed | 668 | qToFloat(ironOffsetXQ, MAGNETOMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 669 | qToFloat(ironOffsetYQ, MAGNETOMETER_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 670 | qToFloat(ironOffsetZQ, MAGNETOMETER_Q_POINT)); |
Jamie Smith |
1:aac28ffd63ed | 671 | |
Jamie Smith |
1:aac28ffd63ed | 672 | currReportOffset += SIZEOF_MAGNETIC_FIELD_UNCALIBRATED; |
Jamie Smith |
1:aac28ffd63ed | 673 | } |
Jamie Smith |
1:aac28ffd63ed | 674 | break; |
Jamie Smith |
1:aac28ffd63ed | 675 | |
Jamie Smith |
1:aac28ffd63ed | 676 | case SENSOR_REPORTID_ROTATION_VECTOR: |
Jamie Smith |
1:aac28ffd63ed | 677 | { |
Jamie Smith |
8:199c7fad233d | 678 | uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10]; |
Jamie Smith |
8:199c7fad233d | 679 | uint16_t accuracyQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12]; |
Jamie Smith |
1:aac28ffd63ed | 680 | |
Jamie Smith |
1:aac28ffd63ed | 681 | rotationVector = TVector4( |
Jamie Smith |
1:aac28ffd63ed | 682 | qToFloat(data1, ROTATION_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 683 | qToFloat(data2, ROTATION_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 684 | qToFloat(data3, ROTATION_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 685 | qToFloat(realPartQ, ROTATION_Q_POINT)); |
Jamie Smith |
1:aac28ffd63ed | 686 | |
Jamie Smith |
1:aac28ffd63ed | 687 | rotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT); |
Jamie Smith |
1:aac28ffd63ed | 688 | |
Jamie Smith |
1:aac28ffd63ed | 689 | currReportOffset += SIZEOF_ROTATION_VECTOR; |
Jamie Smith |
1:aac28ffd63ed | 690 | } |
Jamie Smith |
1:aac28ffd63ed | 691 | break; |
Jamie Smith |
1:aac28ffd63ed | 692 | |
Jamie Smith |
1:aac28ffd63ed | 693 | case SENSOR_REPORTID_GAME_ROTATION_VECTOR: |
Jamie Smith |
1:aac28ffd63ed | 694 | { |
Jamie Smith |
8:199c7fad233d | 695 | uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10]; |
Jamie Smith |
1:aac28ffd63ed | 696 | |
Jamie Smith |
1:aac28ffd63ed | 697 | gameRotationVector = TVector4( |
Jamie Smith |
1:aac28ffd63ed | 698 | qToFloat(data1, ROTATION_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 699 | qToFloat(data2, ROTATION_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 700 | qToFloat(data3, ROTATION_Q_POINT), |
Jamie Smith |
1:aac28ffd63ed | 701 | qToFloat(realPartQ, ROTATION_Q_POINT)); |
Jamie Smith |
1:aac28ffd63ed | 702 | |
Jamie Smith |
1:aac28ffd63ed | 703 | currReportOffset += SIZEOF_GAME_ROTATION_VECTOR; |
Jamie Smith |
1:aac28ffd63ed | 704 | } |
Jamie Smith |
1:aac28ffd63ed | 705 | break; |
Jamie Smith |
1:aac28ffd63ed | 706 | |
Jamie Smith |
1:aac28ffd63ed | 707 | case SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR: |
Jamie Smith |
1:aac28ffd63ed | 708 | { |
Jamie Smith |
8:199c7fad233d | 709 | uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10]; |
Jamie Smith |
8:199c7fad233d | 710 | uint16_t accuracyQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12]; |
Jamie Smith |
1:aac28ffd63ed | 711 | |
Jamie Smith |
1:aac28ffd63ed | 712 | geomagneticRotationVector = 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 | geomagneticRotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT); |
Jamie Smith |
1:aac28ffd63ed | 719 | |
Jamie Smith |
1:aac28ffd63ed | 720 | currReportOffset += SIZEOF_GEOMAGNETIC_ROTATION_VECTOR; |
Jamie Smith |
1:aac28ffd63ed | 721 | } |
Jamie Smith |
1:aac28ffd63ed | 722 | break; |
Jamie Smith |
1:aac28ffd63ed | 723 | |
Jamie Smith |
1:aac28ffd63ed | 724 | case SENSOR_REPORTID_TAP_DETECTOR: |
Jamie Smith |
1:aac28ffd63ed | 725 | |
Jamie Smith |
1:aac28ffd63ed | 726 | // since we got the report, a tap was detected |
Jamie Smith |
1:aac28ffd63ed | 727 | tapDetected = true; |
Jamie Smith |
1:aac28ffd63ed | 728 | |
Jamie Smith |
8:199c7fad233d | 729 | doubleTap = (rxShtpData[currReportOffset + 4] & (1 << 6)) != 0; |
Jamie Smith |
1:aac28ffd63ed | 730 | |
Jamie Smith |
1:aac28ffd63ed | 731 | currReportOffset += SIZEOF_TAP_DETECTOR; |
Jamie Smith |
1:aac28ffd63ed | 732 | break; |
Jamie Smith |
1:aac28ffd63ed | 733 | |
Jamie Smith |
1:aac28ffd63ed | 734 | case SENSOR_REPORTID_STABILITY_CLASSIFIER: |
Jamie Smith |
1:aac28ffd63ed | 735 | { |
Jamie Smith |
8:199c7fad233d | 736 | uint8_t classificationNumber = rxShtpData[currReportOffset + 4]; |
Jamie Smith |
1:aac28ffd63ed | 737 | |
Jamie Smith |
1:aac28ffd63ed | 738 | if(classificationNumber > 4) |
Jamie Smith |
1:aac28ffd63ed | 739 | { |
Jamie Smith |
1:aac28ffd63ed | 740 | classificationNumber = 0; |
Jamie Smith |
1:aac28ffd63ed | 741 | } |
Jamie Smith |
1:aac28ffd63ed | 742 | |
Jamie Smith |
1:aac28ffd63ed | 743 | stability = static_cast<Stability>(classificationNumber); |
Jamie Smith |
1:aac28ffd63ed | 744 | |
Jamie Smith |
1:aac28ffd63ed | 745 | currReportOffset += SIZEOF_STABILITY_REPORT; |
Jamie Smith |
1:aac28ffd63ed | 746 | } |
Jamie Smith |
1:aac28ffd63ed | 747 | break; |
Jamie Smith |
1:aac28ffd63ed | 748 | |
Jamie Smith |
1:aac28ffd63ed | 749 | case SENSOR_REPORTID_STEP_DETECTOR: |
Jamie Smith |
1:aac28ffd63ed | 750 | |
Jamie Smith |
1:aac28ffd63ed | 751 | // the fact that we got the report means that a step was detected |
Jamie Smith |
1:aac28ffd63ed | 752 | stepDetected = true; |
Jamie Smith |
1:aac28ffd63ed | 753 | |
Jamie Smith |
1:aac28ffd63ed | 754 | currReportOffset += SIZEOF_STEP_DETECTOR; |
Jamie Smith |
1:aac28ffd63ed | 755 | |
Jamie Smith |
1:aac28ffd63ed | 756 | break; |
Jamie Smith |
1:aac28ffd63ed | 757 | |
Jamie Smith |
1:aac28ffd63ed | 758 | case SENSOR_REPORTID_STEP_COUNTER: |
Jamie Smith |
1:aac28ffd63ed | 759 | |
Jamie Smith |
8:199c7fad233d | 760 | stepCount = rxShtpData[currReportOffset + 9] << 8 | rxShtpData[currReportOffset + 8]; |
Jamie Smith |
1:aac28ffd63ed | 761 | |
Jamie Smith |
1:aac28ffd63ed | 762 | currReportOffset += SIZEOF_STEP_COUNTER; |
Jamie Smith |
1:aac28ffd63ed | 763 | |
Jamie Smith |
1:aac28ffd63ed | 764 | break; |
Jamie Smith |
1:aac28ffd63ed | 765 | |
Jamie Smith |
1:aac28ffd63ed | 766 | case SENSOR_REPORTID_SIGNIFICANT_MOTION: |
Jamie Smith |
1:aac28ffd63ed | 767 | |
Jamie Smith |
1:aac28ffd63ed | 768 | // the fact that we got the report means that significant motion was detected |
Jamie Smith |
1:aac28ffd63ed | 769 | significantMotionDetected = true; |
Jamie Smith |
1:aac28ffd63ed | 770 | |
Jamie Smith |
1:aac28ffd63ed | 771 | currReportOffset += SIZEOF_SIGNIFICANT_MOTION; |
Jamie Smith |
3:197ad972fb7c | 772 | |
Jamie Smith |
3:197ad972fb7c | 773 | break; |
Jamie Smith |
1:aac28ffd63ed | 774 | |
Jamie Smith |
1:aac28ffd63ed | 775 | case SENSOR_REPORTID_SHAKE_DETECTOR: |
Jamie Smith |
1:aac28ffd63ed | 776 | |
Jamie Smith |
1:aac28ffd63ed | 777 | shakeDetected = true; |
Jamie Smith |
1:aac28ffd63ed | 778 | |
Jamie Smith |
8:199c7fad233d | 779 | xAxisShake = (rxShtpData[currReportOffset + 4] & 1) != 0; |
Jamie Smith |
8:199c7fad233d | 780 | yAxisShake = (rxShtpData[currReportOffset + 4] & (1 << 1)) != 0; |
Jamie Smith |
8:199c7fad233d | 781 | zAxisShake = (rxShtpData[currReportOffset + 4] & (1 << 2)) != 0; |
Jamie Smith |
1:aac28ffd63ed | 782 | |
Jamie Smith |
1:aac28ffd63ed | 783 | currReportOffset += SIZEOF_SHAKE_DETECTOR; |
Jamie Smith |
1:aac28ffd63ed | 784 | |
Jamie Smith |
3:197ad972fb7c | 785 | break; |
Jamie Smith |
3:197ad972fb7c | 786 | |
Jamie Smith |
1:aac28ffd63ed | 787 | default: |
Jamie Smith |
8:199c7fad233d | 788 | _debugPort->printf("Error: unrecognized report ID in sensor report: %hhx. Byte %u, length %hu\n", rxShtpData[currReportOffset], currReportOffset, rxPacketLength); |
Jamie Smith |
1:aac28ffd63ed | 789 | return; |
Jamie Smith |
1:aac28ffd63ed | 790 | } |
Jamie Smith |
8:199c7fad233d | 791 | |
Jamie Smith |
8:199c7fad233d | 792 | if(currReportOffset >= SHTP_RX_PACKET_SIZE) |
Jamie Smith |
8:199c7fad233d | 793 | { |
Jamie Smith |
8:199c7fad233d | 794 | _debugPort->printf("Error: sensor report longer than packet buffer! Some data was not read! Increase buffer size or decrease number of reports!\r\n"); |
Jamie Smith |
8:199c7fad233d | 795 | return; |
Jamie Smith |
8:199c7fad233d | 796 | } |
Jamie Smith |
1:aac28ffd63ed | 797 | } |
Jamie Smith |
1:aac28ffd63ed | 798 | |
Jamie Smith |
1:aac28ffd63ed | 799 | } |
Jamie Smith |
1:aac28ffd63ed | 800 | |
Jamie Smith |
8:199c7fad233d | 801 | bool BNO080Base::waitForPacket(int channel, uint8_t reportID, std::chrono::milliseconds timeout) |
Jamie Smith |
1:aac28ffd63ed | 802 | { |
Jamie Smith |
1:aac28ffd63ed | 803 | Timer timeoutTimer; |
Jamie Smith |
1:aac28ffd63ed | 804 | timeoutTimer.start(); |
Jamie Smith |
1:aac28ffd63ed | 805 | |
Jamie Smith |
8:199c7fad233d | 806 | while(timeoutTimer.elapsed_time() <= timeout) |
Jamie Smith |
1:aac28ffd63ed | 807 | { |
Jamie Smith |
1:aac28ffd63ed | 808 | if(_int.read() == 0) |
Jamie Smith |
1:aac28ffd63ed | 809 | { |
Jamie Smith |
1:aac28ffd63ed | 810 | if(!receivePacket(timeout)) |
Jamie Smith |
1:aac28ffd63ed | 811 | { |
Jamie Smith |
1:aac28ffd63ed | 812 | return false; |
Jamie Smith |
1:aac28ffd63ed | 813 | } |
Jamie Smith |
1:aac28ffd63ed | 814 | |
Jamie Smith |
8:199c7fad233d | 815 | if(channel == rxShtpHeader[2] && reportID == rxShtpData[0]) |
Jamie Smith |
1:aac28ffd63ed | 816 | { |
Jamie Smith |
1:aac28ffd63ed | 817 | // found correct packet! |
Jamie Smith |
1:aac28ffd63ed | 818 | return true; |
Jamie Smith |
1:aac28ffd63ed | 819 | } |
Jamie Smith |
1:aac28ffd63ed | 820 | else |
Jamie Smith |
1:aac28ffd63ed | 821 | { |
Jamie Smith |
1:aac28ffd63ed | 822 | // other data packet, send to proper channels |
Jamie Smith |
1:aac28ffd63ed | 823 | processPacket(); |
Jamie Smith |
1:aac28ffd63ed | 824 | } |
Jamie Smith |
1:aac28ffd63ed | 825 | } |
Jamie Smith |
1:aac28ffd63ed | 826 | } |
Jamie Smith |
1:aac28ffd63ed | 827 | |
Jamie Smith |
1:aac28ffd63ed | 828 | _debugPort->printf("Packet wait timeout.\n"); |
Jamie Smith |
1:aac28ffd63ed | 829 | return false; |
Jamie Smith |
1:aac28ffd63ed | 830 | } |
Jamie Smith |
1:aac28ffd63ed | 831 | |
Jamie Smith |
1:aac28ffd63ed | 832 | //Given a register value and a Q point, convert to float |
Jamie Smith |
1:aac28ffd63ed | 833 | //See https://en.wikipedia.org/wiki/Q_(number_format) |
Jamie Smith |
8:199c7fad233d | 834 | float BNO080Base::qToFloat(int16_t fixedPointValue, uint8_t qPoint) |
Jamie Smith |
1:aac28ffd63ed | 835 | { |
Jamie Smith |
1:aac28ffd63ed | 836 | float qFloat = fixedPointValue; |
Jamie Smith |
5:c75be9000d75 | 837 | qFloat *= pow(2.0f, qPoint * -1); |
Jamie Smith |
1:aac28ffd63ed | 838 | return (qFloat); |
Jamie Smith |
1:aac28ffd63ed | 839 | } |
Jamie Smith |
1:aac28ffd63ed | 840 | |
Jamie Smith |
8:199c7fad233d | 841 | float BNO080Base::qToFloat_dword(uint32_t fixedPointValue, int16_t qPoint) |
Jamie Smith |
1:aac28ffd63ed | 842 | { |
Jamie Smith |
1:aac28ffd63ed | 843 | float qFloat = fixedPointValue; |
Jamie Smith |
5:c75be9000d75 | 844 | qFloat *= pow(2.0f, qPoint * -1); |
Jamie Smith |
1:aac28ffd63ed | 845 | return (qFloat); |
Jamie Smith |
1:aac28ffd63ed | 846 | } |
Jamie Smith |
1:aac28ffd63ed | 847 | |
Jamie Smith |
1:aac28ffd63ed | 848 | //Given a floating point value and a Q point, convert to Q |
Jamie Smith |
1:aac28ffd63ed | 849 | //See https://en.wikipedia.org/wiki/Q_(number_format) |
Jamie Smith |
8:199c7fad233d | 850 | int16_t BNO080Base::floatToQ(float qFloat, uint8_t qPoint) |
Jamie Smith |
1:aac28ffd63ed | 851 | { |
Jamie Smith |
5:c75be9000d75 | 852 | int16_t qVal = static_cast<int16_t>(qFloat * pow(2.0f, qPoint)); |
Jamie Smith |
1:aac28ffd63ed | 853 | return qVal; |
Jamie Smith |
1:aac28ffd63ed | 854 | } |
Jamie Smith |
1:aac28ffd63ed | 855 | |
Jamie Smith |
8:199c7fad233d | 856 | int32_t BNO080Base::floatToQ_dword(float qFloat, uint16_t qPoint) |
Jamie Smith |
3:197ad972fb7c | 857 | { |
Jamie Smith |
5:c75be9000d75 | 858 | int32_t qVal = static_cast<int32_t>(qFloat * pow(2.0f, qPoint)); |
Jamie Smith |
3:197ad972fb7c | 859 | return qVal; |
Jamie Smith |
3:197ad972fb7c | 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 |
8:199c7fad233d | 864 | void BNO080Base::sendCommand(uint8_t command) |
Jamie Smith |
1:aac28ffd63ed | 865 | { |
Jamie Smith |
8:199c7fad233d | 866 | txShtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request |
Jamie Smith |
8:199c7fad233d | 867 | txShtpData[1] = commandSequenceNumber++; //Increments automatically each function call |
Jamie Smith |
8:199c7fad233d | 868 | txShtpData[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 |
8:199c7fad233d | 887 | void BNO080Base::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 |
8:199c7fad233d | 893 | txShtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55 |
Jamie Smith |
8:199c7fad233d | 894 | txShtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector |
Jamie Smith |
8:199c7fad233d | 895 | txShtpData[2] = 0; //Feature flags |
Jamie Smith |
8:199c7fad233d | 896 | txShtpData[3] = 0; //Change sensitivity (LSB) |
Jamie Smith |
8:199c7fad233d | 897 | txShtpData[4] = 0; //Change sensitivity (MSB) |
Jamie Smith |
8:199c7fad233d | 898 | txShtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms |
Jamie Smith |
8:199c7fad233d | 899 | txShtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval |
Jamie Smith |
8:199c7fad233d | 900 | txShtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval |
Jamie Smith |
8:199c7fad233d | 901 | txShtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB) |
Jamie Smith |
8:199c7fad233d | 902 | txShtpData[9] = (batchMicros >> 0) & 0xFF; //Batch Interval (LSB) |
Jamie Smith |
8:199c7fad233d | 903 | txShtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval |
Jamie Smith |
8:199c7fad233d | 904 | txShtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval |
Jamie Smith |
8:199c7fad233d | 905 | txShtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB) |
Jamie Smith |
8:199c7fad233d | 906 | txShtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB) |
Jamie Smith |
8:199c7fad233d | 907 | txShtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config |
Jamie Smith |
8:199c7fad233d | 908 | txShtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config |
Jamie Smith |
8:199c7fad233d | 909 | txShtpData[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 |
8:199c7fad233d | 915 | bool BNO080Base::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 |
9:430f5302f9e1 | 918 | clearSendBuffer(); |
Jamie Smith |
1:aac28ffd63ed | 919 | |
Jamie Smith |
8:199c7fad233d | 920 | txShtpData[0] = SHTP_REPORT_FRS_READ_REQUEST; |
Jamie Smith |
1:aac28ffd63ed | 921 | // read offset of 0 -> start at the start of the record |
Jamie Smith |
8:199c7fad233d | 922 | txShtpData[2] = 0; |
Jamie Smith |
8:199c7fad233d | 923 | txShtpData[3] = 0; |
Jamie Smith |
1:aac28ffd63ed | 924 | // record ID |
Jamie Smith |
8:199c7fad233d | 925 | txShtpData[4] = static_cast<uint8_t>(recordID & 0xFF); |
Jamie Smith |
8:199c7fad233d | 926 | txShtpData[5] = static_cast<uint8_t>(recordID >> 8); |
Jamie Smith |
1:aac28ffd63ed | 927 | // block size |
Jamie Smith |
8:199c7fad233d | 928 | txShtpData[6] = static_cast<uint8_t>(readLength & 0xFF); |
Jamie Smith |
8:199c7fad233d | 929 | txShtpData[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 |
3:197ad972fb7c | 937 | // it seems like it can take quite a long time for FRS data to be read, so we have to increase the timeout |
Jamie Smith |
8:199c7fad233d | 938 | if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE, 300ms)) |
Jamie Smith |
1:aac28ffd63ed | 939 | { |
Jamie Smith |
1:aac28ffd63ed | 940 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 941 | _debugPort->printf("Error: did not receive FRS read response after sending read request!\n"); |
Jamie Smith |
1:aac28ffd63ed | 942 | #endif |
Jamie Smith |
1:aac28ffd63ed | 943 | return false; |
Jamie Smith |
1:aac28ffd63ed | 944 | } |
Jamie Smith |
1:aac28ffd63ed | 945 | |
Jamie Smith |
8:199c7fad233d | 946 | uint8_t status = static_cast<uint8_t>(rxShtpData[1] & 0b1111); |
Jamie Smith |
8:199c7fad233d | 947 | uint8_t dataLength = rxShtpData[1] >> 4; |
Jamie Smith |
1:aac28ffd63ed | 948 | |
Jamie Smith |
1:aac28ffd63ed | 949 | // check status |
Jamie Smith |
1:aac28ffd63ed | 950 | if(status == 1) |
Jamie Smith |
1:aac28ffd63ed | 951 | { |
Jamie Smith |
1:aac28ffd63ed | 952 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 953 | _debugPort->printf("Error: FRS reports invalid record ID!\n"); |
Jamie Smith |
1:aac28ffd63ed | 954 | #endif |
Jamie Smith |
1:aac28ffd63ed | 955 | return false; |
Jamie Smith |
1:aac28ffd63ed | 956 | } |
Jamie Smith |
1:aac28ffd63ed | 957 | else if(status == 2) |
Jamie Smith |
1:aac28ffd63ed | 958 | { |
Jamie Smith |
1:aac28ffd63ed | 959 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 960 | _debugPort->printf("Error: FRS is busy!\n"); |
Jamie Smith |
1:aac28ffd63ed | 961 | #endif |
Jamie Smith |
1:aac28ffd63ed | 962 | return false; |
Jamie Smith |
1:aac28ffd63ed | 963 | } |
Jamie Smith |
1:aac28ffd63ed | 964 | else if(status == 4) |
Jamie Smith |
1:aac28ffd63ed | 965 | { |
Jamie Smith |
1:aac28ffd63ed | 966 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 967 | _debugPort->printf("Error: FRS reports offset is out of range!\n"); |
Jamie Smith |
1:aac28ffd63ed | 968 | #endif |
Jamie Smith |
1:aac28ffd63ed | 969 | return false; |
Jamie Smith |
1:aac28ffd63ed | 970 | } |
Jamie Smith |
1:aac28ffd63ed | 971 | else if(status == 5) |
Jamie Smith |
1:aac28ffd63ed | 972 | { |
Jamie Smith |
1:aac28ffd63ed | 973 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 974 | _debugPort->printf("Error: FRS reports record %hx is empty!\n", recordID); |
Jamie Smith |
1:aac28ffd63ed | 975 | #endif |
Jamie Smith |
1:aac28ffd63ed | 976 | return false; |
Jamie Smith |
1:aac28ffd63ed | 977 | } |
Jamie Smith |
1:aac28ffd63ed | 978 | else if(status == 8) |
Jamie Smith |
1:aac28ffd63ed | 979 | { |
Jamie Smith |
1:aac28ffd63ed | 980 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 981 | _debugPort->printf("Error: FRS reports flash memory device unavailable!\n"); |
Jamie Smith |
1:aac28ffd63ed | 982 | #endif |
Jamie Smith |
1:aac28ffd63ed | 983 | return false; |
Jamie Smith |
1:aac28ffd63ed | 984 | } |
Jamie Smith |
1:aac28ffd63ed | 985 | |
Jamie Smith |
1:aac28ffd63ed | 986 | // check data length |
Jamie Smith |
1:aac28ffd63ed | 987 | if(dataLength == 0) |
Jamie Smith |
1:aac28ffd63ed | 988 | { |
Jamie Smith |
1:aac28ffd63ed | 989 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 990 | _debugPort->printf("Error: Received FRS packet with 0 data length!\n"); |
Jamie Smith |
1:aac28ffd63ed | 991 | #endif |
Jamie Smith |
1:aac28ffd63ed | 992 | return false; |
Jamie Smith |
1:aac28ffd63ed | 993 | } |
Jamie Smith |
1:aac28ffd63ed | 994 | else if(dataLength == 1) |
Jamie Smith |
1:aac28ffd63ed | 995 | { |
Jamie Smith |
1:aac28ffd63ed | 996 | if(readOffset + 1 != readLength) |
Jamie Smith |
1:aac28ffd63ed | 997 | { |
Jamie Smith |
1:aac28ffd63ed | 998 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 999 | _debugPort->printf("Error: Received 1 length packet but more than 1 byte remains to be be read!\n"); |
Jamie Smith |
1:aac28ffd63ed | 1000 | #endif |
Jamie Smith |
1:aac28ffd63ed | 1001 | return false; |
Jamie Smith |
1:aac28ffd63ed | 1002 | } |
Jamie Smith |
1:aac28ffd63ed | 1003 | } |
Jamie Smith |
1:aac28ffd63ed | 1004 | |
Jamie Smith |
1:aac28ffd63ed | 1005 | // now, _finally_, read the dang words |
Jamie Smith |
8:199c7fad233d | 1006 | readBuffer[readOffset] = (rxShtpData[7] << 24) | (rxShtpData[6] << 16) | (rxShtpData[5] << 8) | (rxShtpData[4]); |
Jamie Smith |
1:aac28ffd63ed | 1007 | |
Jamie Smith |
1:aac28ffd63ed | 1008 | // check if we only wanted the first word |
Jamie Smith |
1:aac28ffd63ed | 1009 | ++readOffset; |
Jamie Smith |
1:aac28ffd63ed | 1010 | if(readOffset == readLength) |
Jamie Smith |
1:aac28ffd63ed | 1011 | { |
Jamie Smith |
1:aac28ffd63ed | 1012 | break; |
Jamie Smith |
1:aac28ffd63ed | 1013 | } |
Jamie Smith |
1:aac28ffd63ed | 1014 | |
Jamie Smith |
8:199c7fad233d | 1015 | readBuffer[readOffset] = (rxShtpData[11] << 24) | (rxShtpData[10] << 16) | (rxShtpData[9] << 8) | (rxShtpData[8]); |
Jamie Smith |
1:aac28ffd63ed | 1016 | readOffset++; |
Jamie Smith |
1:aac28ffd63ed | 1017 | } |
Jamie Smith |
1:aac28ffd63ed | 1018 | |
Jamie Smith |
1:aac28ffd63ed | 1019 | // read successful |
Jamie Smith |
1:aac28ffd63ed | 1020 | return true; |
Jamie Smith |
1:aac28ffd63ed | 1021 | |
Jamie Smith |
1:aac28ffd63ed | 1022 | } |
Jamie Smith |
1:aac28ffd63ed | 1023 | |
Jamie Smith |
8:199c7fad233d | 1024 | bool BNO080Base::writeFRSRecord(uint16_t recordID, uint32_t* buffer, uint16_t length) |
Jamie Smith |
3:197ad972fb7c | 1025 | { |
Jamie Smith |
3:197ad972fb7c | 1026 | // send initial write request, which tells the chip where we're writing |
Jamie Smith |
9:430f5302f9e1 | 1027 | clearSendBuffer(); |
Jamie Smith |
3:197ad972fb7c | 1028 | |
Jamie Smith |
8:199c7fad233d | 1029 | txShtpData[0] = SHTP_REPORT_FRS_WRITE_REQUEST; |
Jamie Smith |
3:197ad972fb7c | 1030 | // length to write (must be <= record length) |
Jamie Smith |
8:199c7fad233d | 1031 | txShtpData[2] = static_cast<uint8_t>(length & 0xFF); |
Jamie Smith |
8:199c7fad233d | 1032 | txShtpData[3] = static_cast<uint8_t>(length >> 8); |
Jamie Smith |
3:197ad972fb7c | 1033 | // record ID |
Jamie Smith |
8:199c7fad233d | 1034 | txShtpData[4] = static_cast<uint8_t>(recordID & 0xFF); |
Jamie Smith |
8:199c7fad233d | 1035 | txShtpData[5] = static_cast<uint8_t>(recordID >> 8); |
Jamie Smith |
3:197ad972fb7c | 1036 | |
Jamie Smith |
3:197ad972fb7c | 1037 | sendPacket(CHANNEL_CONTROL, 6); |
Jamie Smith |
3:197ad972fb7c | 1038 | |
Jamie Smith |
3:197ad972fb7c | 1039 | // wait for FRS to become ready |
Jamie Smith |
8:199c7fad233d | 1040 | if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, 300ms)) |
Jamie Smith |
3:197ad972fb7c | 1041 | { |
Jamie Smith |
3:197ad972fb7c | 1042 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1043 | _debugPort->printf("Error: did not receive FRS write ready response after sending write request!\r\n"); |
Jamie Smith |
3:197ad972fb7c | 1044 | #endif |
Jamie Smith |
3:197ad972fb7c | 1045 | return false; |
Jamie Smith |
3:197ad972fb7c | 1046 | } |
Jamie Smith |
3:197ad972fb7c | 1047 | |
Jamie Smith |
8:199c7fad233d | 1048 | if(rxShtpData[1] != 4) |
Jamie Smith |
3:197ad972fb7c | 1049 | { |
Jamie Smith |
3:197ad972fb7c | 1050 | #if BNO_DEBUG |
Jamie Smith |
8:199c7fad233d | 1051 | _debugPort->printf("Error: FRS reports error initiating write operation: %hhu!\r\n", rxShtpData[1]); |
Jamie Smith |
3:197ad972fb7c | 1052 | #endif |
Jamie Smith |
3:197ad972fb7c | 1053 | return false; |
Jamie Smith |
3:197ad972fb7c | 1054 | } |
Jamie Smith |
3:197ad972fb7c | 1055 | |
Jamie Smith |
3:197ad972fb7c | 1056 | // now, send the actual data |
Jamie Smith |
3:197ad972fb7c | 1057 | for(uint16_t wordIndex = 0; wordIndex < length; wordIndex += 2) |
Jamie Smith |
3:197ad972fb7c | 1058 | { |
Jamie Smith |
3:197ad972fb7c | 1059 | // send packet containing 2 words |
Jamie Smith |
9:430f5302f9e1 | 1060 | clearSendBuffer(); |
Jamie Smith |
8:199c7fad233d | 1061 | txShtpData[0] = SHTP_REPORT_FRS_WRITE_DATA; |
Jamie Smith |
3:197ad972fb7c | 1062 | |
Jamie Smith |
3:197ad972fb7c | 1063 | // offset to write at |
Jamie Smith |
8:199c7fad233d | 1064 | txShtpData[2] = static_cast<uint8_t>(wordIndex & 0xFF); |
Jamie Smith |
8:199c7fad233d | 1065 | txShtpData[3] = static_cast<uint8_t>(wordIndex >> 8); |
Jamie Smith |
3:197ad972fb7c | 1066 | |
Jamie Smith |
3:197ad972fb7c | 1067 | // data 0 |
Jamie Smith |
8:199c7fad233d | 1068 | *reinterpret_cast<uint32_t*>(txShtpData + 4) = buffer[wordIndex]; |
Jamie Smith |
3:197ad972fb7c | 1069 | |
Jamie Smith |
3:197ad972fb7c | 1070 | // data 1, if it exists |
Jamie Smith |
3:197ad972fb7c | 1071 | if(wordIndex != length - 1) |
Jamie Smith |
3:197ad972fb7c | 1072 | { |
Jamie Smith |
8:199c7fad233d | 1073 | *reinterpret_cast<uint32_t*>(txShtpData + 8) = buffer[wordIndex + 1]; |
Jamie Smith |
3:197ad972fb7c | 1074 | } |
Jamie Smith |
3:197ad972fb7c | 1075 | |
Jamie Smith |
3:197ad972fb7c | 1076 | sendPacket(CHANNEL_CONTROL, 12); |
Jamie Smith |
3:197ad972fb7c | 1077 | |
Jamie Smith |
3:197ad972fb7c | 1078 | // wait for acknowledge |
Jamie Smith |
8:199c7fad233d | 1079 | if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, 300ms)) |
Jamie Smith |
3:197ad972fb7c | 1080 | { |
Jamie Smith |
3:197ad972fb7c | 1081 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1082 | _debugPort->printf("Error: did not receive FRS write response after sending write data!\r\n"); |
Jamie Smith |
3:197ad972fb7c | 1083 | #endif |
Jamie Smith |
3:197ad972fb7c | 1084 | return false; |
Jamie Smith |
3:197ad972fb7c | 1085 | } |
Jamie Smith |
3:197ad972fb7c | 1086 | |
Jamie Smith |
8:199c7fad233d | 1087 | uint8_t status = rxShtpData[1]; |
Jamie Smith |
3:197ad972fb7c | 1088 | |
Jamie Smith |
3:197ad972fb7c | 1089 | switch(status) |
Jamie Smith |
3:197ad972fb7c | 1090 | { |
Jamie Smith |
3:197ad972fb7c | 1091 | case 0: |
Jamie Smith |
3:197ad972fb7c | 1092 | if(length - wordIndex >= 2) |
Jamie Smith |
3:197ad972fb7c | 1093 | { |
Jamie Smith |
3:197ad972fb7c | 1094 | // status OK, write still in progress |
Jamie Smith |
3:197ad972fb7c | 1095 | } |
Jamie Smith |
3:197ad972fb7c | 1096 | else |
Jamie Smith |
3:197ad972fb7c | 1097 | { |
Jamie Smith |
3:197ad972fb7c | 1098 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1099 | _debugPort->printf("Error: FRS reports write in progress when it should be complete!\r\n"); |
Jamie Smith |
3:197ad972fb7c | 1100 | #endif |
Jamie Smith |
3:197ad972fb7c | 1101 | return false; |
Jamie Smith |
3:197ad972fb7c | 1102 | } |
Jamie Smith |
3:197ad972fb7c | 1103 | break; |
Jamie Smith |
3:197ad972fb7c | 1104 | case 3: |
Jamie Smith |
3:197ad972fb7c | 1105 | case 8: |
Jamie Smith |
3:197ad972fb7c | 1106 | if(length - wordIndex <= 2) |
Jamie Smith |
3:197ad972fb7c | 1107 | { |
Jamie Smith |
3:197ad972fb7c | 1108 | // status OK, write complete |
Jamie Smith |
3:197ad972fb7c | 1109 | } |
Jamie Smith |
3:197ad972fb7c | 1110 | else |
Jamie Smith |
3:197ad972fb7c | 1111 | { |
Jamie Smith |
3:197ad972fb7c | 1112 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1113 | _debugPort->printf("Error: FRS reports write complete when it should be still going!\n"); |
Jamie Smith |
3:197ad972fb7c | 1114 | #endif |
Jamie Smith |
3:197ad972fb7c | 1115 | return false; |
Jamie Smith |
3:197ad972fb7c | 1116 | } |
Jamie Smith |
3:197ad972fb7c | 1117 | break; |
Jamie Smith |
3:197ad972fb7c | 1118 | case 1: |
Jamie Smith |
3:197ad972fb7c | 1119 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1120 | _debugPort->printf("Error: FRS reports invalid record ID!\n"); |
Jamie Smith |
3:197ad972fb7c | 1121 | #endif |
Jamie Smith |
3:197ad972fb7c | 1122 | return false; |
Jamie Smith |
3:197ad972fb7c | 1123 | case 2: |
Jamie Smith |
3:197ad972fb7c | 1124 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1125 | _debugPort->printf("Error: FRS is busy!\n"); |
Jamie Smith |
3:197ad972fb7c | 1126 | #endif |
Jamie Smith |
3:197ad972fb7c | 1127 | return false; |
Jamie Smith |
3:197ad972fb7c | 1128 | case 5: |
Jamie Smith |
3:197ad972fb7c | 1129 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1130 | _debugPort->printf("Error: FRS reports write failed!\n"); |
Jamie Smith |
3:197ad972fb7c | 1131 | #endif |
Jamie Smith |
3:197ad972fb7c | 1132 | return false; |
Jamie Smith |
3:197ad972fb7c | 1133 | case 6: |
Jamie Smith |
3:197ad972fb7c | 1134 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1135 | _debugPort->printf("Error: FRS reports data received while not in write mode!\n"); |
Jamie Smith |
3:197ad972fb7c | 1136 | #endif |
Jamie Smith |
3:197ad972fb7c | 1137 | return false; |
Jamie Smith |
3:197ad972fb7c | 1138 | case 7: |
Jamie Smith |
3:197ad972fb7c | 1139 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1140 | _debugPort->printf("Error: FRS reports invalid length!\n"); |
Jamie Smith |
3:197ad972fb7c | 1141 | #endif |
Jamie Smith |
3:197ad972fb7c | 1142 | return false; |
Jamie Smith |
3:197ad972fb7c | 1143 | case 9: |
Jamie Smith |
3:197ad972fb7c | 1144 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1145 | _debugPort->printf("Error: FRS reports invalid data for this record!\n"); |
Jamie Smith |
3:197ad972fb7c | 1146 | #endif |
Jamie Smith |
3:197ad972fb7c | 1147 | return false; |
Jamie Smith |
3:197ad972fb7c | 1148 | |
Jamie Smith |
3:197ad972fb7c | 1149 | case 10: |
Jamie Smith |
3:197ad972fb7c | 1150 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1151 | _debugPort->printf("Error: FRS reports flash device unavailable!\n"); |
Jamie Smith |
3:197ad972fb7c | 1152 | #endif |
Jamie Smith |
3:197ad972fb7c | 1153 | return false; |
Jamie Smith |
3:197ad972fb7c | 1154 | |
Jamie Smith |
3:197ad972fb7c | 1155 | case 11: |
Jamie Smith |
3:197ad972fb7c | 1156 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1157 | _debugPort->printf("Error: FRS reports record is read-only!\n"); |
Jamie Smith |
3:197ad972fb7c | 1158 | #endif |
Jamie Smith |
3:197ad972fb7c | 1159 | return false; |
Jamie Smith |
3:197ad972fb7c | 1160 | default: |
Jamie Smith |
3:197ad972fb7c | 1161 | #if BNO_DEBUG |
Jamie Smith |
3:197ad972fb7c | 1162 | _debugPort->printf("Error: FRS reports unknown result code %hhu!\n", status); |
Jamie Smith |
3:197ad972fb7c | 1163 | #endif |
Jamie Smith |
3:197ad972fb7c | 1164 | break; |
Jamie Smith |
3:197ad972fb7c | 1165 | |
Jamie Smith |
3:197ad972fb7c | 1166 | } |
Jamie Smith |
3:197ad972fb7c | 1167 | } |
Jamie Smith |
3:197ad972fb7c | 1168 | |
Jamie Smith |
3:197ad972fb7c | 1169 | // write complete |
Jamie Smith |
3:197ad972fb7c | 1170 | return true; |
Jamie Smith |
3:197ad972fb7c | 1171 | } |
Jamie Smith |
3:197ad972fb7c | 1172 | |
Jamie Smith |
8:199c7fad233d | 1173 | //Pretty prints the contents of a packet in the given buffer |
Jamie Smith |
8:199c7fad233d | 1174 | void BNO080Base::printPacket(uint8_t * buffer) |
Jamie Smith |
1:aac28ffd63ed | 1175 | { |
Jamie Smith |
1:aac28ffd63ed | 1176 | #if BNO_DEBUG |
Jamie Smith |
1:aac28ffd63ed | 1177 | //Print the four byte header |
Jamie Smith |
1:aac28ffd63ed | 1178 | _debugPort->printf("Header:"); |
Jamie Smith |
1:aac28ffd63ed | 1179 | for (uint8_t x = 0 ; x < 4 ; x++) |
Jamie Smith |
1:aac28ffd63ed | 1180 | { |
Jamie Smith |
1:aac28ffd63ed | 1181 | _debugPort->printf(" "); |
Jamie Smith |
8:199c7fad233d | 1182 | _debugPort->printf("%02hhx", buffer[x]); |
Jamie Smith |
1:aac28ffd63ed | 1183 | } |
Jamie Smith |
1:aac28ffd63ed | 1184 | |
Jamie Smith |
8:199c7fad233d | 1185 | //Calculate the number of data bytes in this packet |
Jamie Smith |
8:199c7fad233d | 1186 | uint16_t totalLength = (static_cast<uint16_t>(buffer[1]) << 8 | buffer[0]); |
Jamie Smith |
8:199c7fad233d | 1187 | totalLength &= ~(1 << 15); |
Jamie Smith |
8:199c7fad233d | 1188 | uint16_t packetLength = totalLength - 4; //Remove the header bytes from the data count |
Jamie Smith |
8:199c7fad233d | 1189 | |
Jamie Smith |
8:199c7fad233d | 1190 | uint16_t printLength = std::min(packetLength, static_cast<uint16_t>(40)); //Artificial limit. We don't want the phone book. |
Jamie Smith |
1:aac28ffd63ed | 1191 | |
Jamie Smith |
1:aac28ffd63ed | 1192 | _debugPort->printf(" Body:"); |
Jamie Smith |
1:aac28ffd63ed | 1193 | for (uint16_t x = 0 ; x < printLength ; x++) |
Jamie Smith |
1:aac28ffd63ed | 1194 | { |
Jamie Smith |
1:aac28ffd63ed | 1195 | _debugPort->printf(" "); |
Jamie Smith |
8:199c7fad233d | 1196 | _debugPort->printf("%02hhx", buffer[x + SHTP_HEADER_SIZE]); |
Jamie Smith |
1:aac28ffd63ed | 1197 | } |
Jamie Smith |
1:aac28ffd63ed | 1198 | |
Jamie Smith |
1:aac28ffd63ed | 1199 | _debugPort->printf(", Length:"); |
Jamie Smith |
8:199c7fad233d | 1200 | _debugPort->printf("%d", packetLength + SHTP_HEADER_SIZE); |
Jamie Smith |
8:199c7fad233d | 1201 | _debugPort->printf(", SeqNum: %hhu", buffer[3]); |
Jamie Smith |
1:aac28ffd63ed | 1202 | |
Jamie Smith |
1:aac28ffd63ed | 1203 | _debugPort->printf(", Channel:"); |
Jamie Smith |
8:199c7fad233d | 1204 | if (buffer[2] == 0) _debugPort->printf("Command"); |
Jamie Smith |
8:199c7fad233d | 1205 | else if (buffer[2] == 1) _debugPort->printf("Executable"); |
Jamie Smith |
8:199c7fad233d | 1206 | else if (buffer[2] == 2) _debugPort->printf("Control"); |
Jamie Smith |
8:199c7fad233d | 1207 | else if (buffer[2] == 3) _debugPort->printf("Sensor-report"); |
Jamie Smith |
8:199c7fad233d | 1208 | else if (buffer[2] == 4) _debugPort->printf("Wake-report"); |
Jamie Smith |
8:199c7fad233d | 1209 | else if (buffer[2] == 5) _debugPort->printf("Gyro-vector"); |
Jamie Smith |
8:199c7fad233d | 1210 | else _debugPort->printf("%hhu", buffer[2]); |
Jamie Smith |
1:aac28ffd63ed | 1211 | |
Jamie Smith |
1:aac28ffd63ed | 1212 | _debugPort->printf("\n"); |
Jamie Smith |
1:aac28ffd63ed | 1213 | #endif |
Jamie Smith |
1:aac28ffd63ed | 1214 | } |
Jamie Smith |
1:aac28ffd63ed | 1215 | |
Jamie Smith |
1:aac28ffd63ed | 1216 | |
Jamie Smith |
9:430f5302f9e1 | 1217 | void BNO080Base::clearSendBuffer() |
Jamie Smith |
1:aac28ffd63ed | 1218 | { |
Jamie Smith |
8:199c7fad233d | 1219 | memset(txPacketBuffer, 0, SHTP_HEADER_SIZE + SHTP_MAX_TX_PACKET_SIZE); |
Jamie Smith |
1:aac28ffd63ed | 1220 | } |
Jamie Smith |
1:aac28ffd63ed | 1221 | |
Jamie Smith |
8:199c7fad233d | 1222 | bool BNO080Base::loadReportMetadata(BNO080Base::Report report) |
Jamie Smith |
1:aac28ffd63ed | 1223 | { |
Jamie Smith |
3:197ad972fb7c | 1224 | uint16_t reportMetaRecord = 0; |
Jamie Smith |
1:aac28ffd63ed | 1225 | |
Jamie Smith |
1:aac28ffd63ed | 1226 | // first, convert the report into the correct FRS record ID for that report's metadata |
Jamie Smith |
1:aac28ffd63ed | 1227 | // data from SH-2 section 5.1 |
Jamie Smith |
1:aac28ffd63ed | 1228 | switch(report) |
Jamie Smith |
1:aac28ffd63ed | 1229 | { |
Jamie Smith |
2:2269b723d16a | 1230 | case TOTAL_ACCELERATION: |
Jamie Smith |
1:aac28ffd63ed | 1231 | reportMetaRecord = 0xE301; |
Jamie Smith |
1:aac28ffd63ed | 1232 | break; |
Jamie Smith |
2:2269b723d16a | 1233 | case LINEAR_ACCELERATION: |
Jamie Smith |
1:aac28ffd63ed | 1234 | reportMetaRecord = 0xE303; |
Jamie Smith |
1:aac28ffd63ed | 1235 | break; |
Jamie Smith |
2:2269b723d16a | 1236 | case GRAVITY_ACCELERATION: |
Jamie Smith |
1:aac28ffd63ed | 1237 | reportMetaRecord = 0xE304; |
Jamie Smith |
1:aac28ffd63ed | 1238 | break; |
Jamie Smith |
2:2269b723d16a | 1239 | case GYROSCOPE: |
Jamie Smith |
1:aac28ffd63ed | 1240 | reportMetaRecord = 0xE306; |
Jamie Smith |
1:aac28ffd63ed | 1241 | break; |
Jamie Smith |
2:2269b723d16a | 1242 | case MAG_FIELD: |
Jamie Smith |
1:aac28ffd63ed | 1243 | reportMetaRecord = 0xE309; |
Jamie Smith |
1:aac28ffd63ed | 1244 | break; |
Jamie Smith |
2:2269b723d16a | 1245 | case MAG_FIELD_UNCALIBRATED: |
Jamie Smith |
1:aac28ffd63ed | 1246 | reportMetaRecord = 0xE30A; |
Jamie Smith |
1:aac28ffd63ed | 1247 | break; |
Jamie Smith |
2:2269b723d16a | 1248 | case ROTATION: |
Jamie Smith |
1:aac28ffd63ed | 1249 | reportMetaRecord = 0xE30B; |
Jamie Smith |
1:aac28ffd63ed | 1250 | break; |
Jamie Smith |
2:2269b723d16a | 1251 | case GEOMAGNETIC_ROTATION: |
Jamie Smith |
1:aac28ffd63ed | 1252 | reportMetaRecord = 0xE30D; |
Jamie Smith |
1:aac28ffd63ed | 1253 | break; |
Jamie Smith |
2:2269b723d16a | 1254 | case GAME_ROTATION: |
Jamie Smith |
1:aac28ffd63ed | 1255 | reportMetaRecord = 0xE30C; |
Jamie Smith |
1:aac28ffd63ed | 1256 | break; |
Jamie Smith |
2:2269b723d16a | 1257 | case TAP_DETECTOR: |
Jamie Smith |
1:aac28ffd63ed | 1258 | reportMetaRecord = 0xE313; |
Jamie Smith |
1:aac28ffd63ed | 1259 | break; |
Jamie Smith |
2:2269b723d16a | 1260 | case STABILITY_CLASSIFIER: |
Jamie Smith |
1:aac28ffd63ed | 1261 | reportMetaRecord = 0xE317; |
Jamie Smith |
1:aac28ffd63ed | 1262 | break; |
Jamie Smith |
2:2269b723d16a | 1263 | case STEP_DETECTOR: |
Jamie Smith |
1:aac28ffd63ed | 1264 | reportMetaRecord = 0xE314; |
Jamie Smith |
1:aac28ffd63ed | 1265 | break; |
Jamie Smith |
2:2269b723d16a | 1266 | case STEP_COUNTER: |
Jamie Smith |
1:aac28ffd63ed | 1267 | reportMetaRecord = 0xE315; |
Jamie Smith |
1:aac28ffd63ed | 1268 | break; |
Jamie Smith |
2:2269b723d16a | 1269 | case SIGNIFICANT_MOTION: |
Jamie Smith |
1:aac28ffd63ed | 1270 | reportMetaRecord = 0xE316; |
Jamie Smith |
1:aac28ffd63ed | 1271 | break; |
Jamie Smith |
2:2269b723d16a | 1272 | case SHAKE_DETECTOR: |
Jamie Smith |
1:aac28ffd63ed | 1273 | reportMetaRecord = 0xE318; |
Jamie Smith |
1:aac28ffd63ed | 1274 | break; |
Jamie Smith |
1:aac28ffd63ed | 1275 | } |
Jamie Smith |
1:aac28ffd63ed | 1276 | |
Jamie Smith |
1:aac28ffd63ed | 1277 | // if we already have that data stored, everything's OK |
Jamie Smith |
1:aac28ffd63ed | 1278 | if(bufferMetadataRecord == reportMetaRecord) |
Jamie Smith |
1:aac28ffd63ed | 1279 | { |
Jamie Smith |
1:aac28ffd63ed | 1280 | return true; |
Jamie Smith |
1:aac28ffd63ed | 1281 | } |
Jamie Smith |
1:aac28ffd63ed | 1282 | |
Jamie Smith |
1:aac28ffd63ed | 1283 | // now, load the metadata into the buffer |
Jamie Smith |
1:aac28ffd63ed | 1284 | if(!readFRSRecord(reportMetaRecord, metadataRecord, METADATA_BUFFER_LEN)) |
Jamie Smith |
1:aac28ffd63ed | 1285 | { |
Jamie Smith |
1:aac28ffd63ed | 1286 | // clear this so future calls won't try to use the cached version |
Jamie Smith |
1:aac28ffd63ed | 1287 | bufferMetadataRecord = 0; |
Jamie Smith |
1:aac28ffd63ed | 1288 | |
Jamie Smith |
1:aac28ffd63ed | 1289 | return false; |
Jamie Smith |
1:aac28ffd63ed | 1290 | } |
Jamie Smith |
1:aac28ffd63ed | 1291 | |
Jamie Smith |
1:aac28ffd63ed | 1292 | bufferMetadataRecord = reportMetaRecord; |
Jamie Smith |
1:aac28ffd63ed | 1293 | |
Jamie Smith |
1:aac28ffd63ed | 1294 | return true; |
Jamie Smith |
1:aac28ffd63ed | 1295 | } |
Jamie Smith |
8:199c7fad233d | 1296 | |
Jamie Smith |
8:199c7fad233d | 1297 | BNO080I2C::BNO080I2C(Stream *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin, |
Jamie Smith |
8:199c7fad233d | 1298 | uint8_t i2cAddress, int i2cPortSpeed) : |
Jamie Smith |
8:199c7fad233d | 1299 | BNO080Base(debugPort, user_INTPin, user_RSTPin), |
Jamie Smith |
8:199c7fad233d | 1300 | _i2cPort(user_SDApin, user_SCLpin), |
Jamie Smith |
8:199c7fad233d | 1301 | _i2cAddress(i2cAddress) |
Jamie Smith |
8:199c7fad233d | 1302 | { |
Jamie Smith |
8:199c7fad233d | 1303 | |
Jamie Smith |
8:199c7fad233d | 1304 | //Get user settings |
Jamie Smith |
8:199c7fad233d | 1305 | _i2cPortSpeed = i2cPortSpeed; |
Jamie Smith |
8:199c7fad233d | 1306 | if(_i2cPortSpeed > 4000000) |
Jamie Smith |
8:199c7fad233d | 1307 | { |
Jamie Smith |
8:199c7fad233d | 1308 | _i2cPortSpeed = 4000000; //BNO080 max is 400Khz |
Jamie Smith |
8:199c7fad233d | 1309 | } |
Jamie Smith |
8:199c7fad233d | 1310 | _i2cPort.frequency(_i2cPortSpeed); |
Jamie Smith |
8:199c7fad233d | 1311 | } |
Jamie Smith |
8:199c7fad233d | 1312 | |
Jamie Smith |
8:199c7fad233d | 1313 | |
Jamie Smith |
8:199c7fad233d | 1314 | //Given the data packet, send the header then the data |
Jamie Smith |
8:199c7fad233d | 1315 | //Returns false if sensor does not ACK |
Jamie Smith |
8:199c7fad233d | 1316 | bool BNO080I2C::sendPacket(uint8_t channelNumber, uint8_t dataLength) |
Jamie Smith |
8:199c7fad233d | 1317 | { |
Jamie Smith |
8:199c7fad233d | 1318 | |
Jamie Smith |
8:199c7fad233d | 1319 | uint16_t totalLength = dataLength + 4; //Add four bytes for the header |
Jamie Smith |
8:199c7fad233d | 1320 | |
Jamie Smith |
8:199c7fad233d | 1321 | txShtpHeader[0] = totalLength & 0xFF; |
Jamie Smith |
8:199c7fad233d | 1322 | txShtpHeader[1] = totalLength >> 8; |
Jamie Smith |
8:199c7fad233d | 1323 | txShtpHeader[2] = channelNumber; |
Jamie Smith |
8:199c7fad233d | 1324 | txShtpHeader[3] = sequenceNumber[channelNumber]; |
Jamie Smith |
8:199c7fad233d | 1325 | |
Jamie Smith |
8:199c7fad233d | 1326 | #if BNO_DEBUG |
Jamie Smith |
8:199c7fad233d | 1327 | _debugPort->printf("Transmitting packet: ----------------\n"); |
Jamie Smith |
8:199c7fad233d | 1328 | printPacket(txPacketBuffer); |
Jamie Smith |
8:199c7fad233d | 1329 | #endif |
Jamie Smith |
8:199c7fad233d | 1330 | |
Jamie Smith |
8:199c7fad233d | 1331 | // send packet to IMU |
Jamie Smith |
8:199c7fad233d | 1332 | int writeResult = _i2cPort.write(_i2cAddress << 1, reinterpret_cast<char*>(txPacketBuffer), totalLength); |
Jamie Smith |
8:199c7fad233d | 1333 | |
Jamie Smith |
8:199c7fad233d | 1334 | if(writeResult != 0) |
Jamie Smith |
8:199c7fad233d | 1335 | { |
Jamie Smith |
8:199c7fad233d | 1336 | _debugPort->printf("BNO I2C write failed!\n"); |
Jamie Smith |
8:199c7fad233d | 1337 | return false; |
Jamie Smith |
8:199c7fad233d | 1338 | } |
Jamie Smith |
8:199c7fad233d | 1339 | return true; |
Jamie Smith |
8:199c7fad233d | 1340 | } |
Jamie Smith |
8:199c7fad233d | 1341 | |
Jamie Smith |
8:199c7fad233d | 1342 | //Check to see if there is any new data available |
Jamie Smith |
8:199c7fad233d | 1343 | //Read the contents of the incoming packet into the shtpData array |
Jamie Smith |
8:199c7fad233d | 1344 | bool BNO080I2C::receivePacket(std::chrono::milliseconds timeout) |
Jamie Smith |
8:199c7fad233d | 1345 | { |
Jamie Smith |
8:199c7fad233d | 1346 | Timer waitStartTime; |
Jamie Smith |
8:199c7fad233d | 1347 | waitStartTime.start(); |
Jamie Smith |
8:199c7fad233d | 1348 | |
Jamie Smith |
8:199c7fad233d | 1349 | while(_int.read() != 0) |
Jamie Smith |
8:199c7fad233d | 1350 | { |
Jamie Smith |
8:199c7fad233d | 1351 | if(waitStartTime.elapsed_time() > timeout) |
Jamie Smith |
8:199c7fad233d | 1352 | { |
Jamie Smith |
8:199c7fad233d | 1353 | _debugPort->printf("BNO I2C header wait timeout\n"); |
Jamie Smith |
8:199c7fad233d | 1354 | return false; |
Jamie Smith |
8:199c7fad233d | 1355 | } |
Jamie Smith |
8:199c7fad233d | 1356 | |
Jamie Smith |
8:199c7fad233d | 1357 | } |
Jamie Smith |
8:199c7fad233d | 1358 | |
Jamie Smith |
8:199c7fad233d | 1359 | // start the transaction and contact the IMU |
Jamie Smith |
8:199c7fad233d | 1360 | _i2cPort.start(); |
Jamie Smith |
8:199c7fad233d | 1361 | |
Jamie Smith |
8:199c7fad233d | 1362 | // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1 |
Jamie Smith |
8:199c7fad233d | 1363 | int writeResult = _i2cPort.write((_i2cAddress << 1) | 0x1); |
Jamie Smith |
8:199c7fad233d | 1364 | |
Jamie Smith |
8:199c7fad233d | 1365 | if(writeResult != 1) |
Jamie Smith |
8:199c7fad233d | 1366 | { |
Jamie Smith |
8:199c7fad233d | 1367 | _debugPort->printf("BNO I2C read failed!\n"); |
Jamie Smith |
8:199c7fad233d | 1368 | return false; |
Jamie Smith |
8:199c7fad233d | 1369 | } |
Jamie Smith |
8:199c7fad233d | 1370 | |
Jamie Smith |
8:199c7fad233d | 1371 | //Get the first four bytes, aka the packet header |
Jamie Smith |
8:199c7fad233d | 1372 | uint8_t packetLSB = static_cast<uint8_t>(_i2cPort.read(true)); |
Jamie Smith |
8:199c7fad233d | 1373 | uint8_t packetMSB = static_cast<uint8_t>(_i2cPort.read(true)); |
Jamie Smith |
8:199c7fad233d | 1374 | uint8_t channelNumber = static_cast<uint8_t>(_i2cPort.read(true)); |
Jamie Smith |
8:199c7fad233d | 1375 | uint8_t sequenceNum = static_cast<uint8_t>(_i2cPort.read(true)); //Not sure if we need to store this or not |
Jamie Smith |
8:199c7fad233d | 1376 | |
Jamie Smith |
8:199c7fad233d | 1377 | //Store the header info |
Jamie Smith |
8:199c7fad233d | 1378 | rxShtpHeader[0] = packetLSB; |
Jamie Smith |
8:199c7fad233d | 1379 | rxShtpHeader[1] = packetMSB; |
Jamie Smith |
8:199c7fad233d | 1380 | rxShtpHeader[2] = channelNumber; |
Jamie Smith |
8:199c7fad233d | 1381 | rxShtpHeader[3] = sequenceNum; |
Jamie Smith |
8:199c7fad233d | 1382 | |
Jamie Smith |
8:199c7fad233d | 1383 | if(rxShtpHeader[0] == 0xFF && rxShtpHeader[1] == 0xFF) |
Jamie Smith |
8:199c7fad233d | 1384 | { |
Jamie Smith |
8:199c7fad233d | 1385 | // invalid according to BNO080 datasheet section 1.4.1 |
Jamie Smith |
8:199c7fad233d | 1386 | |
Jamie Smith |
8:199c7fad233d | 1387 | #if BNO_DEBUG |
Jamie Smith |
8:199c7fad233d | 1388 | _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n"); |
Jamie Smith |
8:199c7fad233d | 1389 | #endif |
Jamie Smith |
8:199c7fad233d | 1390 | return false; |
Jamie Smith |
8:199c7fad233d | 1391 | } |
Jamie Smith |
8:199c7fad233d | 1392 | |
Jamie Smith |
8:199c7fad233d | 1393 | //Calculate the number of data bytes in this packet |
Jamie Smith |
8:199c7fad233d | 1394 | rxPacketLength = (static_cast<uint16_t>(packetMSB) << 8 | packetLSB); |
Jamie Smith |
8:199c7fad233d | 1395 | |
Jamie Smith |
8:199c7fad233d | 1396 | // Clear the MSbit. |
Jamie Smith |
8:199c7fad233d | 1397 | // 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 |
8:199c7fad233d | 1398 | // but we don't actually care about any of the advertisement packets |
Jamie Smith |
8:199c7fad233d | 1399 | // that use this, so we can just cut off the rest of the packet by releasing chip select. |
Jamie Smith |
8:199c7fad233d | 1400 | rxPacketLength &= ~(1 << 15); |
Jamie Smith |
8:199c7fad233d | 1401 | |
Jamie Smith |
8:199c7fad233d | 1402 | if (rxPacketLength == 0) |
Jamie Smith |
8:199c7fad233d | 1403 | { |
Jamie Smith |
8:199c7fad233d | 1404 | // Packet is empty |
Jamie Smith |
8:199c7fad233d | 1405 | return (false); //All done |
Jamie Smith |
8:199c7fad233d | 1406 | } |
Jamie Smith |
8:199c7fad233d | 1407 | |
Jamie Smith |
8:199c7fad233d | 1408 | rxPacketLength -= 4; //Remove the header bytes from the data count |
Jamie Smith |
8:199c7fad233d | 1409 | |
Jamie Smith |
8:199c7fad233d | 1410 | //Read incoming data into the shtpData array |
Jamie Smith |
8:199c7fad233d | 1411 | for (uint16_t dataSpot = 0 ; dataSpot < rxPacketLength ; dataSpot++) |
Jamie Smith |
8:199c7fad233d | 1412 | { |
Jamie Smith |
8:199c7fad233d | 1413 | bool sendACK = dataSpot < rxPacketLength - 1; |
Jamie Smith |
8:199c7fad233d | 1414 | |
Jamie Smith |
8:199c7fad233d | 1415 | // per the datasheet, 0xFF is used as filler for the receiver to transmit back |
Jamie Smith |
8:199c7fad233d | 1416 | uint8_t incoming = static_cast<uint8_t>(_i2cPort.read(sendACK)); |
Jamie Smith |
8:199c7fad233d | 1417 | if (dataSpot < (sizeof(rxPacketBuffer) - SHTP_HEADER_SIZE)) //BNO080 can respond with upto 270 bytes, avoid overflow |
Jamie Smith |
8:199c7fad233d | 1418 | rxShtpData[dataSpot] = incoming; //Store data into the shtpData array |
Jamie Smith |
8:199c7fad233d | 1419 | } |
Jamie Smith |
8:199c7fad233d | 1420 | |
Jamie Smith |
8:199c7fad233d | 1421 | _i2cPort.stop(); |
Jamie Smith |
8:199c7fad233d | 1422 | |
Jamie Smith |
8:199c7fad233d | 1423 | /* |
Jamie Smith |
8:199c7fad233d | 1424 | // first read the header to get the length of the packet |
Jamie Smith |
8:199c7fad233d | 1425 | int i2cResult; |
Jamie Smith |
8:199c7fad233d | 1426 | i2cResult = _i2cPort.read(_i2cAddress << 1, reinterpret_cast<char*>(packetBuffer), SHTP_HEADER_SIZE, false); |
Jamie Smith |
8:199c7fad233d | 1427 | if(i2cResult != 0) |
Jamie Smith |
8:199c7fad233d | 1428 | { |
Jamie Smith |
8:199c7fad233d | 1429 | _debugPort->printf("BNO I2C length read failed!\n"); |
Jamie Smith |
8:199c7fad233d | 1430 | return false; |
Jamie Smith |
8:199c7fad233d | 1431 | } |
Jamie Smith |
8:199c7fad233d | 1432 | |
Jamie Smith |
8:199c7fad233d | 1433 | if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF) |
Jamie Smith |
8:199c7fad233d | 1434 | { |
Jamie Smith |
8:199c7fad233d | 1435 | // invalid according to BNO080 datasheet section 1.4.1 |
Jamie Smith |
8:199c7fad233d | 1436 | #if BNO_DEBUG |
Jamie Smith |
8:199c7fad233d | 1437 | _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n"); |
Jamie Smith |
8:199c7fad233d | 1438 | #endif |
Jamie Smith |
8:199c7fad233d | 1439 | return false; |
Jamie Smith |
8:199c7fad233d | 1440 | } |
Jamie Smith |
8:199c7fad233d | 1441 | |
Jamie Smith |
8:199c7fad233d | 1442 | //Calculate the number of data bytes in this packet |
Jamie Smith |
8:199c7fad233d | 1443 | uint16_t totalLength = (static_cast<uint16_t>(shtpHeader[1]) << 8 | shtpHeader[0]); |
Jamie Smith |
8:199c7fad233d | 1444 | |
Jamie Smith |
8:199c7fad233d | 1445 | // Clear the MSbit. |
Jamie Smith |
8:199c7fad233d | 1446 | // 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 |
8:199c7fad233d | 1447 | // but we don't actually care about any of the advertisement packets |
Jamie Smith |
8:199c7fad233d | 1448 | // that use this, so we can just cut off the rest of the packet by releasing chip select. |
Jamie Smith |
8:199c7fad233d | 1449 | totalLength &= ~(1 << 15); |
Jamie Smith |
8:199c7fad233d | 1450 | |
Jamie Smith |
8:199c7fad233d | 1451 | if (totalLength == 0) |
Jamie Smith |
8:199c7fad233d | 1452 | { |
Jamie Smith |
8:199c7fad233d | 1453 | // Packet is empty |
Jamie Smith |
8:199c7fad233d | 1454 | return (false); //All done |
Jamie Smith |
8:199c7fad233d | 1455 | } |
Jamie Smith |
8:199c7fad233d | 1456 | |
Jamie Smith |
8:199c7fad233d | 1457 | // only receive as many bytes as we can fit |
Jamie Smith |
8:199c7fad233d | 1458 | if(totalLength > sizeof(packetBuffer)) |
Jamie Smith |
8:199c7fad233d | 1459 | { |
Jamie Smith |
8:199c7fad233d | 1460 | totalLength = sizeof(packetBuffer); |
Jamie Smith |
8:199c7fad233d | 1461 | } |
Jamie Smith |
8:199c7fad233d | 1462 | |
Jamie Smith |
8:199c7fad233d | 1463 | packetLength = totalLength - 4; //Remove the header bytes from the data count |
Jamie Smith |
8:199c7fad233d | 1464 | |
Jamie Smith |
8:199c7fad233d | 1465 | waitStartTime.reset(); |
Jamie Smith |
8:199c7fad233d | 1466 | |
Jamie Smith |
8:199c7fad233d | 1467 | while(_int.read() != 0) |
Jamie Smith |
8:199c7fad233d | 1468 | { |
Jamie Smith |
8:199c7fad233d | 1469 | if(waitStartTime.elapsed_time() > timeout) |
Jamie Smith |
8:199c7fad233d | 1470 | { |
Jamie Smith |
8:199c7fad233d | 1471 | _debugPort->printf("BNO I2C wait timeout\n"); |
Jamie Smith |
8:199c7fad233d | 1472 | return false; |
Jamie Smith |
8:199c7fad233d | 1473 | } |
Jamie Smith |
8:199c7fad233d | 1474 | |
Jamie Smith |
8:199c7fad233d | 1475 | } |
Jamie Smith |
8:199c7fad233d | 1476 | |
Jamie Smith |
8:199c7fad233d | 1477 | //Read the actual packet bytes |
Jamie Smith |
8:199c7fad233d | 1478 | _debugPort->printf("Attempting to read %" PRIu16 " bytes\n", totalLength); |
Jamie Smith |
8:199c7fad233d | 1479 | i2cResult = _i2cPort.read(_i2cAddress << 1, reinterpret_cast<char*>(packetBuffer), totalLength); |
Jamie Smith |
8:199c7fad233d | 1480 | if(i2cResult != 0) |
Jamie Smith |
8:199c7fad233d | 1481 | { |
Jamie Smith |
8:199c7fad233d | 1482 | _debugPort->printf("BNO I2C read failed!\n"); |
Jamie Smith |
8:199c7fad233d | 1483 | return false; |
Jamie Smith |
8:199c7fad233d | 1484 | } |
Jamie Smith |
8:199c7fad233d | 1485 | */ |
Jamie Smith |
8:199c7fad233d | 1486 | |
Jamie Smith |
8:199c7fad233d | 1487 | #if BNO_DEBUG |
Jamie Smith |
8:199c7fad233d | 1488 | _debugPort->printf("Recieved packet: ----------------\n"); |
Jamie Smith |
8:199c7fad233d | 1489 | printPacket(rxPacketBuffer); // note: add 4 for the header length |
Jamie Smith |
8:199c7fad233d | 1490 | #endif |
Jamie Smith |
8:199c7fad233d | 1491 | |
Jamie Smith |
8:199c7fad233d | 1492 | return (true); //We're done! |
Jamie Smith |
8:199c7fad233d | 1493 | } |
Jamie Smith |
8:199c7fad233d | 1494 | |
Jamie Smith |
8:199c7fad233d | 1495 | BNO080SPI::BNO080SPI(Stream *debugPort, PinName rstPin, PinName intPin, PinName wakePin, PinName misoPin, |
Jamie Smith |
8:199c7fad233d | 1496 | PinName mosiPin, PinName sclkPin, PinName csPin, int spiSpeed): |
Jamie Smith |
8:199c7fad233d | 1497 | BNO080Base(debugPort, intPin, rstPin), |
Jamie Smith |
8:199c7fad233d | 1498 | _spiPort(mosiPin, misoPin, sclkPin, csPin, use_gpio_ssel), |
Jamie Smith |
8:199c7fad233d | 1499 | _wakePin(wakePin, 1), // wake pin needs to be 1 on reset so that the BNO boots up into SPI mode |
Jamie Smith |
8:199c7fad233d | 1500 | _spiSpeed(spiSpeed) |
Jamie Smith |
8:199c7fad233d | 1501 | { |
Jamie Smith |
8:199c7fad233d | 1502 | _spiPort.frequency(spiSpeed); |
Jamie Smith |
8:199c7fad233d | 1503 | _spiPort.format(8, 3); |
Jamie Smith |
8:199c7fad233d | 1504 | _spiPort.set_default_write_value(0x0); |
Jamie Smith |
8:199c7fad233d | 1505 | } |
Jamie Smith |
8:199c7fad233d | 1506 | |
Jamie Smith |
8:199c7fad233d | 1507 | bool BNO080SPI::sendPacket(uint8_t channelNumber, uint8_t dataLength) |
Jamie Smith |
8:199c7fad233d | 1508 | { |
Jamie Smith |
8:199c7fad233d | 1509 | if(_int.read() == 0) |
Jamie Smith |
8:199c7fad233d | 1510 | { |
Jamie Smith |
8:199c7fad233d | 1511 | // The BNO is already awake because it has a packet it wants to send to us |
Jamie Smith |
8:199c7fad233d | 1512 | } |
Jamie Smith |
8:199c7fad233d | 1513 | else |
Jamie Smith |
8:199c7fad233d | 1514 | { |
Jamie Smith |
8:199c7fad233d | 1515 | // assert WAKE to tell the BNO to prepare for a transfer |
Jamie Smith |
8:199c7fad233d | 1516 | _wakePin = 0; |
Jamie Smith |
8:199c7fad233d | 1517 | |
Jamie Smith |
8:199c7fad233d | 1518 | Timer waitStartTime; |
Jamie Smith |
8:199c7fad233d | 1519 | waitStartTime.start(); |
Jamie Smith |
8:199c7fad233d | 1520 | const std::chrono::milliseconds timeout = 10ms; |
Jamie Smith |
8:199c7fad233d | 1521 | |
Jamie Smith |
8:199c7fad233d | 1522 | while(_int.read() != 0) |
Jamie Smith |
8:199c7fad233d | 1523 | { |
Jamie Smith |
8:199c7fad233d | 1524 | if(waitStartTime.elapsed_time() > timeout) |
Jamie Smith |
8:199c7fad233d | 1525 | { |
Jamie Smith |
8:199c7fad233d | 1526 | _debugPort->printf("BNO SPI wake wait timeout\n"); |
Jamie Smith |
8:199c7fad233d | 1527 | _wakePin = 1; |
Jamie Smith |
8:199c7fad233d | 1528 | return false; |
Jamie Smith |
8:199c7fad233d | 1529 | } |
Jamie Smith |
8:199c7fad233d | 1530 | } |
Jamie Smith |
8:199c7fad233d | 1531 | _wakePin = 1; |
Jamie Smith |
8:199c7fad233d | 1532 | } |
Jamie Smith |
8:199c7fad233d | 1533 | |
Jamie Smith |
8:199c7fad233d | 1534 | uint16_t totalLength = dataLength + 4; //Add four bytes for the header |
Jamie Smith |
8:199c7fad233d | 1535 | |
Jamie Smith |
8:199c7fad233d | 1536 | txShtpHeader[0] = totalLength & 0xFF; |
Jamie Smith |
8:199c7fad233d | 1537 | txShtpHeader[1] = totalLength >> 8; |
Jamie Smith |
8:199c7fad233d | 1538 | txShtpHeader[2] = channelNumber; |
Jamie Smith |
8:199c7fad233d | 1539 | txShtpHeader[3] = sequenceNumber[channelNumber]; |
Jamie Smith |
8:199c7fad233d | 1540 | |
Jamie Smith |
8:199c7fad233d | 1541 | #if BNO_DEBUG |
Jamie Smith |
8:199c7fad233d | 1542 | _debugPort->printf("Transmitting packet: ----------------\n"); |
Jamie Smith |
8:199c7fad233d | 1543 | printPacket(txPacketBuffer); |
Jamie Smith |
8:199c7fad233d | 1544 | #endif |
Jamie Smith |
8:199c7fad233d | 1545 | |
Jamie Smith |
8:199c7fad233d | 1546 | // wipe out any existing RX packet header so we know if we received a packet |
Jamie Smith |
8:199c7fad233d | 1547 | memset(rxPacketBuffer, 0, SHTP_HEADER_SIZE); |
Jamie Smith |
8:199c7fad233d | 1548 | |
Jamie Smith |
8:199c7fad233d | 1549 | // send packet to IMU. |
Jamie Smith |
8:199c7fad233d | 1550 | // This also might receive the first part of another packet, which there is no way to avoid. |
Jamie Smith |
9:430f5302f9e1 | 1551 | spiTransferAndWait(txPacketBuffer, totalLength, rxPacketBuffer, totalLength); |
Jamie Smith |
8:199c7fad233d | 1552 | |
Jamie Smith |
8:199c7fad233d | 1553 | if(rxShtpHeader[0] == 0 && rxShtpHeader[0] == 0) |
Jamie Smith |
8:199c7fad233d | 1554 | { |
Jamie Smith |
8:199c7fad233d | 1555 | // no header data so no packet received |
Jamie Smith |
8:199c7fad233d | 1556 | return true; |
Jamie Smith |
8:199c7fad233d | 1557 | } |
Jamie Smith |
8:199c7fad233d | 1558 | else |
Jamie Smith |
8:199c7fad233d | 1559 | { |
Jamie Smith |
8:199c7fad233d | 1560 | // received first part of data packet while writing |
Jamie Smith |
8:199c7fad233d | 1561 | if(receiveCompletePacket(totalLength)) |
Jamie Smith |
8:199c7fad233d | 1562 | { |
Jamie Smith |
8:199c7fad233d | 1563 | // received data packet, send to proper channels |
Jamie Smith |
8:199c7fad233d | 1564 | processPacket(); |
Jamie Smith |
8:199c7fad233d | 1565 | return true; |
Jamie Smith |
8:199c7fad233d | 1566 | } |
Jamie Smith |
8:199c7fad233d | 1567 | |
Jamie Smith |
8:199c7fad233d | 1568 | // receive failed |
Jamie Smith |
8:199c7fad233d | 1569 | return false; |
Jamie Smith |
8:199c7fad233d | 1570 | } |
Jamie Smith |
8:199c7fad233d | 1571 | } |
Jamie Smith |
8:199c7fad233d | 1572 | |
Jamie Smith |
8:199c7fad233d | 1573 | //Check to see if there is any new data available |
Jamie Smith |
8:199c7fad233d | 1574 | //Read the contents of the incoming packet into the shtpData array |
Jamie Smith |
8:199c7fad233d | 1575 | bool BNO080SPI::receivePacket(std::chrono::milliseconds timeout) |
Jamie Smith |
8:199c7fad233d | 1576 | { |
Jamie Smith |
8:199c7fad233d | 1577 | Timer waitStartTime; |
Jamie Smith |
8:199c7fad233d | 1578 | waitStartTime.start(); |
Jamie Smith |
8:199c7fad233d | 1579 | |
Jamie Smith |
8:199c7fad233d | 1580 | while (_int.read() != 0) |
Jamie Smith |
8:199c7fad233d | 1581 | { |
Jamie Smith |
8:199c7fad233d | 1582 | if (waitStartTime.elapsed_time() > timeout) |
Jamie Smith |
8:199c7fad233d | 1583 | { |
Jamie Smith |
8:199c7fad233d | 1584 | _debugPort->printf("BNO SPI header wait timeout\n"); |
Jamie Smith |
8:199c7fad233d | 1585 | return false; |
Jamie Smith |
8:199c7fad233d | 1586 | } |
Jamie Smith |
8:199c7fad233d | 1587 | |
Jamie Smith |
8:199c7fad233d | 1588 | } |
Jamie Smith |
8:199c7fad233d | 1589 | |
Jamie Smith |
9:430f5302f9e1 | 1590 | // read the header bytes first. |
Jamie Smith |
9:430f5302f9e1 | 1591 | spiTransferAndWait(nullptr, 0, rxPacketBuffer, SHTP_HEADER_SIZE); |
Jamie Smith |
8:199c7fad233d | 1592 | |
Jamie Smith |
8:199c7fad233d | 1593 | // now read the data |
Jamie Smith |
8:199c7fad233d | 1594 | return receiveCompletePacket(SHTP_HEADER_SIZE, timeout); |
Jamie Smith |
8:199c7fad233d | 1595 | } |
Jamie Smith |
8:199c7fad233d | 1596 | |
Jamie Smith |
8:199c7fad233d | 1597 | bool BNO080SPI::receiveCompletePacket(size_t bytesRead, std::chrono::milliseconds timeout) |
Jamie Smith |
8:199c7fad233d | 1598 | { |
Jamie Smith |
8:199c7fad233d | 1599 | // Process header bytes |
Jamie Smith |
8:199c7fad233d | 1600 | // ------------------------------------------------------------------------ |
Jamie Smith |
8:199c7fad233d | 1601 | if (rxShtpHeader[0] == 0xFF && rxShtpHeader[1] == 0xFF) |
Jamie Smith |
8:199c7fad233d | 1602 | { |
Jamie Smith |
8:199c7fad233d | 1603 | // invalid according to BNO080 datasheet section 1.4.1 |
Jamie Smith |
8:199c7fad233d | 1604 | |
Jamie Smith |
8:199c7fad233d | 1605 | #if BNO_DEBUG |
Jamie Smith |
8:199c7fad233d | 1606 | _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n"); |
Jamie Smith |
8:199c7fad233d | 1607 | #endif |
Jamie Smith |
8:199c7fad233d | 1608 | return false; |
Jamie Smith |
8:199c7fad233d | 1609 | } |
Jamie Smith |
8:199c7fad233d | 1610 | |
Jamie Smith |
8:199c7fad233d | 1611 | //Calculate the number of data bytes in this packet |
Jamie Smith |
8:199c7fad233d | 1612 | uint16_t totalLength = (static_cast<uint16_t>(rxShtpHeader[1]) << 8 | rxShtpHeader[0]); |
Jamie Smith |
8:199c7fad233d | 1613 | |
Jamie Smith |
8:199c7fad233d | 1614 | // Clear the MSbit. |
Jamie Smith |
8:199c7fad233d | 1615 | totalLength &= ~(1 << 15); |
Jamie Smith |
8:199c7fad233d | 1616 | |
Jamie Smith |
8:199c7fad233d | 1617 | if (totalLength == 0) |
Jamie Smith |
8:199c7fad233d | 1618 | { |
Jamie Smith |
8:199c7fad233d | 1619 | // Packet is empty |
Jamie Smith |
8:199c7fad233d | 1620 | return (false); //All done |
Jamie Smith |
8:199c7fad233d | 1621 | } |
Jamie Smith |
8:199c7fad233d | 1622 | |
Jamie Smith |
8:199c7fad233d | 1623 | rxPacketLength = totalLength - SHTP_HEADER_SIZE; |
Jamie Smith |
8:199c7fad233d | 1624 | |
Jamie Smith |
8:199c7fad233d | 1625 | if(totalLength <= bytesRead) |
Jamie Smith |
8:199c7fad233d | 1626 | { |
Jamie Smith |
8:199c7fad233d | 1627 | // the original transaction already read the completed packet! We're done. |
Jamie Smith |
8:199c7fad233d | 1628 | return true; |
Jamie Smith |
8:199c7fad233d | 1629 | } |
Jamie Smith |
8:199c7fad233d | 1630 | |
Jamie Smith |
8:199c7fad233d | 1631 | // Receive data |
Jamie Smith |
8:199c7fad233d | 1632 | // ------------------------------------------------------------------------ |
Jamie Smith |
8:199c7fad233d | 1633 | |
Jamie Smith |
8:199c7fad233d | 1634 | // Wait for it to be ready to talk to us again. |
Jamie Smith |
8:199c7fad233d | 1635 | // Note: in my testing this takes about 200ms |
Jamie Smith |
8:199c7fad233d | 1636 | Timer waitStartTime; |
Jamie Smith |
8:199c7fad233d | 1637 | waitStartTime.start(); |
Jamie Smith |
8:199c7fad233d | 1638 | while (_int.read() != 0) |
Jamie Smith |
8:199c7fad233d | 1639 | { |
Jamie Smith |
8:199c7fad233d | 1640 | if (waitStartTime.elapsed_time() > timeout) |
Jamie Smith |
8:199c7fad233d | 1641 | { |
Jamie Smith |
8:199c7fad233d | 1642 | _debugPort->printf("BNO SPI continued packet header wait timeout\n"); |
Jamie Smith |
8:199c7fad233d | 1643 | return false; |
Jamie Smith |
8:199c7fad233d | 1644 | } |
Jamie Smith |
8:199c7fad233d | 1645 | |
Jamie Smith |
8:199c7fad233d | 1646 | } |
Jamie Smith |
8:199c7fad233d | 1647 | |
Jamie Smith |
8:199c7fad233d | 1648 | if(rxPacketLength > SHTP_RX_PACKET_SIZE) |
Jamie Smith |
8:199c7fad233d | 1649 | { |
Jamie Smith |
8:199c7fad233d | 1650 | _debugPort->printf("Packet too long (%" PRIu16 " bytes), increase SHTP_RX_PACKET_SIZE\n", rxPacketLength); |
Jamie Smith |
8:199c7fad233d | 1651 | _debugPort->printf("Packet dropped, expect subsequent driver errors.\n"); |
Jamie Smith |
8:199c7fad233d | 1652 | return false; |
Jamie Smith |
8:199c7fad233d | 1653 | } |
Jamie Smith |
8:199c7fad233d | 1654 | |
Jamie Smith |
8:199c7fad233d | 1655 | if(bytesRead == SHTP_HEADER_SIZE) |
Jamie Smith |
8:199c7fad233d | 1656 | { |
Jamie Smith |
8:199c7fad233d | 1657 | // just read the entire packet into the buffer |
Jamie Smith |
9:430f5302f9e1 | 1658 | spiTransferAndWait(nullptr, 0, rxPacketBuffer, totalLength); |
Jamie Smith |
8:199c7fad233d | 1659 | } |
Jamie Smith |
8:199c7fad233d | 1660 | else |
Jamie Smith |
8:199c7fad233d | 1661 | { |
Jamie Smith |
8:199c7fad233d | 1662 | // we want to receive a new header, plus the remaining data bytes that haven't been read. |
Jamie Smith |
8:199c7fad233d | 1663 | size_t receiveLength = SHTP_HEADER_SIZE + (totalLength - bytesRead); |
Jamie Smith |
8:199c7fad233d | 1664 | |
Jamie Smith |
8:199c7fad233d | 1665 | // read remaining bytes into the data buffer starting at the next byte |
Jamie Smith |
9:430f5302f9e1 | 1666 | spiTransferAndWait(nullptr, 0, rxPacketBuffer + bytesRead, receiveLength); |
Jamie Smith |
8:199c7fad233d | 1667 | |
Jamie Smith |
8:199c7fad233d | 1668 | // erase the new header we just read, leaving only the data as a contiguous block |
Jamie Smith |
8:199c7fad233d | 1669 | std::memmove(rxPacketBuffer + bytesRead, rxPacketBuffer + bytesRead + SHTP_HEADER_SIZE, receiveLength - SHTP_HEADER_SIZE); |
Jamie Smith |
8:199c7fad233d | 1670 | } |
Jamie Smith |
8:199c7fad233d | 1671 | |
Jamie Smith |
8:199c7fad233d | 1672 | #if BNO_DEBUG |
Jamie Smith |
8:199c7fad233d | 1673 | _debugPort->printf("Recieved packet: ----------------\n"); |
Jamie Smith |
8:199c7fad233d | 1674 | printPacket(rxPacketBuffer); |
Jamie Smith |
8:199c7fad233d | 1675 | #endif |
Jamie Smith |
8:199c7fad233d | 1676 | |
Jamie Smith |
8:199c7fad233d | 1677 | //ThisThread::sleep_for(4ms); |
Jamie Smith |
8:199c7fad233d | 1678 | |
Jamie Smith |
8:199c7fad233d | 1679 | // done! |
Jamie Smith |
8:199c7fad233d | 1680 | return true; |
Jamie Smith |
9:430f5302f9e1 | 1681 | } |
Jamie Smith |
9:430f5302f9e1 | 1682 | |
Jamie Smith |
9:430f5302f9e1 | 1683 | #if USE_ASYNC_SPI |
Jamie Smith |
9:430f5302f9e1 | 1684 | |
Jamie Smith |
9:430f5302f9e1 | 1685 | void BNO080SPI::spiTransferAndWait(const uint8_t *tx_buffer, int tx_length, uint8_t *rx_buffer, int rx_length) |
Jamie Smith |
9:430f5302f9e1 | 1686 | { |
Jamie Smith |
9:430f5302f9e1 | 1687 | _spiPort.transfer(tx_buffer, tx_length, rx_buffer, rx_length, |
Jamie Smith |
9:430f5302f9e1 | 1688 | callback(this, &BNO080SPI::onSPITransferComplete), |
Jamie Smith |
9:430f5302f9e1 | 1689 | SPI_EVENT_COMPLETE | SPI_EVENT_ERROR); |
Jamie Smith |
9:430f5302f9e1 | 1690 | |
Jamie Smith |
9:430f5302f9e1 | 1691 | uint32_t waitResult = spiCompleteFlag.wait_any(SPI_EVENT_ALL); |
Jamie Smith |
9:430f5302f9e1 | 1692 | if(!(waitResult & SPI_EVENT_COMPLETE)) |
Jamie Smith |
9:430f5302f9e1 | 1693 | { |
Jamie Smith |
9:430f5302f9e1 | 1694 | // at least let the user know the error happened... |
Jamie Smith |
9:430f5302f9e1 | 1695 | _debugPort->printf("BNO Async SPI Error %" PRIu32 "\n", waitResult); |
Jamie Smith |
9:430f5302f9e1 | 1696 | } |
Jamie Smith |
9:430f5302f9e1 | 1697 | |
Jamie Smith |
9:430f5302f9e1 | 1698 | } |
Jamie Smith |
9:430f5302f9e1 | 1699 | |
Jamie Smith |
9:430f5302f9e1 | 1700 | void BNO080SPI::onSPITransferComplete(int event) |
Jamie Smith |
9:430f5302f9e1 | 1701 | { |
Jamie Smith |
9:430f5302f9e1 | 1702 | spiCompleteFlag.set(event); |
Jamie Smith |
9:430f5302f9e1 | 1703 | } |
Jamie Smith |
9:430f5302f9e1 | 1704 | |
Jamie Smith |
9:430f5302f9e1 | 1705 | #endif |