Sheila Pham
start the wrapper burrito
BNO080.cpp@0:f677e13975d0, 2018-12-23 (annotated)
- Committer:
- MultipleMonomials
- Date:
- Sun Dec 23 05:23:21 2018 +0000
- Revision:
- 0:f677e13975d0
- Child:
- 1:aac28ffd63ed
Initial commit
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| MultipleMonomials | 0:f677e13975d0 | 1 | // |
| MultipleMonomials | 0:f677e13975d0 | 2 | // USC RPL BNO080 driver. |
| MultipleMonomials | 0:f677e13975d0 | 3 | // |
| MultipleMonomials | 0:f677e13975d0 | 4 | |
| MultipleMonomials | 0:f677e13975d0 | 5 | /* |
| MultipleMonomials | 0:f677e13975d0 | 6 | * Overview of BNO080 Communications |
| MultipleMonomials | 0:f677e13975d0 | 7 | * =============================================== |
| MultipleMonomials | 0:f677e13975d0 | 8 | * |
| MultipleMonomials | 0:f677e13975d0 | 9 | * Hilcrest has developed a protocol called SHTP (Sensor Hub Transport Protocol) for binary communications with |
| MultipleMonomials | 0:f677e13975d0 | 10 | * the BNO080 and the other IMUs it sells. Over this protocol, SH-2 (Sensor Hub 2) messages are sent to configure |
| MultipleMonomials | 0:f677e13975d0 | 11 | * the chip and read data back. |
| MultipleMonomials | 0:f677e13975d0 | 12 | * |
| MultipleMonomials | 0:f677e13975d0 | 13 | * SHTP messages are divided at two hierarchical levels: first the channel, then the report ID. Each category |
| MultipleMonomials | 0:f677e13975d0 | 14 | * of messages (system commands, sensor data reports, etc.) has its own channel, and the individual messages |
| MultipleMonomials | 0:f677e13975d0 | 15 | * in each channel are identified by their report id, which is the first byte of the message payload (note that the |
| MultipleMonomials | 0:f677e13975d0 | 16 | * datasheets don't *always* call the first byte the report ID, but that byte does identify the report, so I'm going |
| MultipleMonomials | 0:f677e13975d0 | 17 | * with it). |
| MultipleMonomials | 0:f677e13975d0 | 18 | * |
| MultipleMonomials | 0:f677e13975d0 | 19 | * =============================================== |
| MultipleMonomials | 0:f677e13975d0 | 20 | * |
| MultipleMonomials | 0:f677e13975d0 | 21 | * Information about the BNO080 is split into three datasheets. Here's the download links and what they cover: |
| MultipleMonomials | 0:f677e13975d0 | 22 | * |
| MultipleMonomials | 0:f677e13975d0 | 23 | * - the BNO080 datasheet: http://www.hillcrestlabs.com/download/5a05f340566d07c196001ec1 |
| MultipleMonomials | 0:f677e13975d0 | 24 | * -- Chip pinouts |
| MultipleMonomials | 0:f677e13975d0 | 25 | * -- Example circuits |
| MultipleMonomials | 0:f677e13975d0 | 26 | * -- Physical specifications |
| MultipleMonomials | 0:f677e13975d0 | 27 | * -- Supported reports and configuration settings (at a high level) |
| MultipleMonomials | 0:f677e13975d0 | 28 | * -- List of packets on the SHTP executable channel |
| MultipleMonomials | 0:f677e13975d0 | 29 | * |
| MultipleMonomials | 0:f677e13975d0 | 30 | * - the SHTP protocol: http://www.hillcrestlabs.com/download/59de8f99cd829e94dc0029d7 |
| MultipleMonomials | 0:f677e13975d0 | 31 | * -- SHTP transmit and receive protcols (for SPI, I2C, and UART) |
| MultipleMonomials | 0:f677e13975d0 | 32 | * -- SHTP binary format |
| MultipleMonomials | 0:f677e13975d0 | 33 | * -- packet types on the SHTP command channel |
| MultipleMonomials | 0:f677e13975d0 | 34 | * |
| MultipleMonomials | 0:f677e13975d0 | 35 | * - the SH-2 reference: http://www.hillcrestlabs.com/download/59de8f398934bf6faa00293f |
| MultipleMonomials | 0:f677e13975d0 | 36 | * -- list of packets and their formats for all channels other than command and executable |
| MultipleMonomials | 0:f677e13975d0 | 37 | * -- list of FRS (Flash Record System) entries and their formats |
| MultipleMonomials | 0:f677e13975d0 | 38 | * |
| MultipleMonomials | 0:f677e13975d0 | 39 | * =============================================== |
| MultipleMonomials | 0:f677e13975d0 | 40 | * |
| MultipleMonomials | 0:f677e13975d0 | 41 | * Overview of SHTP channels: |
| MultipleMonomials | 0:f677e13975d0 | 42 | * |
| MultipleMonomials | 0:f677e13975d0 | 43 | * 0 -> Command |
| MultipleMonomials | 0:f677e13975d0 | 44 | * -- Used for protocol-global packets, currently only the advertisement packet (which lists all the channels) and error reports |
| MultipleMonomials | 0:f677e13975d0 | 45 | * |
| MultipleMonomials | 0:f677e13975d0 | 46 | * 1 -> Executable |
| MultipleMonomials | 0:f677e13975d0 | 47 | * -- Used for things that control the software on the chip: commands to reset and sleep |
| MultipleMonomials | 0:f677e13975d0 | 48 | * -- Also used by the chip to report when it's done booting up |
| MultipleMonomials | 0:f677e13975d0 | 49 | * |
| MultipleMonomials | 0:f677e13975d0 | 50 | * 2 -> Control |
| MultipleMonomials | 0:f677e13975d0 | 51 | * -- Used to send configuration commands to the IMU and for it to send back responses. |
| MultipleMonomials | 0:f677e13975d0 | 52 | * -- Common report IDs: Command Request (0xF2), Set Feature (0xFD) |
| MultipleMonomials | 0:f677e13975d0 | 53 | * |
| MultipleMonomials | 0:f677e13975d0 | 54 | * 3 -> Sensor Reports |
| MultipleMonomials | 0:f677e13975d0 | 55 | * -- Used for sensors to send back data reports. |
| MultipleMonomials | 0:f677e13975d0 | 56 | * -- AFAIK the only report ID on this channel will be 0xFB (Report Base Timestamp); sensor data is send in a series of structures |
| MultipleMonomials | 0:f677e13975d0 | 57 | * following an 0xFB |
| MultipleMonomials | 0:f677e13975d0 | 58 | * |
| MultipleMonomials | 0:f677e13975d0 | 59 | * 4 -> Wake Sensor Reports |
| MultipleMonomials | 0:f677e13975d0 | 60 | * -- same as above, but for sensors configured to wake the device |
| MultipleMonomials | 0:f677e13975d0 | 61 | * |
| MultipleMonomials | 0:f677e13975d0 | 62 | * 5 -> Gyro Rotation Vector |
| MultipleMonomials | 0:f677e13975d0 | 63 | * -- a dedicated channel for the Gyro Rotation Vector sensor report |
| MultipleMonomials | 0:f677e13975d0 | 64 | * -- Why does this get its own channel? I don't know!!! |
| MultipleMonomials | 0:f677e13975d0 | 65 | */ |
| MultipleMonomials | 0:f677e13975d0 | 66 | |
| MultipleMonomials | 0:f677e13975d0 | 67 | #include "BNO080.h" |
| MultipleMonomials | 0:f677e13975d0 | 68 | #include "BNO080Constants.h" |
| MultipleMonomials | 0:f677e13975d0 | 69 | |
| MultipleMonomials | 0:f677e13975d0 | 70 | /// Set to 1 to enable debug printouts. Should be very useful if the chip is giving you trouble. |
| MultipleMonomials | 0:f677e13975d0 | 71 | /// When debugging, it is recommended to use the highest possible serial baudrate so as not to interrupt the timing of operations. |
| MultipleMonomials | 0:f677e13975d0 | 72 | #define BNO_DEBUG 1 |
| MultipleMonomials | 0:f677e13975d0 | 73 | |
| MultipleMonomials | 0:f677e13975d0 | 74 | BNO080::BNO080(Serial *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin, |
| MultipleMonomials | 0:f677e13975d0 | 75 | uint8_t i2cAddress, int i2cPortSpeed) : |
| MultipleMonomials | 0:f677e13975d0 | 76 | _debugPort(debugPort), |
| MultipleMonomials | 0:f677e13975d0 | 77 | _i2cPort(user_SDApin, user_SCLpin), |
| MultipleMonomials | 0:f677e13975d0 | 78 | _i2cAddress(i2cAddress), |
| MultipleMonomials | 0:f677e13975d0 | 79 | _int(user_INTPin), |
| MultipleMonomials | 0:f677e13975d0 | 80 | _rst(user_RSTPin, 1), |
| MultipleMonomials | 0:f677e13975d0 | 81 | _scope(p21, 1) |
| MultipleMonomials | 0:f677e13975d0 | 82 | { |
| MultipleMonomials | 0:f677e13975d0 | 83 | //Get user settings |
| MultipleMonomials | 0:f677e13975d0 | 84 | _i2cPortSpeed = i2cPortSpeed; |
| MultipleMonomials | 0:f677e13975d0 | 85 | if(_i2cPortSpeed > 4000000) |
| MultipleMonomials | 0:f677e13975d0 | 86 | { |
| MultipleMonomials | 0:f677e13975d0 | 87 | _i2cPortSpeed = 4000000; //BNO080 max is 400Khz |
| MultipleMonomials | 0:f677e13975d0 | 88 | } |
| MultipleMonomials | 0:f677e13975d0 | 89 | _i2cPort.frequency(_i2cPortSpeed); |
| MultipleMonomials | 0:f677e13975d0 | 90 | |
| MultipleMonomials | 0:f677e13975d0 | 91 | } |
| MultipleMonomials | 0:f677e13975d0 | 92 | |
| MultipleMonomials | 0:f677e13975d0 | 93 | bool BNO080::begin() |
| MultipleMonomials | 0:f677e13975d0 | 94 | { |
| MultipleMonomials | 0:f677e13975d0 | 95 | //Configure the BNO080 for SPI communication |
| MultipleMonomials | 0:f677e13975d0 | 96 | |
| MultipleMonomials | 0:f677e13975d0 | 97 | _rst = 0; // Reset BNO080 |
| MultipleMonomials | 0:f677e13975d0 | 98 | wait(.002f); // Min length not specified in datasheet? |
| MultipleMonomials | 0:f677e13975d0 | 99 | _rst = 1; // Bring out of reset |
| MultipleMonomials | 0:f677e13975d0 | 100 | |
| MultipleMonomials | 0:f677e13975d0 | 101 | // wait for a falling edge (NOT just a low) on the INT pin to denote startup |
| MultipleMonomials | 0:f677e13975d0 | 102 | Timer timeoutTimer; |
| MultipleMonomials | 0:f677e13975d0 | 103 | |
| MultipleMonomials | 0:f677e13975d0 | 104 | bool highDetected = false; |
| MultipleMonomials | 0:f677e13975d0 | 105 | bool lowDetected = false; |
| MultipleMonomials | 0:f677e13975d0 | 106 | |
| MultipleMonomials | 0:f677e13975d0 | 107 | while(true) |
| MultipleMonomials | 0:f677e13975d0 | 108 | { |
| MultipleMonomials | 0:f677e13975d0 | 109 | if(timeoutTimer.read() > BNO080_RESET_TIMEOUT) |
| MultipleMonomials | 0:f677e13975d0 | 110 | { |
| MultipleMonomials | 0:f677e13975d0 | 111 | _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n"); |
| MultipleMonomials | 0:f677e13975d0 | 112 | return false; |
| MultipleMonomials | 0:f677e13975d0 | 113 | } |
| MultipleMonomials | 0:f677e13975d0 | 114 | |
| MultipleMonomials | 0:f677e13975d0 | 115 | // simple edge detector |
| MultipleMonomials | 0:f677e13975d0 | 116 | if(!highDetected) |
| MultipleMonomials | 0:f677e13975d0 | 117 | { |
| MultipleMonomials | 0:f677e13975d0 | 118 | if(_int == 1) |
| MultipleMonomials | 0:f677e13975d0 | 119 | { |
| MultipleMonomials | 0:f677e13975d0 | 120 | highDetected = true; |
| MultipleMonomials | 0:f677e13975d0 | 121 | } |
| MultipleMonomials | 0:f677e13975d0 | 122 | } |
| MultipleMonomials | 0:f677e13975d0 | 123 | else if(!lowDetected) |
| MultipleMonomials | 0:f677e13975d0 | 124 | { |
| MultipleMonomials | 0:f677e13975d0 | 125 | if(_int == 0) |
| MultipleMonomials | 0:f677e13975d0 | 126 | { |
| MultipleMonomials | 0:f677e13975d0 | 127 | lowDetected = true; |
| MultipleMonomials | 0:f677e13975d0 | 128 | } |
| MultipleMonomials | 0:f677e13975d0 | 129 | } |
| MultipleMonomials | 0:f677e13975d0 | 130 | else |
| MultipleMonomials | 0:f677e13975d0 | 131 | { |
| MultipleMonomials | 0:f677e13975d0 | 132 | // high and low detected |
| MultipleMonomials | 0:f677e13975d0 | 133 | break; |
| MultipleMonomials | 0:f677e13975d0 | 134 | } |
| MultipleMonomials | 0:f677e13975d0 | 135 | } |
| MultipleMonomials | 0:f677e13975d0 | 136 | |
| MultipleMonomials | 0:f677e13975d0 | 137 | _debugPort->printf("BNO080 detected!\n"); |
| MultipleMonomials | 0:f677e13975d0 | 138 | |
| MultipleMonomials | 0:f677e13975d0 | 139 | // At system startup, the hub must send its full advertisement message (see SHTP 5.2 and 5.3) to the |
| MultipleMonomials | 0:f677e13975d0 | 140 | // host. It must not send any other data until this step is complete. |
| MultipleMonomials | 0:f677e13975d0 | 141 | // We don't actually care what's in it, we're just using it as a signal to indicate that the reset is complete. |
| MultipleMonomials | 0:f677e13975d0 | 142 | receivePacket(); |
| MultipleMonomials | 0:f677e13975d0 | 143 | |
| MultipleMonomials | 0:f677e13975d0 | 144 | // now, after startup, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command |
| MultipleMonomials | 0:f677e13975d0 | 145 | waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET); |
| MultipleMonomials | 0:f677e13975d0 | 146 | |
| MultipleMonomials | 0:f677e13975d0 | 147 | // Next, officially tell it to initialize, and wait for a successful Initialize Response |
| MultipleMonomials | 0:f677e13975d0 | 148 | zeroBuffer(); |
| MultipleMonomials | 0:f677e13975d0 | 149 | shtpData[3] = 0; |
| MultipleMonomials | 0:f677e13975d0 | 150 | _scope = 0; |
| MultipleMonomials | 0:f677e13975d0 | 151 | sendCommand(COMMAND_INITIALIZE); |
| MultipleMonomials | 0:f677e13975d0 | 152 | |
| MultipleMonomials | 0:f677e13975d0 | 153 | |
| MultipleMonomials | 0:f677e13975d0 | 154 | if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE) || shtpData[2] != COMMAND_INITIALIZE || shtpData[5] != 0) |
| MultipleMonomials | 0:f677e13975d0 | 155 | { |
| MultipleMonomials | 0:f677e13975d0 | 156 | _debugPort->printf("BNO080 reports initialization failed.\n"); |
| MultipleMonomials | 0:f677e13975d0 | 157 | __enable_irq(); |
| MultipleMonomials | 0:f677e13975d0 | 158 | return false; |
| MultipleMonomials | 0:f677e13975d0 | 159 | } |
| MultipleMonomials | 0:f677e13975d0 | 160 | else |
| MultipleMonomials | 0:f677e13975d0 | 161 | { |
| MultipleMonomials | 0:f677e13975d0 | 162 | #if BNO_DEBUG |
| MultipleMonomials | 0:f677e13975d0 | 163 | _debugPort->printf("BNO080 reports initialization successful!\n"); |
| MultipleMonomials | 0:f677e13975d0 | 164 | #endif |
| MultipleMonomials | 0:f677e13975d0 | 165 | } |
| MultipleMonomials | 0:f677e13975d0 | 166 | |
| MultipleMonomials | 0:f677e13975d0 | 167 | |
| MultipleMonomials | 0:f677e13975d0 | 168 | // Finally, we want to interrogate the device about its model and version. |
| MultipleMonomials | 0:f677e13975d0 | 169 | zeroBuffer(); |
| MultipleMonomials | 0:f677e13975d0 | 170 | shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info |
| MultipleMonomials | 0:f677e13975d0 | 171 | shtpData[1] = 0; //Reserved |
| MultipleMonomials | 0:f677e13975d0 | 172 | sendPacket(CHANNEL_CONTROL, 2); |
| MultipleMonomials | 0:f677e13975d0 | 173 | |
| MultipleMonomials | 0:f677e13975d0 | 174 | waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5); |
| MultipleMonomials | 0:f677e13975d0 | 175 | |
| MultipleMonomials | 0:f677e13975d0 | 176 | if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) |
| MultipleMonomials | 0:f677e13975d0 | 177 | { |
| MultipleMonomials | 0:f677e13975d0 | 178 | majorSoftwareVersion = shtpData[2]; |
| MultipleMonomials | 0:f677e13975d0 | 179 | minorSoftwareVersion = shtpData[3]; |
| MultipleMonomials | 0:f677e13975d0 | 180 | patchSoftwareVersion = (shtpData[13] << 8) | shtpData[12]; |
| MultipleMonomials | 0:f677e13975d0 | 181 | partNumber = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | shtpData[4]; |
| MultipleMonomials | 0:f677e13975d0 | 182 | buildNumber = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | shtpData[8]; |
| MultipleMonomials | 0:f677e13975d0 | 183 | |
| MultipleMonomials | 0:f677e13975d0 | 184 | #if BNO_DEBUG |
| MultipleMonomials | 0:f677e13975d0 | 185 | _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n", |
| MultipleMonomials | 0:f677e13975d0 | 186 | majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion, |
| MultipleMonomials | 0:f677e13975d0 | 187 | buildNumber, partNumber); |
| MultipleMonomials | 0:f677e13975d0 | 188 | #endif |
| MultipleMonomials | 0:f677e13975d0 | 189 | |
| MultipleMonomials | 0:f677e13975d0 | 190 | } |
| MultipleMonomials | 0:f677e13975d0 | 191 | else |
| MultipleMonomials | 0:f677e13975d0 | 192 | { |
| MultipleMonomials | 0:f677e13975d0 | 193 | _debugPort->printf("Bad response from product ID command.\n"); |
| MultipleMonomials | 0:f677e13975d0 | 194 | return false; |
| MultipleMonomials | 0:f677e13975d0 | 195 | } |
| MultipleMonomials | 0:f677e13975d0 | 196 | |
| MultipleMonomials | 0:f677e13975d0 | 197 | // successful init |
| MultipleMonomials | 0:f677e13975d0 | 198 | return true; |
| MultipleMonomials | 0:f677e13975d0 | 199 | |
| MultipleMonomials | 0:f677e13975d0 | 200 | } |
| MultipleMonomials | 0:f677e13975d0 | 201 | |
| MultipleMonomials | 0:f677e13975d0 | 202 | void BNO080::tare(bool zOnly) |
| MultipleMonomials | 0:f677e13975d0 | 203 | { |
| MultipleMonomials | 0:f677e13975d0 | 204 | zeroBuffer(); |
| MultipleMonomials | 0:f677e13975d0 | 205 | |
| MultipleMonomials | 0:f677e13975d0 | 206 | // from SH-2 section 6.4.4.1 |
| MultipleMonomials | 0:f677e13975d0 | 207 | shtpData[3] = 0; // perform tare now |
| MultipleMonomials | 0:f677e13975d0 | 208 | |
| MultipleMonomials | 0:f677e13975d0 | 209 | if(zOnly) |
| MultipleMonomials | 0:f677e13975d0 | 210 | { |
| MultipleMonomials | 0:f677e13975d0 | 211 | shtpData[4] = 0b100; // tare Z axis |
| MultipleMonomials | 0:f677e13975d0 | 212 | } |
| MultipleMonomials | 0:f677e13975d0 | 213 | else |
| MultipleMonomials | 0:f677e13975d0 | 214 | { |
| MultipleMonomials | 0:f677e13975d0 | 215 | shtpData[4] = 0b111; // tare X, Y, and Z axes |
| MultipleMonomials | 0:f677e13975d0 | 216 | } |
| MultipleMonomials | 0:f677e13975d0 | 217 | |
| MultipleMonomials | 0:f677e13975d0 | 218 | shtpData[5] = 0; // reorient all motion outputs |
| MultipleMonomials | 0:f677e13975d0 | 219 | |
| MultipleMonomials | 0:f677e13975d0 | 220 | sendCommand(COMMAND_TARE); |
| MultipleMonomials | 0:f677e13975d0 | 221 | } |
| MultipleMonomials | 0:f677e13975d0 | 222 | |
| MultipleMonomials | 0:f677e13975d0 | 223 | bool BNO080::updateData() |
| MultipleMonomials | 0:f677e13975d0 | 224 | { |
| MultipleMonomials | 0:f677e13975d0 | 225 | if(_int.read() != 0) |
| MultipleMonomials | 0:f677e13975d0 | 226 | { |
| MultipleMonomials | 0:f677e13975d0 | 227 | // no waiting packets |
| MultipleMonomials | 0:f677e13975d0 | 228 | return false; |
| MultipleMonomials | 0:f677e13975d0 | 229 | } |
| MultipleMonomials | 0:f677e13975d0 | 230 | |
| MultipleMonomials | 0:f677e13975d0 | 231 | while(_int.read() == 0) |
| MultipleMonomials | 0:f677e13975d0 | 232 | { |
| MultipleMonomials | 0:f677e13975d0 | 233 | if(!receivePacket()) |
| MultipleMonomials | 0:f677e13975d0 | 234 | { |
| MultipleMonomials | 0:f677e13975d0 | 235 | // comms error |
| MultipleMonomials | 0:f677e13975d0 | 236 | return false; |
| MultipleMonomials | 0:f677e13975d0 | 237 | } |
| MultipleMonomials | 0:f677e13975d0 | 238 | |
| MultipleMonomials | 0:f677e13975d0 | 239 | processPacket(); |
| MultipleMonomials | 0:f677e13975d0 | 240 | } |
| MultipleMonomials | 0:f677e13975d0 | 241 | |
| MultipleMonomials | 0:f677e13975d0 | 242 | // packets were received, so data may have changed |
| MultipleMonomials | 0:f677e13975d0 | 243 | return true; |
| MultipleMonomials | 0:f677e13975d0 | 244 | } |
| MultipleMonomials | 0:f677e13975d0 | 245 | |
| MultipleMonomials | 0:f677e13975d0 | 246 | uint8_t BNO080::getReportStatus(Report report) |
| MultipleMonomials | 0:f677e13975d0 | 247 | { |
| MultipleMonomials | 0:f677e13975d0 | 248 | uint8_t reportNum = static_cast<uint8_t>(report); |
| MultipleMonomials | 0:f677e13975d0 | 249 | if(reportNum > STATUS_ARRAY_LEN) |
| MultipleMonomials | 0:f677e13975d0 | 250 | { |
| MultipleMonomials | 0:f677e13975d0 | 251 | return 0; |
| MultipleMonomials | 0:f677e13975d0 | 252 | } |
| MultipleMonomials | 0:f677e13975d0 | 253 | |
| MultipleMonomials | 0:f677e13975d0 | 254 | return reportStatus[reportNum]; |
| MultipleMonomials | 0:f677e13975d0 | 255 | } |
| MultipleMonomials | 0:f677e13975d0 | 256 | |
| MultipleMonomials | 0:f677e13975d0 | 257 | //Sends the packet to enable the rotation vector |
| MultipleMonomials | 0:f677e13975d0 | 258 | void BNO080::enableReport(Report report, uint16_t timeBetweenReports) |
| MultipleMonomials | 0:f677e13975d0 | 259 | { |
| MultipleMonomials | 0:f677e13975d0 | 260 | setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports); |
| MultipleMonomials | 0:f677e13975d0 | 261 | |
| MultipleMonomials | 0:f677e13975d0 | 262 | // note: we don't wait for ACKs on these packets because they can take quite a while, like half a second, to come in |
| MultipleMonomials | 0:f677e13975d0 | 263 | } |
| MultipleMonomials | 0:f677e13975d0 | 264 | |
| MultipleMonomials | 0:f677e13975d0 | 265 | |
| MultipleMonomials | 0:f677e13975d0 | 266 | void BNO080::processPacket() |
| MultipleMonomials | 0:f677e13975d0 | 267 | { |
| MultipleMonomials | 0:f677e13975d0 | 268 | if(shtpHeader[2] == CHANNEL_CONTROL) |
| MultipleMonomials | 0:f677e13975d0 | 269 | { |
| MultipleMonomials | 0:f677e13975d0 | 270 | // currently no command reports are read |
| MultipleMonomials | 0:f677e13975d0 | 271 | } |
| MultipleMonomials | 0:f677e13975d0 | 272 | else if(shtpHeader[2] == CHANNEL_EXECUTABLE) |
| MultipleMonomials | 0:f677e13975d0 | 273 | { |
| MultipleMonomials | 0:f677e13975d0 | 274 | // currently no executable reports are read |
| MultipleMonomials | 0:f677e13975d0 | 275 | } |
| MultipleMonomials | 0:f677e13975d0 | 276 | else if(shtpHeader[2] == CHANNEL_COMMAND) |
| MultipleMonomials | 0:f677e13975d0 | 277 | { |
| MultipleMonomials | 0:f677e13975d0 | 278 | |
| MultipleMonomials | 0:f677e13975d0 | 279 | } |
| MultipleMonomials | 0:f677e13975d0 | 280 | else if(shtpHeader[2] == CHANNEL_REPORTS || shtpHeader[2] == CHANNEL_WAKE_REPORTS) |
| MultipleMonomials | 0:f677e13975d0 | 281 | { |
| MultipleMonomials | 0:f677e13975d0 | 282 | if(shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP) |
| MultipleMonomials | 0:f677e13975d0 | 283 | { |
| MultipleMonomials | 0:f677e13975d0 | 284 | // sensor data packet |
| MultipleMonomials | 0:f677e13975d0 | 285 | parseSensorDataPacket(); |
| MultipleMonomials | 0:f677e13975d0 | 286 | } |
| MultipleMonomials | 0:f677e13975d0 | 287 | } |
| MultipleMonomials | 0:f677e13975d0 | 288 | } |
| MultipleMonomials | 0:f677e13975d0 | 289 | |
| MultipleMonomials | 0:f677e13975d0 | 290 | // sizes of various sensor data packet elements |
| MultipleMonomials | 0:f677e13975d0 | 291 | #define SIZEOF_BASE_TIMESTAMP 5 |
| MultipleMonomials | 0:f677e13975d0 | 292 | #define SIZEOF_TIMESTAMP_REBASE 5 |
| MultipleMonomials | 0:f677e13975d0 | 293 | #define SIZEOF_ACCELEROMETER 10 |
| MultipleMonomials | 0:f677e13975d0 | 294 | #define SIZEOF_LINEAR_ACCELERATION 10 |
| MultipleMonomials | 0:f677e13975d0 | 295 | #define SIZEOF_GYROSCOPE_CALIBRATED 10 |
| MultipleMonomials | 0:f677e13975d0 | 296 | #define SIZEOF_MAGNETIC_FIELD_CALIBRATED 10 |
| MultipleMonomials | 0:f677e13975d0 | 297 | #define SIZEOF_ROTATION_VECTOR 14 |
| MultipleMonomials | 0:f677e13975d0 | 298 | #define SIZEOF_GAME_ROTATION_VECTOR 12 |
| MultipleMonomials | 0:f677e13975d0 | 299 | #define SIZEOF_GEOMAGNETIC_ROTATION_VECTOR 14 |
| MultipleMonomials | 0:f677e13975d0 | 300 | #define SIZEOF_TAP_DETECTOR 5 |
| MultipleMonomials | 0:f677e13975d0 | 301 | |
| MultipleMonomials | 0:f677e13975d0 | 302 | |
| MultipleMonomials | 0:f677e13975d0 | 303 | void BNO080::parseSensorDataPacket() |
| MultipleMonomials | 0:f677e13975d0 | 304 | { |
| MultipleMonomials | 0:f677e13975d0 | 305 | size_t currReportOffset = 0; |
| MultipleMonomials | 0:f677e13975d0 | 306 | |
| MultipleMonomials | 0:f677e13975d0 | 307 | // every sensor data report first contains a timestamp offset to show how long it has been between when |
| MultipleMonomials | 0:f677e13975d0 | 308 | // the host interrupt was sent and when the packet was transmitted. |
| MultipleMonomials | 0:f677e13975d0 | 309 | // We don't use interrupts and don't care about times, so we can throw this out. |
| MultipleMonomials | 0:f677e13975d0 | 310 | currReportOffset += SIZEOF_BASE_TIMESTAMP; |
| MultipleMonomials | 0:f677e13975d0 | 311 | |
| MultipleMonomials | 0:f677e13975d0 | 312 | while(currReportOffset < packetLength) |
| MultipleMonomials | 0:f677e13975d0 | 313 | { |
| MultipleMonomials | 0:f677e13975d0 | 314 | // lots of sensor reports use 3 16-bit numbers stored in bytes 4 through 9 |
| MultipleMonomials | 0:f677e13975d0 | 315 | // we can save some time by parsing those out here. |
| MultipleMonomials | 0:f677e13975d0 | 316 | uint16_t data1 = (uint16_t)shtpData[currReportOffset + 5] << 8 | shtpData[currReportOffset + 4]; |
| MultipleMonomials | 0:f677e13975d0 | 317 | uint16_t data2 = (uint16_t)shtpData[currReportOffset + 7] << 8 | shtpData[currReportOffset + 6]; |
| MultipleMonomials | 0:f677e13975d0 | 318 | uint16_t data3 = (uint16_t)shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8]; |
| MultipleMonomials | 0:f677e13975d0 | 319 | |
| MultipleMonomials | 0:f677e13975d0 | 320 | uint8_t reportNum = shtpData[currReportOffset]; |
| MultipleMonomials | 0:f677e13975d0 | 321 | |
| MultipleMonomials | 0:f677e13975d0 | 322 | if(reportNum != SENSOR_REPORTID_TIMESTAMP_REBASE) |
| MultipleMonomials | 0:f677e13975d0 | 323 | { |
| MultipleMonomials | 0:f677e13975d0 | 324 | // set status from byte 2 |
| MultipleMonomials | 0:f677e13975d0 | 325 | reportStatus[reportNum] = static_cast<uint8_t>(shtpData[currReportOffset + 2] & 0b11); |
| MultipleMonomials | 0:f677e13975d0 | 326 | } |
| MultipleMonomials | 0:f677e13975d0 | 327 | |
| MultipleMonomials | 0:f677e13975d0 | 328 | switch(shtpData[currReportOffset]) |
| MultipleMonomials | 0:f677e13975d0 | 329 | { |
| MultipleMonomials | 0:f677e13975d0 | 330 | case SENSOR_REPORTID_TIMESTAMP_REBASE: |
| MultipleMonomials | 0:f677e13975d0 | 331 | currReportOffset += SIZEOF_TIMESTAMP_REBASE; |
| MultipleMonomials | 0:f677e13975d0 | 332 | break; |
| MultipleMonomials | 0:f677e13975d0 | 333 | |
| MultipleMonomials | 0:f677e13975d0 | 334 | case SENSOR_REPORTID_ACCELEROMETER: |
| MultipleMonomials | 0:f677e13975d0 | 335 | |
| MultipleMonomials | 0:f677e13975d0 | 336 | totalAcceleration = TVector3( |
| MultipleMonomials | 0:f677e13975d0 | 337 | qToFloat(data1, ACCELEROMETER_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 338 | qToFloat(data2, ACCELEROMETER_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 339 | qToFloat(data3, ACCELEROMETER_Q_POINT)); |
| MultipleMonomials | 0:f677e13975d0 | 340 | |
| MultipleMonomials | 0:f677e13975d0 | 341 | currReportOffset += SIZEOF_ACCELEROMETER; |
| MultipleMonomials | 0:f677e13975d0 | 342 | break; |
| MultipleMonomials | 0:f677e13975d0 | 343 | |
| MultipleMonomials | 0:f677e13975d0 | 344 | case SENSOR_REPORTID_LINEAR_ACCELERATION: |
| MultipleMonomials | 0:f677e13975d0 | 345 | |
| MultipleMonomials | 0:f677e13975d0 | 346 | linearAcceleration = TVector3( |
| MultipleMonomials | 0:f677e13975d0 | 347 | qToFloat(data1, ACCELEROMETER_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 348 | qToFloat(data2, ACCELEROMETER_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 349 | qToFloat(data3, ACCELEROMETER_Q_POINT)); |
| MultipleMonomials | 0:f677e13975d0 | 350 | |
| MultipleMonomials | 0:f677e13975d0 | 351 | currReportOffset += SIZEOF_LINEAR_ACCELERATION; |
| MultipleMonomials | 0:f677e13975d0 | 352 | break; |
| MultipleMonomials | 0:f677e13975d0 | 353 | |
| MultipleMonomials | 0:f677e13975d0 | 354 | case SENSOR_REPORTID_GRAVITY: |
| MultipleMonomials | 0:f677e13975d0 | 355 | |
| MultipleMonomials | 0:f677e13975d0 | 356 | gravityAcceleration = TVector3( |
| MultipleMonomials | 0:f677e13975d0 | 357 | qToFloat(data1, ACCELEROMETER_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 358 | qToFloat(data2, ACCELEROMETER_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 359 | qToFloat(data3, ACCELEROMETER_Q_POINT)); |
| MultipleMonomials | 0:f677e13975d0 | 360 | |
| MultipleMonomials | 0:f677e13975d0 | 361 | currReportOffset += SIZEOF_LINEAR_ACCELERATION; |
| MultipleMonomials | 0:f677e13975d0 | 362 | break; |
| MultipleMonomials | 0:f677e13975d0 | 363 | |
| MultipleMonomials | 0:f677e13975d0 | 364 | case SENSOR_REPORTID_GYROSCOPE_CALIBRATED: |
| MultipleMonomials | 0:f677e13975d0 | 365 | |
| MultipleMonomials | 0:f677e13975d0 | 366 | gyroRotation = TVector3( |
| MultipleMonomials | 0:f677e13975d0 | 367 | qToFloat(data1, GYRO_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 368 | qToFloat(data2, GYRO_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 369 | qToFloat(data3, GYRO_Q_POINT)); |
| MultipleMonomials | 0:f677e13975d0 | 370 | |
| MultipleMonomials | 0:f677e13975d0 | 371 | currReportOffset += SIZEOF_GYROSCOPE_CALIBRATED; |
| MultipleMonomials | 0:f677e13975d0 | 372 | break; |
| MultipleMonomials | 0:f677e13975d0 | 373 | |
| MultipleMonomials | 0:f677e13975d0 | 374 | case SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED: |
| MultipleMonomials | 0:f677e13975d0 | 375 | |
| MultipleMonomials | 0:f677e13975d0 | 376 | magField = TVector3( |
| MultipleMonomials | 0:f677e13975d0 | 377 | qToFloat(data1, MAGNETOMETER_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 378 | qToFloat(data2, MAGNETOMETER_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 379 | qToFloat(data3, MAGNETOMETER_Q_POINT)); |
| MultipleMonomials | 0:f677e13975d0 | 380 | |
| MultipleMonomials | 0:f677e13975d0 | 381 | currReportOffset += SIZEOF_MAGNETIC_FIELD_CALIBRATED; |
| MultipleMonomials | 0:f677e13975d0 | 382 | break; |
| MultipleMonomials | 0:f677e13975d0 | 383 | |
| MultipleMonomials | 0:f677e13975d0 | 384 | case SENSOR_REPORTID_ROTATION_VECTOR: |
| MultipleMonomials | 0:f677e13975d0 | 385 | { |
| MultipleMonomials | 0:f677e13975d0 | 386 | uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; |
| MultipleMonomials | 0:f677e13975d0 | 387 | uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12]; |
| MultipleMonomials | 0:f677e13975d0 | 388 | |
| MultipleMonomials | 0:f677e13975d0 | 389 | rotationVector = TVector4( |
| MultipleMonomials | 0:f677e13975d0 | 390 | qToFloat(data1, ROTATION_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 391 | qToFloat(data2, ROTATION_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 392 | qToFloat(data3, ROTATION_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 393 | qToFloat(realPartQ, ROTATION_Q_POINT)); |
| MultipleMonomials | 0:f677e13975d0 | 394 | |
| MultipleMonomials | 0:f677e13975d0 | 395 | rotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT); |
| MultipleMonomials | 0:f677e13975d0 | 396 | |
| MultipleMonomials | 0:f677e13975d0 | 397 | currReportOffset += SIZEOF_ROTATION_VECTOR; |
| MultipleMonomials | 0:f677e13975d0 | 398 | } |
| MultipleMonomials | 0:f677e13975d0 | 399 | break; |
| MultipleMonomials | 0:f677e13975d0 | 400 | |
| MultipleMonomials | 0:f677e13975d0 | 401 | case SENSOR_REPORTID_GAME_ROTATION_VECTOR: |
| MultipleMonomials | 0:f677e13975d0 | 402 | { |
| MultipleMonomials | 0:f677e13975d0 | 403 | uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; |
| MultipleMonomials | 0:f677e13975d0 | 404 | |
| MultipleMonomials | 0:f677e13975d0 | 405 | gameRotationVector = TVector4( |
| MultipleMonomials | 0:f677e13975d0 | 406 | qToFloat(data1, ROTATION_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 407 | qToFloat(data2, ROTATION_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 408 | qToFloat(data3, ROTATION_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 409 | qToFloat(realPartQ, ROTATION_Q_POINT)); |
| MultipleMonomials | 0:f677e13975d0 | 410 | |
| MultipleMonomials | 0:f677e13975d0 | 411 | currReportOffset += SIZEOF_GAME_ROTATION_VECTOR; |
| MultipleMonomials | 0:f677e13975d0 | 412 | } |
| MultipleMonomials | 0:f677e13975d0 | 413 | break; |
| MultipleMonomials | 0:f677e13975d0 | 414 | |
| MultipleMonomials | 0:f677e13975d0 | 415 | case SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR: |
| MultipleMonomials | 0:f677e13975d0 | 416 | { |
| MultipleMonomials | 0:f677e13975d0 | 417 | uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; |
| MultipleMonomials | 0:f677e13975d0 | 418 | uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12]; |
| MultipleMonomials | 0:f677e13975d0 | 419 | |
| MultipleMonomials | 0:f677e13975d0 | 420 | geomagneticRotationVector = TVector4( |
| MultipleMonomials | 0:f677e13975d0 | 421 | qToFloat(data1, ROTATION_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 422 | qToFloat(data2, ROTATION_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 423 | qToFloat(data3, ROTATION_Q_POINT), |
| MultipleMonomials | 0:f677e13975d0 | 424 | qToFloat(realPartQ, ROTATION_Q_POINT)); |
| MultipleMonomials | 0:f677e13975d0 | 425 | |
| MultipleMonomials | 0:f677e13975d0 | 426 | geomagneticRotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT); |
| MultipleMonomials | 0:f677e13975d0 | 427 | |
| MultipleMonomials | 0:f677e13975d0 | 428 | currReportOffset += SIZEOF_GEOMAGNETIC_ROTATION_VECTOR; |
| MultipleMonomials | 0:f677e13975d0 | 429 | } |
| MultipleMonomials | 0:f677e13975d0 | 430 | break; |
| MultipleMonomials | 0:f677e13975d0 | 431 | |
| MultipleMonomials | 0:f677e13975d0 | 432 | case SENSOR_REPORTID_TAP_DETECTOR: |
| MultipleMonomials | 0:f677e13975d0 | 433 | |
| MultipleMonomials | 0:f677e13975d0 | 434 | // since we got the report, a tap was detected |
| MultipleMonomials | 0:f677e13975d0 | 435 | tapDetected = true; |
| MultipleMonomials | 0:f677e13975d0 | 436 | |
| MultipleMonomials | 0:f677e13975d0 | 437 | doubleTap = (shtpData[currReportOffset + 4] & (1 << 6)) != 0; |
| MultipleMonomials | 0:f677e13975d0 | 438 | |
| MultipleMonomials | 0:f677e13975d0 | 439 | currReportOffset += SIZEOF_TAP_DETECTOR; |
| MultipleMonomials | 0:f677e13975d0 | 440 | break; |
| MultipleMonomials | 0:f677e13975d0 | 441 | |
| MultipleMonomials | 0:f677e13975d0 | 442 | default: |
| MultipleMonomials | 0:f677e13975d0 | 443 | _debugPort->printf("Error: unrecognized report ID in sensor report: %hhx. Byte %u, length %hu\n", shtpData[currReportOffset], currReportOffset, packetLength); |
| MultipleMonomials | 0:f677e13975d0 | 444 | return; |
| MultipleMonomials | 0:f677e13975d0 | 445 | } |
| MultipleMonomials | 0:f677e13975d0 | 446 | } |
| MultipleMonomials | 0:f677e13975d0 | 447 | |
| MultipleMonomials | 0:f677e13975d0 | 448 | } |
| MultipleMonomials | 0:f677e13975d0 | 449 | |
| MultipleMonomials | 0:f677e13975d0 | 450 | bool BNO080::waitForPacket(int channel, uint8_t reportID, float timeout) |
| MultipleMonomials | 0:f677e13975d0 | 451 | { |
| MultipleMonomials | 0:f677e13975d0 | 452 | Timer timeoutTimer; |
| MultipleMonomials | 0:f677e13975d0 | 453 | timeoutTimer.start(); |
| MultipleMonomials | 0:f677e13975d0 | 454 | |
| MultipleMonomials | 0:f677e13975d0 | 455 | while(timeoutTimer.read() <= timeout) |
| MultipleMonomials | 0:f677e13975d0 | 456 | { |
| MultipleMonomials | 0:f677e13975d0 | 457 | if(_int.read() == 0) |
| MultipleMonomials | 0:f677e13975d0 | 458 | { |
| MultipleMonomials | 0:f677e13975d0 | 459 | if(!receivePacket(timeout)) |
| MultipleMonomials | 0:f677e13975d0 | 460 | { |
| MultipleMonomials | 0:f677e13975d0 | 461 | return false; |
| MultipleMonomials | 0:f677e13975d0 | 462 | } |
| MultipleMonomials | 0:f677e13975d0 | 463 | |
| MultipleMonomials | 0:f677e13975d0 | 464 | if(channel == shtpHeader[2] && reportID == shtpData[0]) |
| MultipleMonomials | 0:f677e13975d0 | 465 | { |
| MultipleMonomials | 0:f677e13975d0 | 466 | // found correct packet! |
| MultipleMonomials | 0:f677e13975d0 | 467 | return true; |
| MultipleMonomials | 0:f677e13975d0 | 468 | } |
| MultipleMonomials | 0:f677e13975d0 | 469 | else |
| MultipleMonomials | 0:f677e13975d0 | 470 | { |
| MultipleMonomials | 0:f677e13975d0 | 471 | // other data packet, send to proper channels |
| MultipleMonomials | 0:f677e13975d0 | 472 | processPacket(); |
| MultipleMonomials | 0:f677e13975d0 | 473 | } |
| MultipleMonomials | 0:f677e13975d0 | 474 | } |
| MultipleMonomials | 0:f677e13975d0 | 475 | } |
| MultipleMonomials | 0:f677e13975d0 | 476 | |
| MultipleMonomials | 0:f677e13975d0 | 477 | _debugPort->printf("Packet wait timeout.\n"); |
| MultipleMonomials | 0:f677e13975d0 | 478 | return false; |
| MultipleMonomials | 0:f677e13975d0 | 479 | } |
| MultipleMonomials | 0:f677e13975d0 | 480 | |
| MultipleMonomials | 0:f677e13975d0 | 481 | //Given a register value and a Q point, convert to float |
| MultipleMonomials | 0:f677e13975d0 | 482 | //See https://en.wikipedia.org/wiki/Q_(number_format) |
| MultipleMonomials | 0:f677e13975d0 | 483 | float BNO080::qToFloat(int16_t fixedPointValue, uint8_t qPoint) |
| MultipleMonomials | 0:f677e13975d0 | 484 | { |
| MultipleMonomials | 0:f677e13975d0 | 485 | float qFloat = fixedPointValue; |
| MultipleMonomials | 0:f677e13975d0 | 486 | qFloat *= pow(2, qPoint * -1); |
| MultipleMonomials | 0:f677e13975d0 | 487 | return (qFloat); |
| MultipleMonomials | 0:f677e13975d0 | 488 | } |
| MultipleMonomials | 0:f677e13975d0 | 489 | |
| MultipleMonomials | 0:f677e13975d0 | 490 | //Given a floating point value and a Q point, convert to Q |
| MultipleMonomials | 0:f677e13975d0 | 491 | //See https://en.wikipedia.org/wiki/Q_(number_format) |
| MultipleMonomials | 0:f677e13975d0 | 492 | int16_t BNO080::floatToQ(float qFloat, uint8_t qPoint) |
| MultipleMonomials | 0:f677e13975d0 | 493 | { |
| MultipleMonomials | 0:f677e13975d0 | 494 | int16_t qVal = static_cast<int16_t>(qFloat * pow(2, qPoint)); |
| MultipleMonomials | 0:f677e13975d0 | 495 | return qVal; |
| MultipleMonomials | 0:f677e13975d0 | 496 | } |
| MultipleMonomials | 0:f677e13975d0 | 497 | |
| MultipleMonomials | 0:f677e13975d0 | 498 | //Tell the sensor to do a command |
| MultipleMonomials | 0:f677e13975d0 | 499 | //See 6.3.8 page 41, Command request |
| MultipleMonomials | 0:f677e13975d0 | 500 | //The caller is expected to set P0 through P8 prior to calling |
| MultipleMonomials | 0:f677e13975d0 | 501 | void BNO080::sendCommand(uint8_t command) |
| MultipleMonomials | 0:f677e13975d0 | 502 | { |
| MultipleMonomials | 0:f677e13975d0 | 503 | shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request |
| MultipleMonomials | 0:f677e13975d0 | 504 | shtpData[1] = commandSequenceNumber++; //Increments automatically each function call |
| MultipleMonomials | 0:f677e13975d0 | 505 | shtpData[2] = command; //Command |
| MultipleMonomials | 0:f677e13975d0 | 506 | |
| MultipleMonomials | 0:f677e13975d0 | 507 | //Caller must set these |
| MultipleMonomials | 0:f677e13975d0 | 508 | /*shtpData[3] = 0; //P0 |
| MultipleMonomials | 0:f677e13975d0 | 509 | shtpData[4] = 0; //P1 |
| MultipleMonomials | 0:f677e13975d0 | 510 | shtpData[5] = 0; //P2 |
| MultipleMonomials | 0:f677e13975d0 | 511 | shtpData[6] = 0; |
| MultipleMonomials | 0:f677e13975d0 | 512 | shtpData[7] = 0; |
| MultipleMonomials | 0:f677e13975d0 | 513 | shtpData[8] = 0; |
| MultipleMonomials | 0:f677e13975d0 | 514 | shtpData[9] = 0; |
| MultipleMonomials | 0:f677e13975d0 | 515 | shtpData[10] = 0; |
| MultipleMonomials | 0:f677e13975d0 | 516 | shtpData[11] = 0;*/ |
| MultipleMonomials | 0:f677e13975d0 | 517 | |
| MultipleMonomials | 0:f677e13975d0 | 518 | //Transmit packet on channel 2, 12 bytes |
| MultipleMonomials | 0:f677e13975d0 | 519 | sendPacket(CHANNEL_CONTROL, 12); |
| MultipleMonomials | 0:f677e13975d0 | 520 | } |
| MultipleMonomials | 0:f677e13975d0 | 521 | |
| MultipleMonomials | 0:f677e13975d0 | 522 | //Given a sensor's report ID, this tells the BNO080 to begin reporting the values |
| MultipleMonomials | 0:f677e13975d0 | 523 | //Also sets the specific config word. Useful for personal activity classifier |
| MultipleMonomials | 0:f677e13975d0 | 524 | void BNO080::setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig) |
| MultipleMonomials | 0:f677e13975d0 | 525 | { |
| MultipleMonomials | 0:f677e13975d0 | 526 | uint32_t microsBetweenReports = static_cast<uint32_t>(timeBetweenReports * 1000); |
| MultipleMonomials | 0:f677e13975d0 | 527 | |
| MultipleMonomials | 0:f677e13975d0 | 528 | const uint32_t batchMicros = 0; |
| MultipleMonomials | 0:f677e13975d0 | 529 | |
| MultipleMonomials | 0:f677e13975d0 | 530 | shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55 |
| MultipleMonomials | 0:f677e13975d0 | 531 | shtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector |
| MultipleMonomials | 0:f677e13975d0 | 532 | shtpData[2] = 0; //Feature flags |
| MultipleMonomials | 0:f677e13975d0 | 533 | shtpData[3] = 0; //Change sensitivity (LSB) |
| MultipleMonomials | 0:f677e13975d0 | 534 | shtpData[4] = 0; //Change sensitivity (MSB) |
| MultipleMonomials | 0:f677e13975d0 | 535 | shtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms |
| MultipleMonomials | 0:f677e13975d0 | 536 | shtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval |
| MultipleMonomials | 0:f677e13975d0 | 537 | shtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval |
| MultipleMonomials | 0:f677e13975d0 | 538 | shtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB) |
| MultipleMonomials | 0:f677e13975d0 | 539 | shtpData[9] = (batchMicros >> 0) & 0xFF; //Batch Interval (LSB) |
| MultipleMonomials | 0:f677e13975d0 | 540 | shtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval |
| MultipleMonomials | 0:f677e13975d0 | 541 | shtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval |
| MultipleMonomials | 0:f677e13975d0 | 542 | shtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB) |
| MultipleMonomials | 0:f677e13975d0 | 543 | shtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB) |
| MultipleMonomials | 0:f677e13975d0 | 544 | shtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config |
| MultipleMonomials | 0:f677e13975d0 | 545 | shtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config |
| MultipleMonomials | 0:f677e13975d0 | 546 | shtpData[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB) |
| MultipleMonomials | 0:f677e13975d0 | 547 | |
| MultipleMonomials | 0:f677e13975d0 | 548 | //Transmit packet on channel 2, 17 bytes |
| MultipleMonomials | 0:f677e13975d0 | 549 | sendPacket(CHANNEL_CONTROL, 17); |
| MultipleMonomials | 0:f677e13975d0 | 550 | } |
| MultipleMonomials | 0:f677e13975d0 | 551 | |
| MultipleMonomials | 0:f677e13975d0 | 552 | |
| MultipleMonomials | 0:f677e13975d0 | 553 | //Given the data packet, send the header then the data |
| MultipleMonomials | 0:f677e13975d0 | 554 | //Returns false if sensor does not ACK |
| MultipleMonomials | 0:f677e13975d0 | 555 | bool BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength) |
| MultipleMonomials | 0:f677e13975d0 | 556 | { |
| MultipleMonomials | 0:f677e13975d0 | 557 | // start the transaction and contact the IMU |
| MultipleMonomials | 0:f677e13975d0 | 558 | _i2cPort.start(); |
| MultipleMonomials | 0:f677e13975d0 | 559 | |
| MultipleMonomials | 0:f677e13975d0 | 560 | // to indicate an i2c read, shift the 7 bit address up 1 bit and keep bit 0 as a 0 |
| MultipleMonomials | 0:f677e13975d0 | 561 | int writeResult = _i2cPort.write(_i2cAddress << 1); |
| MultipleMonomials | 0:f677e13975d0 | 562 | |
| MultipleMonomials | 0:f677e13975d0 | 563 | if(writeResult != 1) |
| MultipleMonomials | 0:f677e13975d0 | 564 | { |
| MultipleMonomials | 0:f677e13975d0 | 565 | _debugPort->printf("BNO I2C write failed!\n"); |
| MultipleMonomials | 0:f677e13975d0 | 566 | _scope = 0; |
| MultipleMonomials | 0:f677e13975d0 | 567 | _i2cPort.stop(); |
| MultipleMonomials | 0:f677e13975d0 | 568 | return false; |
| MultipleMonomials | 0:f677e13975d0 | 569 | } |
| MultipleMonomials | 0:f677e13975d0 | 570 | |
| MultipleMonomials | 0:f677e13975d0 | 571 | |
| MultipleMonomials | 0:f677e13975d0 | 572 | uint16_t totalLength = dataLength + 4; //Add four bytes for the header |
| MultipleMonomials | 0:f677e13975d0 | 573 | packetLength = dataLength; |
| MultipleMonomials | 0:f677e13975d0 | 574 | |
| MultipleMonomials | 0:f677e13975d0 | 575 | #if BNO_DEBUG |
| MultipleMonomials | 0:f677e13975d0 | 576 | shtpHeader[0] = totalLength & 0xFF; |
| MultipleMonomials | 0:f677e13975d0 | 577 | shtpHeader[1] = totalLength >> 8; |
| MultipleMonomials | 0:f677e13975d0 | 578 | shtpHeader[2] = channelNumber; |
| MultipleMonomials | 0:f677e13975d0 | 579 | shtpHeader[3] = sequenceNumber[channelNumber]; |
| MultipleMonomials | 0:f677e13975d0 | 580 | |
| MultipleMonomials | 0:f677e13975d0 | 581 | _debugPort->printf("Transmitting packet: ----------------\n"); |
| MultipleMonomials | 0:f677e13975d0 | 582 | printPacket(); |
| MultipleMonomials | 0:f677e13975d0 | 583 | #endif |
| MultipleMonomials | 0:f677e13975d0 | 584 | |
| MultipleMonomials | 0:f677e13975d0 | 585 | //Send the 4 byte packet header |
| MultipleMonomials | 0:f677e13975d0 | 586 | _i2cPort.write(totalLength & 0xFF); //Packet length LSB |
| MultipleMonomials | 0:f677e13975d0 | 587 | _i2cPort.write(totalLength >> 8); //Packet length MSB |
| MultipleMonomials | 0:f677e13975d0 | 588 | _i2cPort.write(channelNumber); //Channel number |
| MultipleMonomials | 0:f677e13975d0 | 589 | _i2cPort.write(sequenceNumber[channelNumber]++); //Send the sequence number, increments with each packet sent, different counter for each channel |
| MultipleMonomials | 0:f677e13975d0 | 590 | |
| MultipleMonomials | 0:f677e13975d0 | 591 | //Send the user's data packet |
| MultipleMonomials | 0:f677e13975d0 | 592 | for (uint8_t i = 0 ; i < dataLength ; i++) |
| MultipleMonomials | 0:f677e13975d0 | 593 | { |
| MultipleMonomials | 0:f677e13975d0 | 594 | _i2cPort.write(shtpData[i]); |
| MultipleMonomials | 0:f677e13975d0 | 595 | } |
| MultipleMonomials | 0:f677e13975d0 | 596 | _i2cPort.stop(); |
| MultipleMonomials | 0:f677e13975d0 | 597 | |
| MultipleMonomials | 0:f677e13975d0 | 598 | return (true); |
| MultipleMonomials | 0:f677e13975d0 | 599 | } |
| MultipleMonomials | 0:f677e13975d0 | 600 | |
| MultipleMonomials | 0:f677e13975d0 | 601 | //Check to see if there is any new data available |
| MultipleMonomials | 0:f677e13975d0 | 602 | //Read the contents of the incoming packet into the shtpData array |
| MultipleMonomials | 0:f677e13975d0 | 603 | bool BNO080::receivePacket(float timeout) |
| MultipleMonomials | 0:f677e13975d0 | 604 | { |
| MultipleMonomials | 0:f677e13975d0 | 605 | Timer waitStartTime; |
| MultipleMonomials | 0:f677e13975d0 | 606 | waitStartTime.start(); |
| MultipleMonomials | 0:f677e13975d0 | 607 | |
| MultipleMonomials | 0:f677e13975d0 | 608 | while(_int.read() != 0) |
| MultipleMonomials | 0:f677e13975d0 | 609 | { |
| MultipleMonomials | 0:f677e13975d0 | 610 | if(waitStartTime.read() > timeout) |
| MultipleMonomials | 0:f677e13975d0 | 611 | { |
| MultipleMonomials | 0:f677e13975d0 | 612 | _debugPort->printf("BNO I2C wait timeout\n"); |
| MultipleMonomials | 0:f677e13975d0 | 613 | return false; |
| MultipleMonomials | 0:f677e13975d0 | 614 | } |
| MultipleMonomials | 0:f677e13975d0 | 615 | |
| MultipleMonomials | 0:f677e13975d0 | 616 | } |
| MultipleMonomials | 0:f677e13975d0 | 617 | |
| MultipleMonomials | 0:f677e13975d0 | 618 | // start the transaction and contact the IMU |
| MultipleMonomials | 0:f677e13975d0 | 619 | _i2cPort.start(); |
| MultipleMonomials | 0:f677e13975d0 | 620 | |
| MultipleMonomials | 0:f677e13975d0 | 621 | // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1 |
| MultipleMonomials | 0:f677e13975d0 | 622 | int writeResult = _i2cPort.write((_i2cAddress << 1) | 0x1); |
| MultipleMonomials | 0:f677e13975d0 | 623 | |
| MultipleMonomials | 0:f677e13975d0 | 624 | if(writeResult != 1) |
| MultipleMonomials | 0:f677e13975d0 | 625 | { |
| MultipleMonomials | 0:f677e13975d0 | 626 | _debugPort->printf("BNO I2C read failed!\n"); |
| MultipleMonomials | 0:f677e13975d0 | 627 | return false; |
| MultipleMonomials | 0:f677e13975d0 | 628 | } |
| MultipleMonomials | 0:f677e13975d0 | 629 | |
| MultipleMonomials | 0:f677e13975d0 | 630 | //Get the first four bytes, aka the packet header |
| MultipleMonomials | 0:f677e13975d0 | 631 | uint8_t packetLSB = static_cast<uint8_t>(_i2cPort.read(true)); |
| MultipleMonomials | 0:f677e13975d0 | 632 | uint8_t packetMSB = static_cast<uint8_t>(_i2cPort.read(true)); |
| MultipleMonomials | 0:f677e13975d0 | 633 | uint8_t channelNumber = static_cast<uint8_t>(_i2cPort.read(true)); |
| MultipleMonomials | 0:f677e13975d0 | 634 | uint8_t sequenceNum = static_cast<uint8_t>(_i2cPort.read(true)); //Not sure if we need to store this or not |
| MultipleMonomials | 0:f677e13975d0 | 635 | |
| MultipleMonomials | 0:f677e13975d0 | 636 | //Store the header info |
| MultipleMonomials | 0:f677e13975d0 | 637 | shtpHeader[0] = packetLSB; |
| MultipleMonomials | 0:f677e13975d0 | 638 | shtpHeader[1] = packetMSB; |
| MultipleMonomials | 0:f677e13975d0 | 639 | shtpHeader[2] = channelNumber; |
| MultipleMonomials | 0:f677e13975d0 | 640 | shtpHeader[3] = sequenceNum; |
| MultipleMonomials | 0:f677e13975d0 | 641 | |
| MultipleMonomials | 0:f677e13975d0 | 642 | if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF) |
| MultipleMonomials | 0:f677e13975d0 | 643 | { |
| MultipleMonomials | 0:f677e13975d0 | 644 | // invalid according to BNO080 datasheet section 1.4.1 |
| MultipleMonomials | 0:f677e13975d0 | 645 | |
| MultipleMonomials | 0:f677e13975d0 | 646 | #if BNO_DEBUG |
| MultipleMonomials | 0:f677e13975d0 | 647 | _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n"); |
| MultipleMonomials | 0:f677e13975d0 | 648 | #endif |
| MultipleMonomials | 0:f677e13975d0 | 649 | return false; |
| MultipleMonomials | 0:f677e13975d0 | 650 | } |
| MultipleMonomials | 0:f677e13975d0 | 651 | |
| MultipleMonomials | 0:f677e13975d0 | 652 | //Calculate the number of data bytes in this packet |
| MultipleMonomials | 0:f677e13975d0 | 653 | packetLength = (static_cast<uint16_t>(packetMSB) << 8 | packetLSB); |
| MultipleMonomials | 0:f677e13975d0 | 654 | |
| MultipleMonomials | 0:f677e13975d0 | 655 | // Clear the MSbit. |
| MultipleMonomials | 0:f677e13975d0 | 656 | // 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) |
| MultipleMonomials | 0:f677e13975d0 | 657 | // but we don't actually care about any of the advertisement packets |
| MultipleMonomials | 0:f677e13975d0 | 658 | // that use this, so we can just cut off the rest of the packet by releasing chip select. |
| MultipleMonomials | 0:f677e13975d0 | 659 | packetLength &= ~(1 << 15); |
| MultipleMonomials | 0:f677e13975d0 | 660 | |
| MultipleMonomials | 0:f677e13975d0 | 661 | if (packetLength == 0) |
| MultipleMonomials | 0:f677e13975d0 | 662 | { |
| MultipleMonomials | 0:f677e13975d0 | 663 | // Packet is empty |
| MultipleMonomials | 0:f677e13975d0 | 664 | return (false); //All done |
| MultipleMonomials | 0:f677e13975d0 | 665 | } |
| MultipleMonomials | 0:f677e13975d0 | 666 | |
| MultipleMonomials | 0:f677e13975d0 | 667 | packetLength -= 4; //Remove the header bytes from the data count |
| MultipleMonomials | 0:f677e13975d0 | 668 | |
| MultipleMonomials | 0:f677e13975d0 | 669 | //Read incoming data into the shtpData array |
| MultipleMonomials | 0:f677e13975d0 | 670 | for (uint16_t dataSpot = 0 ; dataSpot < packetLength ; dataSpot++) |
| MultipleMonomials | 0:f677e13975d0 | 671 | { |
| MultipleMonomials | 0:f677e13975d0 | 672 | bool sendACK = dataSpot < packetLength - 1; |
| MultipleMonomials | 0:f677e13975d0 | 673 | |
| MultipleMonomials | 0:f677e13975d0 | 674 | // per the datasheet, 0xFF is used as filler for the receiver to transmit back |
| MultipleMonomials | 0:f677e13975d0 | 675 | uint8_t incoming = static_cast<uint8_t>(_i2cPort.read(sendACK)); |
| MultipleMonomials | 0:f677e13975d0 | 676 | if (dataSpot < STORED_PACKET_SIZE) //BNO080 can respond with upto 270 bytes, avoid overflow |
| MultipleMonomials | 0:f677e13975d0 | 677 | shtpData[dataSpot] = incoming; //Store data into the shtpData array |
| MultipleMonomials | 0:f677e13975d0 | 678 | } |
| MultipleMonomials | 0:f677e13975d0 | 679 | |
| MultipleMonomials | 0:f677e13975d0 | 680 | _i2cPort.stop(); |
| MultipleMonomials | 0:f677e13975d0 | 681 | |
| MultipleMonomials | 0:f677e13975d0 | 682 | #if BNO_DEBUG |
| MultipleMonomials | 0:f677e13975d0 | 683 | _debugPort->printf("Recieved packet: ----------------\n"); |
| MultipleMonomials | 0:f677e13975d0 | 684 | printPacket(); // note: add 4 for the header length |
| MultipleMonomials | 0:f677e13975d0 | 685 | #endif |
| MultipleMonomials | 0:f677e13975d0 | 686 | |
| MultipleMonomials | 0:f677e13975d0 | 687 | return (true); //We're done! |
| MultipleMonomials | 0:f677e13975d0 | 688 | } |
| MultipleMonomials | 0:f677e13975d0 | 689 | |
| MultipleMonomials | 0:f677e13975d0 | 690 | //Pretty prints the contents of the current shtp header and data packets |
| MultipleMonomials | 0:f677e13975d0 | 691 | void BNO080::printPacket() |
| MultipleMonomials | 0:f677e13975d0 | 692 | { |
| MultipleMonomials | 0:f677e13975d0 | 693 | #if BNO_DEBUG |
| MultipleMonomials | 0:f677e13975d0 | 694 | //Print the four byte header |
| MultipleMonomials | 0:f677e13975d0 | 695 | _debugPort->printf("Header:"); |
| MultipleMonomials | 0:f677e13975d0 | 696 | for (uint8_t x = 0 ; x < 4 ; x++) |
| MultipleMonomials | 0:f677e13975d0 | 697 | { |
| MultipleMonomials | 0:f677e13975d0 | 698 | _debugPort->printf(" "); |
| MultipleMonomials | 0:f677e13975d0 | 699 | if (shtpHeader[x] < 0x10) _debugPort->printf("0"); |
| MultipleMonomials | 0:f677e13975d0 | 700 | _debugPort->printf("%hhx", shtpHeader[x]); |
| MultipleMonomials | 0:f677e13975d0 | 701 | } |
| MultipleMonomials | 0:f677e13975d0 | 702 | |
| MultipleMonomials | 0:f677e13975d0 | 703 | uint16_t printLength = packetLength; |
| MultipleMonomials | 0:f677e13975d0 | 704 | if (printLength > 40) printLength = 40; //Artificial limit. We don't want the phone book. |
| MultipleMonomials | 0:f677e13975d0 | 705 | |
| MultipleMonomials | 0:f677e13975d0 | 706 | _debugPort->printf(" Body:"); |
| MultipleMonomials | 0:f677e13975d0 | 707 | for (uint16_t x = 0 ; x < printLength ; x++) |
| MultipleMonomials | 0:f677e13975d0 | 708 | { |
| MultipleMonomials | 0:f677e13975d0 | 709 | _debugPort->printf(" "); |
| MultipleMonomials | 0:f677e13975d0 | 710 | if (shtpData[x] < 0x10) _debugPort->printf("0"); |
| MultipleMonomials | 0:f677e13975d0 | 711 | _debugPort->printf("%hhx", shtpData[x]); |
| MultipleMonomials | 0:f677e13975d0 | 712 | } |
| MultipleMonomials | 0:f677e13975d0 | 713 | |
| MultipleMonomials | 0:f677e13975d0 | 714 | _debugPort->printf(", Length:"); |
| MultipleMonomials | 0:f677e13975d0 | 715 | _debugPort->printf("%hhu", packetLength + SHTP_HEADER_SIZE); |
| MultipleMonomials | 0:f677e13975d0 | 716 | |
| MultipleMonomials | 0:f677e13975d0 | 717 | if(shtpHeader[1] >> 7) |
| MultipleMonomials | 0:f677e13975d0 | 718 | { |
| MultipleMonomials | 0:f677e13975d0 | 719 | _debugPort->printf("[C]"); |
| MultipleMonomials | 0:f677e13975d0 | 720 | } |
| MultipleMonomials | 0:f677e13975d0 | 721 | |
| MultipleMonomials | 0:f677e13975d0 | 722 | _debugPort->printf(", SeqNum: %hhu", shtpHeader[3]); |
| MultipleMonomials | 0:f677e13975d0 | 723 | |
| MultipleMonomials | 0:f677e13975d0 | 724 | _debugPort->printf(", Channel:"); |
| MultipleMonomials | 0:f677e13975d0 | 725 | if (shtpHeader[2] == 0) _debugPort->printf("Command"); |
| MultipleMonomials | 0:f677e13975d0 | 726 | else if (shtpHeader[2] == 1) _debugPort->printf("Executable"); |
| MultipleMonomials | 0:f677e13975d0 | 727 | else if (shtpHeader[2] == 2) _debugPort->printf("Control"); |
| MultipleMonomials | 0:f677e13975d0 | 728 | else if (shtpHeader[2] == 3) _debugPort->printf("Sensor-report"); |
| MultipleMonomials | 0:f677e13975d0 | 729 | else if (shtpHeader[2] == 4) _debugPort->printf("Wake-report"); |
| MultipleMonomials | 0:f677e13975d0 | 730 | else if (shtpHeader[2] == 5) _debugPort->printf("Gyro-vector"); |
| MultipleMonomials | 0:f677e13975d0 | 731 | else _debugPort->printf("%hhu", shtpHeader[2]); |
| MultipleMonomials | 0:f677e13975d0 | 732 | |
| MultipleMonomials | 0:f677e13975d0 | 733 | _debugPort->printf("\n"); |
| MultipleMonomials | 0:f677e13975d0 | 734 | #endif |
| MultipleMonomials | 0:f677e13975d0 | 735 | } |
| MultipleMonomials | 0:f677e13975d0 | 736 | |
| MultipleMonomials | 0:f677e13975d0 | 737 | |
| MultipleMonomials | 0:f677e13975d0 | 738 | void BNO080::zeroBuffer() |
| MultipleMonomials | 0:f677e13975d0 | 739 | { |
| MultipleMonomials | 0:f677e13975d0 | 740 | memset(shtpHeader, 0, SHTP_HEADER_SIZE); |
| MultipleMonomials | 0:f677e13975d0 | 741 | memset(shtpData, 0, STORED_PACKET_SIZE); |
| MultipleMonomials | 0:f677e13975d0 | 742 | packetLength = 0; |
| MultipleMonomials | 0:f677e13975d0 | 743 | } |