Affordable Smart Wheelchair / BNO080_library

Contains added code for stm32-L432KC compatibility

Dependents:   BNO080_stm32_compatible

BNO080.cpp@8:92d4109beb3c, 2019-07-30 (annotated)

Committer:
JesiMiranda
Date:
Tue Jul 30 16:44:07 2019 +0000
Revision:
8:92d4109beb3c
Parent:
7:050dc348e690
Child:
9:68e4b1932497
Jamie changed the cpp;

Who changed what in which revision?

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