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