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