Contains added code for stm32-L432KC compatibility
Dependents: BNO080_stm32_compatible
Diff: BNO080.cpp
- Revision:
- 6:546e4553cf6d
- Parent:
- 5:7e2cd0f351b2
- Child:
- 7:050dc348e690
diff -r 7e2cd0f351b2 -r 546e4553cf6d BNO080.cpp --- a/BNO080.cpp Mon Jul 22 18:54:25 2019 +0000 +++ b/BNO080.cpp Sat Jul 27 00:12:20 2019 +0000 @@ -53,7 +53,7 @@ * * 3 -> Sensor Reports * -- Used for sensors to send back data reports. - * -- AFAIK the only report ID on this channel will be 0xFB (Report Base Timestamp); sensor data is sent in a series of structures + * -- AFAIK the only report ID on this channel will be 0xFB (Report Base Timestamp); sensor data is send in a series of structures * following an 0xFB * * 4 -> Wake Sensor Reports @@ -69,513 +69,454 @@ /// Set to 1 to enable debug printouts. Should be very useful if the chip is giving you trouble. /// When debugging, it is recommended to use the highest possible serial baudrate so as not to interrupt the timing of operations. -#define BNO_DEBUG 1 +#define BNO_DEBUG 0 BNO080::BNO080(Serial *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin, - uint8_t i2cAddress, int i2cPortSpeed) : - _debugPort(debugPort), - _i2cPort(user_SDApin, user_SCLpin), - _i2cAddress(i2cAddress), - _int(user_INTPin), - _rst(user_RSTPin, 1), - commandSequenceNumber(0), - stability(UNKNOWN), - stepDetected(false), - stepCount(0), - significantMotionDetected(false), - shakeDetected(false), - xAxisShake(false), - yAxisShake(false), - zAxisShake(false) + uint8_t i2cAddress, int i2cPortSpeed) : + _debugPort(debugPort), + _i2cPort(user_SDApin, user_SCLpin), + _i2cAddress(i2cAddress), + _int(user_INTPin), + _rst(user_RSTPin, 1), + commandSequenceNumber(0), + stability(UNKNOWN), + stepDetected(false), + stepCount(0), + significantMotionDetected(false), + shakeDetected(false), + xAxisShake(false), + yAxisShake(false), + zAxisShake(false) { - // zero sequence numbers - memset(sequenceNumber, 0, sizeof(sequenceNumber)); + // zero sequence numbers + memset(sequenceNumber, 0, sizeof(sequenceNumber)); - //Get user settings - _i2cPortSpeed = i2cPortSpeed; - if(_i2cPortSpeed > 4000000) - { - _i2cPortSpeed = 4000000; //BNO080 max is 400Khz - } - _i2cPort.frequency(_i2cPortSpeed); + //Get user settings + _i2cPortSpeed = i2cPortSpeed; + if(_i2cPortSpeed > 4000000) { + _i2cPortSpeed = 4000000; //BNO080 max is 400Khz + } + _i2cPort.frequency(_i2cPortSpeed); } bool BNO080::begin() { - //Configure the BNO080 for I2C communication + //Configure the BNO080 for SPI communication + + _rst = 0; // Reset BNO080 + wait(.002f); // Min length not specified in datasheet? + _rst = 1; // Bring out of reset - _rst = 0; // Reset BNO080 - wait(.002f); // Min length not specified in datasheet? - _rst = 1; // Bring out of reset + // wait for a falling edge (NOT just a low) on the INT pin to denote startup + Timer timeoutTimer; - // wait for a falling edge (NOT just a low) on the INT pin to denote startup - Timer timeoutTimer; - timeoutTimer.start(); + bool highDetected = false; + bool lowDetected = false; - bool highDetected = false; - bool lowDetected = false; + while(true) { + if(timeoutTimer.read() > BNO080_RESET_TIMEOUT) { + _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n"); + return false; + } - while(true) - { - if(timeoutTimer.read() > BNO080_RESET_TIMEOUT) - { - _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n"); - return false; - } + // simple edge detector + if(!highDetected) { + if(_int == 1) { + highDetected = true; + } + } else if(!lowDetected) { + if(_int == 0) { + lowDetected = true; + } + } else { + // high and low detected + break; + } + } + + _debugPort->printf("BNO080 detected!\n"); - // simple edge detector - if(!highDetected) - { - if(_int == 1) - { - highDetected = true; - } - } - else if(!lowDetected) - { - if(_int == 0) - { - lowDetected = true; - } - } - else - { - // high and low detected - - break; - } - } + // At system startup, the hub must send its full advertisement message (see SHTP 5.2 and 5.3) to the + // host. It must not send any other data until this step is complete. + // We don't actually care what's in it, we're just using it as a signal to indicate that the reset is complete. + receivePacket(); + + // now, after startup, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command + waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET); + + // Next, officially tell it to initialize, and wait for a successful Initialize Response + zeroBuffer(); + shtpData[3] = 0; + //changed from sendCommand + sendPacket(COMMAND_INITIALIZE, 3); + + + if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE) || shtpData[2] != COMMAND_INITIALIZE || shtpData[5] != 0) { + _debugPort->printf("BNO080 reports initialization failed.\n"); + __enable_irq(); + return false; + } else { +#if BNO_DEBUG + _debugPort->printf("BNO080 reports initialization successful!\n"); +#endif + } + + + // Finally, we want to interrogate the device about its model and version. + zeroBuffer(); + shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info + shtpData[1] = 0; //Reserved + sendPacket(CHANNEL_CONTROL, 2); + + waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5); + + if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) { + majorSoftwareVersion = shtpData[2]; + minorSoftwareVersion = shtpData[3]; + patchSoftwareVersion = (shtpData[13] << 8) | shtpData[12]; + partNumber = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | shtpData[4]; + buildNumber = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | shtpData[8]; #if BNO_DEBUG - _debugPort->printf("BNO080 detected!\r\n"); + _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n", + majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion, + buildNumber, partNumber); #endif - // At system startup, the hub must send its full advertisement message (see SHTP 5.2 and 5.3) to the - // host. It must not send any other data until this step is complete. - // We don't actually care what's in it, we're just using it as a signal to indicate that the reset is complete. - receivePacket(); - _debugPort->printf("::begin -> exited receivePacket() function \r\n"); - // now, after startup, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command - waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET); - - // Next, officially tell it to initialize, and wait for a successful Initialize Response - zeroBuffer(); - shtpData[3] = 0; - sendCommand(COMMAND_INITIALIZE); - - if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE) || shtpData[2] != COMMAND_INITIALIZE || shtpData[5] != 0) - { - _debugPort->printf("%u %u \r\n ", shtpData[2], shtpData[5]); - _debugPort->printf("BNO080 reports initialization failed.\n"); - __enable_irq(); - _debugPort->printf("returning false \r\n"); - return false; - } - else - { -#if BNO_DEBUG - _debugPort->printf("BNO080 reports initialization successful!\n"); -#endif - } - - _debugPort->printf("outside if \r\n"); + } else { + _debugPort->printf("Bad response from product ID command.\n"); + return false; + } - // Finally, we want to interrogate the device about its model and version. - zeroBuffer(); - shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info - shtpData[1] = 0; //Reserved - sendPacket(CHANNEL_CONTROL, 2); - - waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5); - - if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) - { - majorSoftwareVersion = shtpData[2]; - minorSoftwareVersion = shtpData[3]; - patchSoftwareVersion = (shtpData[13] << 8) | shtpData[12]; - partNumber = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | shtpData[4]; - buildNumber = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | shtpData[8]; - -#if BNO_DEBUG - _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n", - majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion, - buildNumber, partNumber); -#endif - - } - else - { - _debugPort->printf("Bad response from product ID command.\n"); - return false; - } - - // successful init - return true; + // successful init + return true; } void BNO080::tare(bool zOnly) { - zeroBuffer(); + zeroBuffer(); - // from SH-2 section 6.4.4.1 - shtpData[3] = 0; // perform tare now + // from SH-2 section 6.4.4.1 + shtpData[3] = 0; // perform tare now - if(zOnly) - { - shtpData[4] = 0b100; // tare Z axis - } - else - { - shtpData[4] = 0b111; // tare X, Y, and Z axes - } + if(zOnly) { + shtpData[4] = 0b100; // tare Z axis + } else { + shtpData[4] = 0b111; // tare X, Y, and Z axes + } - shtpData[5] = 0; // reorient all motion outputs + shtpData[5] = 0; // reorient all motion outputs - sendCommand(COMMAND_TARE); + sendCommand(COMMAND_TARE); } bool BNO080::enableCalibration(bool calibrateAccel, bool calibrateGyro, bool calibrateMag) { - // send the Configure ME Calibration command - zeroBuffer(); + // send the Configure ME Calibration command + zeroBuffer(); - shtpData[3] = static_cast<uint8_t>(calibrateAccel ? 1 : 0); - shtpData[4] = static_cast<uint8_t>(calibrateGyro ? 1 : 0); - shtpData[5] = static_cast<uint8_t>(calibrateMag ? 1 : 0); + shtpData[3] = static_cast<uint8_t>(calibrateAccel ? 1 : 0); + shtpData[4] = static_cast<uint8_t>(calibrateGyro ? 1 : 0); + shtpData[5] = static_cast<uint8_t>(calibrateMag ? 1 : 0); - shtpData[6] = 0; // Configure ME Calibration command + shtpData[6] = 0; // Configure ME Calibration command - shtpData[7] = 0; // planar accelerometer calibration always disabled + shtpData[7] = 0; // planar accelerometer calibration always disabled - sendCommand(COMMAND_ME_CALIBRATE); + sendCommand(COMMAND_ME_CALIBRATE); - // now, wait for the response - if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE)) - { + // now, wait for the response + if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE)) { #if BNO_DEBUG - _debugPort->printf("Timeout waiting for calibration response!\n"); + _debugPort->printf("Timeout waiting for calibration response!\n"); #endif - return false; - } + return false; + } - if(shtpData[2] != COMMAND_ME_CALIBRATE) - { + if(shtpData[2] != COMMAND_ME_CALIBRATE) { #if BNO_DEBUG - _debugPort->printf("Received wrong response to calibration command!\n"); + _debugPort->printf("Received wrong response to calibration command!\n"); #endif - return false; - } + return false; + } - if(shtpData[5] != 0) - { + if(shtpData[5] != 0) { #if BNO_DEBUG - _debugPort->printf("IMU reports calibrate command failed!\n"); + _debugPort->printf("IMU reports calibrate command failed!\n"); #endif - return false; - } + return false; + } - // acknowledge checks out! - return true; + // acknowledge checks out! + return true; } bool BNO080::saveCalibration() { - zeroBuffer(); + zeroBuffer(); - // no arguments - sendCommand(COMMAND_SAVE_DCD); + // no arguments + sendCommand(COMMAND_SAVE_DCD); - // now, wait for the response - if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE)) - { + // now, wait for the response + if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE)) { #if BNO_DEBUG - _debugPort->printf("Timeout waiting for calibration response!\n"); + _debugPort->printf("Timeout waiting for calibration response!\n"); #endif - return false; - } + return false; + } - if(shtpData[2] != COMMAND_SAVE_DCD) - { + if(shtpData[2] != COMMAND_SAVE_DCD) { #if BNO_DEBUG - _debugPort->printf("Received wrong response to calibration command!\n"); + _debugPort->printf("Received wrong response to calibration command!\n"); #endif - return false; - } + return false; + } - if(shtpData[5] != 0) - { + if(shtpData[5] != 0) { #if BNO_DEBUG - _debugPort->printf("IMU reports calibrate command failed!\n"); + _debugPort->printf("IMU reports calibrate command failed!\n"); #endif - return false; - } + return false; + } - // acknowledge checks out! - return true; + // acknowledge checks out! + return true; } void BNO080::setSensorOrientation(Quaternion orientation) { - zeroBuffer(); + zeroBuffer(); + + _debugPort->printf("y: %f", orientation.y()); - // convert floats to Q - int16_t Q_x = floatToQ(orientation.x(), ORIENTATION_QUAT_Q_POINT); - int16_t Q_y = floatToQ(orientation.y(), ORIENTATION_QUAT_Q_POINT); - int16_t Q_z = floatToQ(orientation.z(), ORIENTATION_QUAT_Q_POINT); - int16_t Q_w = floatToQ(orientation.w(), ORIENTATION_QUAT_Q_POINT); + // convert floats to Q + int16_t Q_x = floatToQ(orientation.x(), ORIENTATION_QUAT_Q_POINT); + int16_t Q_y = floatToQ(orientation.y(), ORIENTATION_QUAT_Q_POINT); + int16_t Q_z = floatToQ(orientation.z(), ORIENTATION_QUAT_Q_POINT); + int16_t Q_w = floatToQ(orientation.w(), ORIENTATION_QUAT_Q_POINT); - shtpData[3] = 2; // set reorientation + _debugPort->printf("Q_y: %hd", Q_y); - shtpData[4] = static_cast<uint8_t>(Q_x & 0xFF); //P1 - X component LSB - shtpData[5] = static_cast<uint8_t>(Q_x >> 8); //P2 - X component MSB + shtpData[3] = 2; // set reorientation - shtpData[6] = static_cast<uint8_t>(Q_y & 0xFF); //P3 - Y component LSB - shtpData[7] = static_cast<uint8_t>(Q_y >> 8); //P4 - Y component MSB + shtpData[4] = static_cast<uint8_t>(Q_x & 0xFF); //P1 - X component LSB + shtpData[5] = static_cast<uint8_t>(Q_x >> 8); //P2 - X component MSB - shtpData[8] = static_cast<uint8_t>(Q_z & 0xFF); //P5 - Z component LSB - shtpData[9] = static_cast<uint8_t>(Q_z >> 8); //P6 - Z component MSB + shtpData[6] = static_cast<uint8_t>(Q_y & 0xFF); //P3 - Y component LSB + shtpData[7] = static_cast<uint8_t>(Q_y >> 8); //P4 - Y component MSB - shtpData[10] = static_cast<uint8_t>(Q_w & 0xFF); //P7 - W component LSB - shtpData[11] = static_cast<uint8_t>(Q_w >> 8); //P8 - W component MSB + shtpData[8] = static_cast<uint8_t>(Q_z & 0xFF); //P5 - Z component LSB + shtpData[9] = static_cast<uint8_t>(Q_z >> 8); //P6 - Z component MSB - //Using this shtpData packet, send a command - sendCommand(COMMAND_TARE); // Send tare command + shtpData[10] = static_cast<uint8_t>(Q_w & 0xFF); //P7 - W component LSB + shtpData[11] = static_cast<uint8_t>(Q_w >> 8); //P8 - W component MSB - // NOTE: unlike literally every other command, a sensor orientation command is never acknowledged in any way. + //Using this shtpData packet, send a command + sendCommand(COMMAND_TARE); // Send tare command + + // NOTE: unlike literally every other command, a sensor orientation command is never acknowledged in any way. } -#define ORIENTATION_RECORD_LEN 4 - -bool BNO080::setPermanentOrientation(Quaternion orientation) -{ - uint32_t orientationRecord[ORIENTATION_RECORD_LEN]; - - // each word is one element of the quaternion - orientationRecord[0] = static_cast<uint32_t>(floatToQ_dword(orientation.x(), FRS_ORIENTATION_Q_POINT)); - orientationRecord[1] = static_cast<uint32_t>(floatToQ_dword(orientation.y(), FRS_ORIENTATION_Q_POINT)); - orientationRecord[2] = static_cast<uint32_t>(floatToQ_dword(orientation.z(), FRS_ORIENTATION_Q_POINT)); - orientationRecord[3] = static_cast<uint32_t>(floatToQ_dword(orientation.w(), FRS_ORIENTATION_Q_POINT)); - - return writeFRSRecord(FRS_RECORDID_SYSTEM_ORIENTATION, orientationRecord, ORIENTATION_RECORD_LEN); -} bool BNO080::updateData() { - if(_int.read() != 0) - { - // no waiting packets - return false; - } + if(_int.read() != 0) { + // no waiting packets + return false; + } - while(_int.read() == 0) - { - if(!receivePacket()) - { - // comms error - return false; - } + while(_int.read() == 0) { + if(!receivePacket()) { + // comms error + return false; + } - processPacket(); - } + processPacket(); + } - // packets were received, so data may have changed - return true; + // packets were received, so data may have changed + return true; } uint8_t BNO080::getReportStatus(Report report) { - uint8_t reportNum = static_cast<uint8_t>(report); - if(reportNum > STATUS_ARRAY_LEN) - { - return 0; - } + uint8_t reportNum = static_cast<uint8_t>(report); + if(reportNum > STATUS_ARRAY_LEN) { + return 0; + } - return reportStatus[reportNum]; + return reportStatus[reportNum]; } const char* BNO080::getReportStatusString(Report report) { - switch(getReportStatus(report)) - { - case 0: - return "Unreliable"; - case 1: - return "Accuracy Low"; - case 2: - return "Accuracy Medium"; - case 3: - return "Accuracy High"; - default: - return "Error"; - } + switch(getReportStatus(report)) { + case 0: + return "Unreliable"; + case 1: + return "Accuracy Low"; + case 2: + return "Accuracy Medium"; + case 3: + return "Accuracy High"; + default: + return "Error"; + } } bool BNO080::hasNewData(Report report) { - uint8_t reportNum = static_cast<uint8_t>(report); - if(reportNum > STATUS_ARRAY_LEN) - { - return false; - } + uint8_t reportNum = static_cast<uint8_t>(report); + if(reportNum > STATUS_ARRAY_LEN) { + return false; + } - bool newData = reportHasBeenUpdated[reportNum]; - reportHasBeenUpdated[reportNum] = false; // clear flag - return newData; + bool newData = reportHasBeenUpdated[reportNum]; + reportHasBeenUpdated[reportNum] = false; // clear flag + return newData; } //Sends the packet to enable the rotation vector void BNO080::enableReport(Report report, uint16_t timeBetweenReports) { - _debugPort->printf("begin enable/r/n"); -#if BNO_DEBUG - // check time is valid - float periodSeconds = static_cast<float>(timeBetweenReports / 1000.0); + // check time + float periodSeconds = timeBetweenReports / 1000.0; - if(periodSeconds < getMinPeriod(report)) - { - _debugPort->printf("Error: attempt made to set report 0x%02hhx to period of %.06f s, which is smaller than its min period of %.06f s.\r\n", - static_cast<uint8_t>(report), periodSeconds, getMinPeriod(report)); - return; - } - - - - - - - -#endif - setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports); + if(periodSeconds < getMinPeriod(report)) { + _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", + static_cast<uint8_t>(report), periodSeconds, getMinPeriod(report)); + return; + } + /* + else if(getMaxPeriod(report) > 0 && periodSeconds > getMaxPeriod(report)) + { + _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", + static_cast<uint8_t>(report), periodSeconds, getMaxPeriod(report)); + return; + } + */ + setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports); - // note: we don't wait for ACKs on these packets because they can take quite a while, like half a second, to come in + // note: we don't wait for ACKs on these packets because they can take quite a while, like half a second, to come in } void BNO080::disableReport(Report report) { - // set the report's polling period to zero to disable it - setFeatureCommand(static_cast<uint8_t>(report), 0); + // set the report's polling period to zero to disable it + setFeatureCommand(static_cast<uint8_t>(report), 0); } uint32_t BNO080::getSerialNumber() { - uint32_t serNoBuffer; + uint32_t serNoBuffer; - if(!readFRSRecord(FRS_RECORDID_SERIAL_NUMBER, &serNoBuffer, 1)) - { - return 0; - } + if(!readFRSRecord(FRS_RECORDID_SERIAL_NUMBER, &serNoBuffer, 1)) { + return 0; + } - return serNoBuffer; + return serNoBuffer; } float BNO080::getRange(Report report) { - loadReportMetadata(report); + loadReportMetadata(report); - return qToFloat_dword(metadataRecord[1], getQ1(report)); + return qToFloat_dword(metadataRecord[1], getQ1(report)); } float BNO080::getResolution(Report report) { - loadReportMetadata(report); + loadReportMetadata(report); - return qToFloat_dword(metadataRecord[2], getQ1(report)); + return qToFloat_dword(metadataRecord[2], getQ1(report)); } float BNO080::getPower(Report report) { - loadReportMetadata(report); + loadReportMetadata(report); - uint16_t powerQ = static_cast<uint16_t>(metadataRecord[3] & 0xFFFF); + uint16_t powerQ = static_cast<uint16_t>(metadataRecord[3] & 0xFFFF); - return qToFloat_dword(powerQ, POWER_Q_POINT); + return qToFloat_dword(powerQ, POWER_Q_POINT); } float BNO080::getMinPeriod(Report report) { - loadReportMetadata(report); + loadReportMetadata(report); - return metadataRecord[4] / 1e6f; // convert from microseconds to seconds + return metadataRecord[4] / 1e6f; // convert from microseconds to seconds } float BNO080::getMaxPeriod(Report report) { - loadReportMetadata(report); + loadReportMetadata(report); - if(getMetaVersion() == 3) - { - // no max period entry in this record format - return -1.0f; - } + if(getMetaVersion() == 3) { + // no max period entry in this record format + return -1.0f; + } - return metadataRecord[9] / 1e6f; // convert from microseconds to seconds + return metadataRecord[9] / 1e6f; // convert from microseconds to seconds } void BNO080::printMetadataSummary(Report report) { #if BNO_DEBUG - if(!loadReportMetadata(report)) - { - _debugPort->printf("Failed to load report metadata!\n"); - } + if(!loadReportMetadata(report)) { + _debugPort->printf("Failed to load report metadata!\n"); + } + + _debugPort->printf("======= Metadata for report 0x%02hhx =======\n", static_cast<uint8_t>(report)); - _debugPort->printf("======= Metadata for report 0x%02hhx =======\n", static_cast<uint8_t>(report)); - - _debugPort->printf("Range: +- %.04f units\n", getRange(report)); - _debugPort->printf("Resolution: %.04f units\n", getResolution(report)); - _debugPort->printf("Power Used: %.03f mA\n", getPower(report)); - _debugPort->printf("Min Period: %.06f s\n", getMinPeriod(report)); - _debugPort->printf("Max Period: %.06f s\n\n", getMaxPeriod(report)); + _debugPort->printf("Range: +- %.04f units\n", getRange(report)); + _debugPort->printf("Resolution: %.04f units\n", getResolution(report)); + _debugPort->printf("Power Used: %.03f mA\n", getPower(report)); + _debugPort->printf("Min Period: %.06f s\n", getMinPeriod(report)); + _debugPort->printf("Max Period: %.06f s\n\n", getMaxPeriod(report)); #endif } int16_t BNO080::getQ1(Report report) { - loadReportMetadata(report); + loadReportMetadata(report); - return static_cast<int16_t>(metadataRecord[7] & 0xFFFF); + return static_cast<int16_t>(metadataRecord[7] & 0xFFFF); } int16_t BNO080::getQ2(Report report) { - loadReportMetadata(report); + loadReportMetadata(report); - return static_cast<int16_t>(metadataRecord[7] >> 16); + return static_cast<int16_t>(metadataRecord[7] >> 16); } int16_t BNO080::getQ3(Report report) { - loadReportMetadata(report); + loadReportMetadata(report); - return static_cast<int16_t>(metadataRecord[8] >> 16); + return static_cast<int16_t>(metadataRecord[8] >> 16); } void BNO080::processPacket() { - if(shtpHeader[2] == CHANNEL_CONTROL) - { - // currently no command reports are read - } - else if(shtpHeader[2] == CHANNEL_EXECUTABLE) - { - // currently no executable reports are read - } - else if(shtpHeader[2] == CHANNEL_COMMAND) - { + if(shtpHeader[2] == CHANNEL_CONTROL) { + // currently no command reports are read + } else if(shtpHeader[2] == CHANNEL_EXECUTABLE) { + // currently no executable reports are read + } else if(shtpHeader[2] == CHANNEL_COMMAND) { - } - else if(shtpHeader[2] == CHANNEL_REPORTS || shtpHeader[2] == CHANNEL_WAKE_REPORTS) - { - if(shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP) - { - // sensor data packet - parseSensorDataPacket(); - } - } + } else if(shtpHeader[2] == CHANNEL_REPORTS || shtpHeader[2] == CHANNEL_WAKE_REPORTS) { + if(shtpData[0] == SHTP_REPORT_BASE_TIMESTAMP) { + // sensor data packet + //_debugPort->printf("\r\t\t enter pareseSensorDataPacket \r\n"); + parseSensorDataPacket(); + } + } } // sizes of various sensor data packet elements @@ -598,875 +539,672 @@ void BNO080::parseSensorDataPacket() { - size_t currReportOffset = 0; + size_t currReportOffset = 0; - // every sensor data report first contains a timestamp offset to show how long it has been between when - // the host interrupt was sent and when the packet was transmitted. - // We don't use interrupts and don't care about times, so we can throw this out. - currReportOffset += SIZEOF_BASE_TIMESTAMP; + // every sensor data report first contains a timestamp offset to show how long it has been between when + // the host interrupt was sent and when the packet was transmitted. + // We don't use interrupts and don't care about times, so we can throw this out. + currReportOffset += SIZEOF_BASE_TIMESTAMP; - while(currReportOffset < packetLength) - { - if(currReportOffset >= STORED_PACKET_SIZE) - { - _debugPort->printf("Error: sensor report longer than packet buffer! Some data was not read! Increase buffer size or decrease number of reports!\r\n"); - return; - } + while(currReportOffset < packetLength) { + if(currReportOffset >= STORED_PACKET_SIZE) { + _debugPort->printf("Error: sensor report longer than packet buffer!\n"); + return; + } - // lots of sensor reports use 3 16-bit numbers stored in bytes 4 through 9 - // we can save some time by parsing those out here. - uint16_t data1 = (uint16_t)shtpData[currReportOffset + 5] << 8 | shtpData[currReportOffset + 4]; - uint16_t data2 = (uint16_t)shtpData[currReportOffset + 7] << 8 | shtpData[currReportOffset + 6]; - uint16_t data3 = (uint16_t)shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8]; + // lots of sensor reports use 3 16-bit numbers stored in bytes 4 through 9 + // we can save some time by parsing those out here. + uint16_t data1 = (uint16_t)shtpData[currReportOffset + 5] << 8 | shtpData[currReportOffset + 4]; + uint16_t data2 = (uint16_t)shtpData[currReportOffset + 7] << 8 | shtpData[currReportOffset + 6]; + uint16_t data3 = (uint16_t)shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8]; - uint8_t reportNum = shtpData[currReportOffset]; + uint8_t reportNum = shtpData[currReportOffset]; + + if(reportNum != SENSOR_REPORTID_TIMESTAMP_REBASE) { + // set status from byte 2 + reportStatus[reportNum] = static_cast<uint8_t>(shtpData[currReportOffset + 2] & 0b11); - if(reportNum != SENSOR_REPORTID_TIMESTAMP_REBASE) - { - // set status from byte 2 - reportStatus[reportNum] = static_cast<uint8_t>(shtpData[currReportOffset + 2] & 0b11); + // set updated flag + reportHasBeenUpdated[reportNum] = true; + } - // set updated flag - reportHasBeenUpdated[reportNum] = true; - } + switch(shtpData[currReportOffset]) { + case SENSOR_REPORTID_TIMESTAMP_REBASE: + currReportOffset += SIZEOF_TIMESTAMP_REBASE; + break; + + case SENSOR_REPORTID_ACCELEROMETER: - switch(shtpData[currReportOffset]) - { - case SENSOR_REPORTID_TIMESTAMP_REBASE: - currReportOffset += SIZEOF_TIMESTAMP_REBASE; - break; - - case SENSOR_REPORTID_ACCELEROMETER: + totalAcceleration = TVector3( + qToFloat(data1, ACCELEROMETER_Q_POINT), + qToFloat(data2, ACCELEROMETER_Q_POINT), + qToFloat(data3, ACCELEROMETER_Q_POINT)); - totalAcceleration = TVector3( - qToFloat(data1, ACCELEROMETER_Q_POINT), - qToFloat(data2, ACCELEROMETER_Q_POINT), - qToFloat(data3, ACCELEROMETER_Q_POINT)); + currReportOffset += SIZEOF_ACCELEROMETER; + break; + + case SENSOR_REPORTID_LINEAR_ACCELERATION: - currReportOffset += SIZEOF_ACCELEROMETER; - break; - - case SENSOR_REPORTID_LINEAR_ACCELERATION: + linearAcceleration = TVector3( + qToFloat(data1, ACCELEROMETER_Q_POINT), + qToFloat(data2, ACCELEROMETER_Q_POINT), + qToFloat(data3, ACCELEROMETER_Q_POINT)); - linearAcceleration = TVector3( - qToFloat(data1, ACCELEROMETER_Q_POINT), - qToFloat(data2, ACCELEROMETER_Q_POINT), - qToFloat(data3, ACCELEROMETER_Q_POINT)); + currReportOffset += SIZEOF_LINEAR_ACCELERATION; + break; + + case SENSOR_REPORTID_GRAVITY: - currReportOffset += SIZEOF_LINEAR_ACCELERATION; - break; - - case SENSOR_REPORTID_GRAVITY: + gravityAcceleration = TVector3( + qToFloat(data1, ACCELEROMETER_Q_POINT), + qToFloat(data2, ACCELEROMETER_Q_POINT), + qToFloat(data3, ACCELEROMETER_Q_POINT)); - gravityAcceleration = TVector3( - qToFloat(data1, ACCELEROMETER_Q_POINT), - qToFloat(data2, ACCELEROMETER_Q_POINT), - qToFloat(data3, ACCELEROMETER_Q_POINT)); + currReportOffset += SIZEOF_LINEAR_ACCELERATION; + break; - currReportOffset += SIZEOF_LINEAR_ACCELERATION; - break; + case SENSOR_REPORTID_GYROSCOPE_CALIBRATED: - case SENSOR_REPORTID_GYROSCOPE_CALIBRATED: + gyroRotation = TVector3( + qToFloat(data1, GYRO_Q_POINT), + qToFloat(data2, GYRO_Q_POINT), + qToFloat(data3, GYRO_Q_POINT)); - gyroRotation = TVector3( - qToFloat(data1, GYRO_Q_POINT), - qToFloat(data2, GYRO_Q_POINT), - qToFloat(data3, GYRO_Q_POINT)); + currReportOffset += SIZEOF_GYROSCOPE_CALIBRATED; + break; - currReportOffset += SIZEOF_GYROSCOPE_CALIBRATED; - break; + case SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED: - case SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED: + magField = TVector3( + qToFloat(data1, MAGNETOMETER_Q_POINT), + qToFloat(data2, MAGNETOMETER_Q_POINT), + qToFloat(data3, MAGNETOMETER_Q_POINT)); - magField = TVector3( - qToFloat(data1, MAGNETOMETER_Q_POINT), - qToFloat(data2, MAGNETOMETER_Q_POINT), - qToFloat(data3, MAGNETOMETER_Q_POINT)); - - currReportOffset += SIZEOF_MAGNETIC_FIELD_CALIBRATED; - break; + currReportOffset += SIZEOF_MAGNETIC_FIELD_CALIBRATED; + break; - case SENSOR_REPORTID_MAGNETIC_FIELD_UNCALIBRATED: - { - magFieldUncalibrated = TVector3( - qToFloat(data1, MAGNETOMETER_Q_POINT), - qToFloat(data2, MAGNETOMETER_Q_POINT), - qToFloat(data3, MAGNETOMETER_Q_POINT)); + case SENSOR_REPORTID_MAGNETIC_FIELD_UNCALIBRATED: { + magFieldUncalibrated = TVector3( + qToFloat(data1, MAGNETOMETER_Q_POINT), + qToFloat(data2, MAGNETOMETER_Q_POINT), + qToFloat(data3, MAGNETOMETER_Q_POINT)); - uint16_t ironOffsetXQ = shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; - uint16_t ironOffsetYQ = shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12]; - uint16_t ironOffsetZQ = shtpData[currReportOffset + 15] << 8 | shtpData[currReportOffset + 14]; + uint16_t ironOffsetXQ = shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; + uint16_t ironOffsetYQ = shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12]; + uint16_t ironOffsetZQ = shtpData[currReportOffset + 15] << 8 | shtpData[currReportOffset + 14]; - hardIronOffset = TVector3( - qToFloat(ironOffsetXQ, MAGNETOMETER_Q_POINT), - qToFloat(ironOffsetYQ, MAGNETOMETER_Q_POINT), - qToFloat(ironOffsetZQ, MAGNETOMETER_Q_POINT)); + hardIronOffset = TVector3( + qToFloat(ironOffsetXQ, MAGNETOMETER_Q_POINT), + qToFloat(ironOffsetYQ, MAGNETOMETER_Q_POINT), + qToFloat(ironOffsetZQ, MAGNETOMETER_Q_POINT)); - currReportOffset += SIZEOF_MAGNETIC_FIELD_UNCALIBRATED; - } - break; + currReportOffset += SIZEOF_MAGNETIC_FIELD_UNCALIBRATED; + } + break; - case SENSOR_REPORTID_ROTATION_VECTOR: - { - uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; - uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12]; + case SENSOR_REPORTID_ROTATION_VECTOR: { + uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; + uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12]; - rotationVector = TVector4( - qToFloat(data1, ROTATION_Q_POINT), - qToFloat(data2, ROTATION_Q_POINT), - qToFloat(data3, ROTATION_Q_POINT), - qToFloat(realPartQ, ROTATION_Q_POINT)); + rotationVector = TVector4( + qToFloat(data1, ROTATION_Q_POINT), + qToFloat(data2, ROTATION_Q_POINT), + qToFloat(data3, ROTATION_Q_POINT), + qToFloat(realPartQ, ROTATION_Q_POINT)); - rotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT); + rotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT); - currReportOffset += SIZEOF_ROTATION_VECTOR; - } - break; + currReportOffset += SIZEOF_ROTATION_VECTOR; + } + break; - case SENSOR_REPORTID_GAME_ROTATION_VECTOR: - { - uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; + case SENSOR_REPORTID_GAME_ROTATION_VECTOR: { + uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; - gameRotationVector = TVector4( - qToFloat(data1, ROTATION_Q_POINT), - qToFloat(data2, ROTATION_Q_POINT), - qToFloat(data3, ROTATION_Q_POINT), - qToFloat(realPartQ, ROTATION_Q_POINT)); + gameRotationVector = TVector4( + qToFloat(data1, ROTATION_Q_POINT), + qToFloat(data2, ROTATION_Q_POINT), + qToFloat(data3, ROTATION_Q_POINT), + qToFloat(realPartQ, ROTATION_Q_POINT)); - currReportOffset += SIZEOF_GAME_ROTATION_VECTOR; - } - break; + currReportOffset += SIZEOF_GAME_ROTATION_VECTOR; + } + break; - case SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR: - { - uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; - uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12]; - - geomagneticRotationVector = TVector4( - qToFloat(data1, ROTATION_Q_POINT), - qToFloat(data2, ROTATION_Q_POINT), - qToFloat(data3, ROTATION_Q_POINT), - qToFloat(realPartQ, ROTATION_Q_POINT)); + case SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR: { + uint16_t realPartQ = (uint16_t) shtpData[currReportOffset + 11] << 8 | shtpData[currReportOffset + 10]; + uint16_t accuracyQ = (uint16_t) shtpData[currReportOffset + 13] << 8 | shtpData[currReportOffset + 12]; - geomagneticRotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT); + geomagneticRotationVector = TVector4( + qToFloat(data1, ROTATION_Q_POINT), + qToFloat(data2, ROTATION_Q_POINT), + qToFloat(data3, ROTATION_Q_POINT), + qToFloat(realPartQ, ROTATION_Q_POINT)); - currReportOffset += SIZEOF_GEOMAGNETIC_ROTATION_VECTOR; - } - break; - - case SENSOR_REPORTID_TAP_DETECTOR: + geomagneticRotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT); - // since we got the report, a tap was detected - tapDetected = true; + currReportOffset += SIZEOF_GEOMAGNETIC_ROTATION_VECTOR; + } + break; - doubleTap = (shtpData[currReportOffset + 4] & (1 << 6)) != 0; + case SENSOR_REPORTID_TAP_DETECTOR: - currReportOffset += SIZEOF_TAP_DETECTOR; - break; + // since we got the report, a tap was detected + tapDetected = true; + + doubleTap = (shtpData[currReportOffset + 4] & (1 << 6)) != 0; - case SENSOR_REPORTID_STABILITY_CLASSIFIER: - { - uint8_t classificationNumber = shtpData[currReportOffset + 4]; + currReportOffset += SIZEOF_TAP_DETECTOR; + break; + + case SENSOR_REPORTID_STABILITY_CLASSIFIER: { + uint8_t classificationNumber = shtpData[currReportOffset + 4]; - if(classificationNumber > 4) - { - classificationNumber = 0; - } + if(classificationNumber > 4) { + classificationNumber = 0; + } - stability = static_cast<Stability>(classificationNumber); + stability = static_cast<Stability>(classificationNumber); - currReportOffset += SIZEOF_STABILITY_REPORT; - } - break; + currReportOffset += SIZEOF_STABILITY_REPORT; + } + break; - case SENSOR_REPORTID_STEP_DETECTOR: + case SENSOR_REPORTID_STEP_DETECTOR: - // the fact that we got the report means that a step was detected - stepDetected = true; - - currReportOffset += SIZEOF_STEP_DETECTOR; + // the fact that we got the report means that a step was detected + stepDetected = true; - break; + currReportOffset += SIZEOF_STEP_DETECTOR; - case SENSOR_REPORTID_STEP_COUNTER: + break; - stepCount = shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8]; + case SENSOR_REPORTID_STEP_COUNTER: - currReportOffset += SIZEOF_STEP_COUNTER; + stepCount = shtpData[currReportOffset + 9] << 8 | shtpData[currReportOffset + 8]; - break; + currReportOffset += SIZEOF_STEP_COUNTER; - case SENSOR_REPORTID_SIGNIFICANT_MOTION: + break; - // the fact that we got the report means that significant motion was detected - significantMotionDetected = true; + case SENSOR_REPORTID_SIGNIFICANT_MOTION: - currReportOffset += SIZEOF_SIGNIFICANT_MOTION; - - break; + // the fact that we got the report means that significant motion was detected + significantMotionDetected = true; - case SENSOR_REPORTID_SHAKE_DETECTOR: + currReportOffset += SIZEOF_SIGNIFICANT_MOTION; - shakeDetected = true; + case SENSOR_REPORTID_SHAKE_DETECTOR: + + shakeDetected = true; - xAxisShake = (shtpData[currReportOffset + 4] & 1) != 0; - yAxisShake = (shtpData[currReportOffset + 4] & (1 << 1)) != 0; - zAxisShake = (shtpData[currReportOffset + 4] & (1 << 2)) != 0; - - currReportOffset += SIZEOF_SHAKE_DETECTOR; + xAxisShake = (shtpData[currReportOffset + 4] & 1) != 0; + yAxisShake = (shtpData[currReportOffset + 4] & (1 << 1)) != 0; + zAxisShake = (shtpData[currReportOffset + 4] & (1 << 2)) != 0; - break; - - default: - _debugPort->printf("Error: unrecognized report ID in sensor report: %hhx. Byte %u, length %hu\n", shtpData[currReportOffset], currReportOffset, packetLength); - return; - } - } + currReportOffset += SIZEOF_SHAKE_DETECTOR; + + default: + _debugPort->printf("Error: unrecognized report ID in sensor report: %hhx. Byte %u, length %hu\n", shtpData[currReportOffset], currReportOffset, packetLength); + return; + } + } } bool BNO080::waitForPacket(int channel, uint8_t reportID, float timeout) { - Timer timeoutTimer; - timeoutTimer.start(); + Timer timeoutTimer; + timeoutTimer.start(); - while(timeoutTimer.read() <= timeout) - { - if(_int.read() == 0) - { - if(!receivePacket(timeout)) - { - _debugPort->printf("cannot receive\r\n"); - return false; - } + while(timeoutTimer.read() <= timeout) { + if(_int.read() == 0) { + if(!receivePacket(timeout)) { + return false; + } - if(channel == shtpHeader[2] && reportID == shtpData[0]) - { - // found correct packet! - _debugPort->printf("can receive/r/n"); - return true; - } - else - { - // other data packet, send to proper channels - _debugPort->printf("other/r/n"); - processPacket(); - } - } - } + if(channel == shtpHeader[2] && reportID == shtpData[0]) { + // found correct packet! + _debugPort->printf("\r\t found the correct packet \r\n"); + return true; + } else { + // other data packet, send to proper channels + _debugPort->printf("\r\t other data packets, sending to proper channel\r\n"); + processPacket(); + //return false; + } + } + } - _debugPort->printf("Packet wait timeout.\n"); - return false; + _debugPort->printf("Packet wait timeout.\n"); + return false; } //Given a register value and a Q point, convert to float //See https://en.wikipedia.org/wiki/Q_(number_format) float BNO080::qToFloat(int16_t fixedPointValue, uint8_t qPoint) { - float qFloat = fixedPointValue; - qFloat *= pow(2, qPoint * -1); - return (qFloat); + float qFloat = fixedPointValue; + qFloat *= pow(2.0, qPoint * -1.0); + return (qFloat); } float BNO080::qToFloat_dword(uint32_t fixedPointValue, int16_t qPoint) { - float qFloat = fixedPointValue; - qFloat *= pow(2, qPoint * -1); - return (qFloat); + float qFloat = fixedPointValue; + qFloat *= pow(2.0, qPoint * -1.0); + return (qFloat); } //Given a floating point value and a Q point, convert to Q //See https://en.wikipedia.org/wiki/Q_(number_format) int16_t BNO080::floatToQ(float qFloat, uint8_t qPoint) { - int16_t qVal = static_cast<int16_t>(qFloat * pow(2, qPoint)); - return qVal; + int16_t qVal = static_cast<int16_t>(qFloat * pow(2.0, qPoint)); + return qVal; } -int32_t BNO080::floatToQ_dword(float qFloat, uint16_t qPoint) -{ - int32_t qVal = static_cast<int32_t>(qFloat * pow(2, qPoint)); - return qVal; -} //Tell the sensor to do a command //See 6.3.8 page 41, Command request //The caller is expected to set P0 through P8 prior to calling void BNO080::sendCommand(uint8_t command) { - shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request - shtpData[1] = commandSequenceNumber++; //Increments automatically each function call - shtpData[2] = command; //Command + shtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request + shtpData[1] = commandSequenceNumber++; //Increments automatically each function call + shtpData[2] = command; //Command - //Caller must set these - /* - shtpData[3] = 0; //P0 - shtpData[4] = 0; //P1 - shtpData[5] = 0; //P2 - shtpData[6] = 0; - shtpData[7] = 0; - shtpData[8] = 0; - shtpData[9] = 0; - shtpData[10] = 0; - shtpData[11] = 0; - */ - //Transmit packet on channel 2, 12 bytes - sendPacket(CHANNEL_CONTROL, 12); + //Caller must set these + /*shtpData[3] = 0; //P0 + shtpData[4] = 0; //P1 + shtpData[5] = 0; //P2 + shtpData[6] = 0; + shtpData[7] = 0; + shtpData[8] = 0; + shtpData[9] = 0; + shtpData[10] = 0; + shtpData[11] = 0;*/ + + //Transmit packet on channel 2, 12 bytes + sendPacket(CHANNEL_CONTROL, 12); } //Given a sensor's report ID, this tells the BNO080 to begin reporting the values //Also sets the specific config word. Useful for personal activity classifier void BNO080::setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig) { - uint32_t microsBetweenReports = static_cast<uint32_t>(timeBetweenReports * 1000); + uint32_t microsBetweenReports = static_cast<uint32_t>(timeBetweenReports * 1000); - const uint32_t batchMicros = 0; + const uint32_t batchMicros = 0; - shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55 - shtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector - shtpData[2] = 0; //Feature flags - shtpData[3] = 0; //Change sensitivity (LSB) - shtpData[4] = 0; //Change sensitivity (MSB) - shtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms - shtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval - shtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval - shtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB) - shtpData[9] = (batchMicros >> 0) & 0xFF; //Batch Interval (LSB) - shtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval - shtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval - shtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB) - shtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB) - shtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config - shtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config - shtpData[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB) + shtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55 + shtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector + shtpData[2] = 0; //Feature flags + shtpData[3] = 0; //Change sensitivity (LSB) + shtpData[4] = 0; //Change sensitivity (MSB) + shtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms + shtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval + shtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval + shtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB) + shtpData[9] = (batchMicros >> 0) & 0xFF; //Batch Interval (LSB) + shtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval + shtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval + shtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB) + shtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB) + shtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config + shtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config + shtpData[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB) - //Transmit packet on channel 2, 17 bytes - sendPacket(CHANNEL_CONTROL, 17); + //Transmit packet on channel 2, 17 bytes + sendPacket(CHANNEL_CONTROL, 17); } bool BNO080::readFRSRecord(uint16_t recordID, uint32_t* readBuffer, uint16_t readLength) { - // send initial read request - zeroBuffer(); - - shtpData[0] = SHTP_REPORT_FRS_READ_REQUEST; - // read offset of 0 -> start at the start of the record - shtpData[2] = 0; - shtpData[3] = 0; - // record ID - shtpData[4] = static_cast<uint8_t>(recordID & 0xFF); - shtpData[5] = static_cast<uint8_t>(recordID >> 8); - // block size - shtpData[6] = static_cast<uint8_t>(readLength & 0xFF); - shtpData[7] = static_cast<uint8_t>(readLength >> 8); - - sendPacket(CHANNEL_CONTROL, 8); - - // now, read back the responses - size_t readOffset = 0; - while(readOffset < readLength) - { - // it seems like it can take quite a long time for FRS data to be read, so we have to increase the timeout - if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE, .3f)) - { -#if BNO_DEBUG - _debugPort->printf("Error: did not receive FRS read response after sending read request!\n"); -#endif - return false; - } + // send initial read request + zeroBuffer(); - uint8_t status = static_cast<uint8_t>(shtpData[1] & 0b1111); - uint8_t dataLength = shtpData[1] >> 4; + shtpData[0] = SHTP_REPORT_FRS_READ_REQUEST; + // read offset of 0 -> start at the start of the record + shtpData[2] = 0; + shtpData[3] = 0; + // record ID + shtpData[4] = static_cast<uint8_t>(recordID & 0xFF); + shtpData[5] = static_cast<uint8_t>(recordID >> 8); + // block size + shtpData[6] = static_cast<uint8_t>(readLength & 0xFF); + shtpData[7] = static_cast<uint8_t>(readLength >> 8); - // check status - if(status == 1) - { -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports invalid record ID!\n"); -#endif - return false; - } - else if(status == 2) - { + sendPacket(CHANNEL_CONTROL, 8); + + // now, read back the responses + size_t readOffset = 0; + while(readOffset < readLength) { + if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE)) { #if BNO_DEBUG - _debugPort->printf("Error: FRS is busy!\n"); -#endif - return false; - } - else if(status == 4) - { -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports offset is out of range!\n"); -#endif - return false; - } - else if(status == 5) - { -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports record %hx is empty!\n", recordID); + _debugPort->printf("Error: did not receive FRS read response after sending read request!\n"); #endif - return false; - } - else if(status == 8) - { -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports flash memory device unavailable!\n"); -#endif - return false; - } + return false; + } - // check data length - if(dataLength == 0) - { -#if BNO_DEBUG - _debugPort->printf("Error: Received FRS packet with 0 data length!\n"); -#endif - return false; - } - else if(dataLength == 1) - { - if(readOffset + 1 != readLength) - { -#if BNO_DEBUG - _debugPort->printf("Error: Received 1 length packet but more than 1 byte remains to be be read!\n"); -#endif - return false; - } - } - - // now, _finally_, read the dang words - readBuffer[readOffset] = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | (shtpData[4]); + uint8_t status = static_cast<uint8_t>(shtpData[1] & 0b1111); + uint8_t dataLength = shtpData[1] >> 4; - // check if we only wanted the first word - ++readOffset; - if(readOffset == readLength) - { - break; - } - - readBuffer[readOffset] = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | (shtpData[8]); - readOffset++; - } - - // read successful - return true; - -} - -bool BNO080::writeFRSRecord(uint16_t recordID, uint32_t* buffer, uint16_t length) -{ - // send initial write request, which tells the chip where we're writing - zeroBuffer(); - - shtpData[0] = SHTP_REPORT_FRS_WRITE_REQUEST; - // length to write (must be <= record length) - shtpData[2] = static_cast<uint8_t>(length & 0xFF); - shtpData[3] = static_cast<uint8_t>(length >> 8); - // record ID - shtpData[4] = static_cast<uint8_t>(recordID & 0xFF); - shtpData[5] = static_cast<uint8_t>(recordID >> 8); - - sendPacket(CHANNEL_CONTROL, 6); - - // wait for FRS to become ready - if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, .3f)) - { + // check status + if(status == 1) { +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports invalid record ID!\n"); +#endif + return false; + } else if(status == 2) { +#if BNO_DEBUG + _debugPort->printf("Error: FRS is busy!\n"); +#endif + return false; + } else if(status == 4) { #if BNO_DEBUG - _debugPort->printf("Error: did not receive FRS write ready response after sending write request!\r\n"); + _debugPort->printf("Error: FRS reports offset is out of range!\n"); #endif - return false; - } - - if(shtpData[1] != 4) - { + return false; + } else if(status == 5) { #if BNO_DEBUG - _debugPort->printf("Error: FRS reports error initiating write operation: %hhu!\r\n", shtpData[1]); + _debugPort->printf("Error: FRS reports record %hx is empty!\n", recordID); #endif - return false; - } - - // now, send the actual data - for(uint16_t wordIndex = 0; wordIndex < length; wordIndex += 2) - { - // send packet containing 2 words - zeroBuffer(); - shtpData[0] = SHTP_REPORT_FRS_WRITE_DATA; - - // offset to write at - shtpData[2] = static_cast<uint8_t>(wordIndex & 0xFF); - shtpData[3] = static_cast<uint8_t>(wordIndex >> 8); - - // data 0 - *reinterpret_cast<uint32_t*>(shtpData + 4) = buffer[wordIndex]; + return false; + } else if(status == 8) { +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports flash memory device unavailable!\n"); +#endif + return false; + } - // data 1, if it exists - if(wordIndex != length - 1) - { - *reinterpret_cast<uint32_t*>(shtpData + 8) = buffer[wordIndex + 1]; - } - - sendPacket(CHANNEL_CONTROL, 12); - - // wait for acknowledge - if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, .3f)) - { + // check data length + if(dataLength == 0) { #if BNO_DEBUG - _debugPort->printf("Error: did not receive FRS write response after sending write data!\r\n"); -#endif - return false; - } - - uint8_t status = shtpData[1]; - - switch(status) - { - case 0: - if(length - wordIndex >= 2) - { - // status OK, write still in progress - } - else - { -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports write in progress when it should be complete!\r\n"); + _debugPort->printf("Error: Received FRS packet with 0 data length!\n"); #endif - return false; - } - break; - case 3: - case 8: - if(length - wordIndex <= 2) - { - // status OK, write complete - } - else - { -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports write complete when it should be still going!\n"); -#endif - return false; - } - break; - case 1: -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports invalid record ID!\n"); -#endif - return false; - case 2: -#if BNO_DEBUG - _debugPort->printf("Error: FRS is busy!\n"); -#endif - return false; - case 5: -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports write failed!\n"); -#endif - return false; - case 6: + return false; + } else if(dataLength == 1) { + if(readOffset + 1 != readLength) { #if BNO_DEBUG - _debugPort->printf("Error: FRS reports data received while not in write mode!\n"); -#endif - return false; - case 7: -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports invalid length!\n"); -#endif - return false; - case 9: -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports invalid data for this record!\n"); -#endif - return false; - - case 10: -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports flash device unavailable!\n"); + _debugPort->printf("Error: Received 1 length packet but more than 1 byte remains to be be read!\n"); #endif - return false; + return false; + } + } + + // now, _finally_, read the dang words + readBuffer[readOffset] = (shtpData[7] << 24) | (shtpData[6] << 16) | (shtpData[5] << 8) | (shtpData[4]); - case 11: -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports record is read-only!\n"); -#endif - return false; - default: -#if BNO_DEBUG - _debugPort->printf("Error: FRS reports unknown result code %hhu!\n", status); -#endif - break; + // check if we only wanted the first word + ++readOffset; + if(readOffset == readLength) { + break; + } - } - } + readBuffer[readOffset] = (shtpData[11] << 24) | (shtpData[10] << 16) | (shtpData[9] << 8) | (shtpData[8]); + readOffset++; + } - // write complete - return true; + // read successful + return true; + } //Given the data packet, send the header then the data //Returns false if sensor does not ACK bool BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength) { - // start the transaction and contact the IMU - _i2cPort.start(); + // start the transaction and contact the IMU + _i2cPort.start(); - // to indicate an i2c read, shift the 7 bit address up 1 bit and keep bit 0 as a 0 - int writeResult = _i2cPort.write(_i2cAddress << 1); + // to indicate an i2c read, shift the 7 bit address up 1 bit and keep bit 0 as a 0 + int writeResult = _i2cPort.write(_i2cAddress << 1); - if(writeResult != 1) - { - _debugPort->printf("BNO I2C write failed!\n"); - _i2cPort.stop(); - return false; - } + if(writeResult != 1) { + _debugPort->printf("BNO I2C write failed!\n"); + _i2cPort.stop(); + return false; + } - uint16_t totalLength = dataLength + 4; //Add four bytes for the header - packetLength = dataLength; + uint16_t totalLength = dataLength + 4; //Add four bytes for the header + packetLength = dataLength; #if BNO_DEBUG - shtpHeader[0] = totalLength & 0xFF; - shtpHeader[1] = totalLength >> 8; - shtpHeader[2] = channelNumber; - shtpHeader[3] = sequenceNumber[channelNumber]; - - _debugPort->printf("Transmitting packet: ----------------\n"); - printPacket(); -#endif + shtpHeader[0] = totalLength & 0xFF; + shtpHeader[1] = totalLength >> 8; + shtpHeader[2] = channelNumber; + shtpHeader[3] = sequenceNumber[channelNumber]; - //Send the 4 byte packet header - _i2cPort.write(totalLength & 0xFF); //Packet length LSB - _i2cPort.write(totalLength >> 8); //Packet length MSB - _i2cPort.write(channelNumber); //Channel number - _i2cPort.write(sequenceNumber[channelNumber]++); //Send the sequence number, increments with each packet sent, different counter for each channel + _debugPort->printf("Transmitting packet: ----------------\n"); + printPacket(); +#endif + + //Send the 4 byte packet header + _i2cPort.write(totalLength & 0xFF); //Packet length LSB + _i2cPort.write(totalLength >> 8); //Packet length MSB + _i2cPort.write(channelNumber); //Channel number + _i2cPort.write(sequenceNumber[channelNumber]++); //Send the sequence number, increments with each packet sent, different counter for each channel + /* + char data_write[4]; + data_write[0] = totalLength & 0xFF; + data_write[1] = totalLength >> 8; + data_write[2] = channelNumber; + data_write[3] = sequenceNumber[channelNumber]++; + _i2cPort.write(_i2cAddress << 1, data_write, 4); + */ + //Send the user's data packet + for (uint8_t i = 0 ; i < dataLength ; i++) { + _i2cPort.write(shtpData[i]); + } + _i2cPort.stop(); - //Send the user's data packet - for (uint8_t i = 0 ; i < dataLength ; i++) - { - _i2cPort.write(shtpData[i]); - } - _i2cPort.stop(); - - return (true); + return (true); } //Check to see if there is any new data available //Read the contents of the incoming packet into the shtpData array bool BNO080::receivePacket(float timeout) { - Timer waitStartTime; - waitStartTime.start(); - _debugPort->printf("::receivePacket -> timer started and entering while after _int.read() \r\n"); - while(_int.read() != 0) - { - _debugPort->printf("::receivePacket -> _int.read was non-zero \r\n"); - if(waitStartTime.read() > timeout) - { - _debugPort->printf("BNO I2C wait timeout\r\n"); - return false; - } + Timer waitStartTime; + waitStartTime.start(); - } + while(_int.read() != 0) { + if(waitStartTime.read() > timeout) { + _debugPort->printf("BNO I2C wait timeout\n"); + return false; + } + } - // start the transaction and contact the IMU - _i2cPort.start(); + // start the transaction and contact the IMU + _i2cPort.start(); + + // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1 + int writeResult = _i2cPort.write((_i2cAddress << 1) | 0x1); - // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1 - int writeResult = _i2cPort.write((_i2cAddress << 1) | 0x1); - _debugPort->printf("::recievePacket -> i2cPort started and writeResult = %d \r\n", writeResult); - if(writeResult != 1) - { - _debugPort->printf("BNO I2C read failed!\n"); - return false; - } + if(writeResult != 1) { + _debugPort->printf("BNO I2C read failed!\n"); + return false; + } - //Get the first four bytes, aka the packet header - uint8_t packetLSB = static_cast<uint8_t>(_i2cPort.read(true)); - uint8_t packetMSB = static_cast<uint8_t>(_i2cPort.read(true)); - uint8_t channelNumber = static_cast<uint8_t>(_i2cPort.read(true)); - uint8_t sequenceNum = static_cast<uint8_t>(_i2cPort.read(true)); //Not sure if we need to store this or not + //Get the first four bytes, aka the packet header + uint8_t packetLSB = static_cast<uint8_t>(_i2cPort.read(true)); + uint8_t packetMSB = static_cast<uint8_t>(_i2cPort.read(true)); + uint8_t channelNumber = static_cast<uint8_t>(_i2cPort.read(true)); + uint8_t sequenceNum = static_cast<uint8_t>(_i2cPort.read(true)); //Not sure if we need to store this or not - //Store the header info - shtpHeader[0] = packetLSB; - shtpHeader[1] = packetMSB; - shtpHeader[2] = channelNumber; - shtpHeader[3] = sequenceNum; - //---------------------------added code for debugging ------------------------------------------ - for(int i =0; i<4; i++) - _debugPort->printf("::receivPacket -> shtpHeader[%d] = %u \r\n", i, shtpHeader[i]); - - if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF) - { - // invalid according to BNO080 datasheet section 1.4.1 + //Store the header info + shtpHeader[0] = packetLSB; + shtpHeader[1] = packetMSB; + shtpHeader[2] = channelNumber; + shtpHeader[3] = sequenceNum; + + if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF) { + // invalid according to BNO080 datasheet section 1.4.1 #if BNO_DEBUG - _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n"); + _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n"); #endif - return false; - } + _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n"); + return false; + } - //Calculate the number of data bytes in this packet - packetLength = (static_cast<uint16_t>(packetMSB) << 8 | packetLSB); + //Calculate the number of data bytes in this packet + packetLength = (static_cast<uint16_t>(packetMSB) << 8 | packetLSB); - // Clear the MSbit. - // 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) - // but we don't actually care about any of the advertisement packets - // that use this, so we can just cut off the rest of the packet by releasing chip select. - packetLength &= ~(1 << 15); + // Clear the MSbit. + // 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) + // but we don't actually care about any of the advertisement packets + // that use this, so we can just cut off the rest of the packet by releasing chip select. + packetLength &= ~(1 << 15); - if (packetLength == 0) - { - // Packet is empty - return (false); //All done - } + if (packetLength == 0) { + // Packet is empty + return (false); //All done + } - packetLength -= 4; //Remove the header bytes from the data count + packetLength -= 4; //Remove the header bytes from the data count - //Read incoming data into the shtpData array - for (uint16_t dataSpot = 0 ; dataSpot < packetLength ; dataSpot++) - { - bool sendACK = dataSpot < packetLength - 1; + //Read incoming data into the shtpData array + for (uint16_t dataSpot = 0 ; dataSpot < packetLength ; dataSpot++) { + bool sendACK = dataSpot < packetLength - 1; - // per the datasheet, 0xFF is used as filler for the receiver to transmit back - uint8_t incoming = static_cast<uint8_t>(_i2cPort.read(sendACK)); - if (dataSpot < STORED_PACKET_SIZE) //BNO080 can respond with upto 270 bytes, avoid overflow - shtpData[dataSpot] = incoming; //Store data into the shtpData array - } - _debugPort->printf("in between \r\n"); - _i2cPort.stop(); + // per the datasheet, 0xFF is used as filler for the receiver to transmit back + uint8_t incoming = static_cast<uint8_t>(_i2cPort.read(sendACK)); + if (dataSpot < STORED_PACKET_SIZE) //BNO080 can respond with upto 270 bytes, avoid overflow + shtpData[dataSpot] = incoming; //Store data into the shtpData array + } + + _i2cPort.stop(); #if BNO_DEBUG - _debugPort->printf("Recieved packet: ----------------\n"); - printPacket(); // note: add 4 for the header length + _debugPort->printf("Recieved packet: ----------------\n"); + printPacket(); // note: add 4 for the header length #endif - - return (true); //We're done! + //_debugPort->printf("\r\t\t\t We're done!\r\n"); + return (true); //We're done! } //Pretty prints the contents of the current shtp header and data packets void BNO080::printPacket() { #if BNO_DEBUG - //Print the four byte header - _debugPort->printf("Header:"); - for (uint8_t x = 0 ; x < 4 ; x++) - { - _debugPort->printf(" "); - if (shtpHeader[x] < 0x10) _debugPort->printf("0"); - _debugPort->printf("%hhx", shtpHeader[x]); - } + //Print the four byte header + _debugPort->printf("Header:"); + for (uint8_t x = 0 ; x < 4 ; x++) { + _debugPort->printf(" "); + if (shtpHeader[x] < 0x10) _debugPort->printf("0"); + _debugPort->printf("%hhx", shtpHeader[x]); + } - uint16_t printLength = packetLength; - if (printLength > 40) printLength = 40; //Artificial limit. We don't want the phone book. + uint16_t printLength = packetLength; + if (printLength > 40) printLength = 40; //Artificial limit. We don't want the phone book. - _debugPort->printf(" Body:"); - for (uint16_t x = 0 ; x < printLength ; x++) - { - _debugPort->printf(" "); - if (shtpData[x] < 0x10) _debugPort->printf("0"); - _debugPort->printf("%hhx", shtpData[x]); - } + _debugPort->printf(" Body:"); + for (uint16_t x = 0 ; x < printLength ; x++) { + _debugPort->printf(" "); + if (shtpData[x] < 0x10) _debugPort->printf("0"); + _debugPort->printf("%hhx", shtpData[x]); + } - _debugPort->printf(", Length:"); - _debugPort->printf("%hhu", packetLength + SHTP_HEADER_SIZE); + _debugPort->printf(", Length:"); + _debugPort->printf("%hhu", packetLength + SHTP_HEADER_SIZE); - if(shtpHeader[1] >> 7) - { - _debugPort->printf("[C]"); - } + if(shtpHeader[1] >> 7) { + _debugPort->printf("[C]"); + } - _debugPort->printf(", SeqNum: %hhu", shtpHeader[3]); + _debugPort->printf(", SeqNum: %hhu", shtpHeader[3]); - _debugPort->printf(", Channel:"); - if (shtpHeader[2] == 0) _debugPort->printf("Command"); - else if (shtpHeader[2] == 1) _debugPort->printf("Executable"); - else if (shtpHeader[2] == 2) _debugPort->printf("Control"); - else if (shtpHeader[2] == 3) _debugPort->printf("Sensor-report"); - else if (shtpHeader[2] == 4) _debugPort->printf("Wake-report"); - else if (shtpHeader[2] == 5) _debugPort->printf("Gyro-vector"); - else _debugPort->printf("%hhu", shtpHeader[2]); + _debugPort->printf(", Channel:"); + if (shtpHeader[2] == 0) _debugPort->printf("Command"); + else if (shtpHeader[2] == 1) _debugPort->printf("Executable"); + else if (shtpHeader[2] == 2) _debugPort->printf("Control"); + else if (shtpHeader[2] == 3) _debugPort->printf("Sensor-report"); + else if (shtpHeader[2] == 4) _debugPort->printf("Wake-report"); + else if (shtpHeader[2] == 5) _debugPort->printf("Gyro-vector"); + else _debugPort->printf("%hhu", shtpHeader[2]); - _debugPort->printf("\n"); + _debugPort->printf("\n"); #endif } void BNO080::zeroBuffer() { - memset(shtpHeader, 0, SHTP_HEADER_SIZE); - memset(shtpData, 0, STORED_PACKET_SIZE); - packetLength = 0; + memset(shtpHeader, 0, SHTP_HEADER_SIZE); + memset(shtpData, 0, STORED_PACKET_SIZE); + packetLength = 0; } bool BNO080::loadReportMetadata(BNO080::Report report) { - uint16_t reportMetaRecord = 0; + uint16_t reportMetaRecord; - // first, convert the report into the correct FRS record ID for that report's metadata - // data from SH-2 section 5.1 - switch(report) - { - case TOTAL_ACCELERATION: - reportMetaRecord = 0xE301; - break; - case LINEAR_ACCELERATION: - reportMetaRecord = 0xE303; - break; - case GRAVITY_ACCELERATION: - reportMetaRecord = 0xE304; - break; - case GYROSCOPE: - reportMetaRecord = 0xE306; - break; - case MAG_FIELD: - reportMetaRecord = 0xE309; - break; - case MAG_FIELD_UNCALIBRATED: - reportMetaRecord = 0xE30A; - break; - case ROTATION: - reportMetaRecord = 0xE30B; - break; - case GEOMAGNETIC_ROTATION: - reportMetaRecord = 0xE30D; - break; - case GAME_ROTATION: - reportMetaRecord = 0xE30C; - break; - case TAP_DETECTOR: - reportMetaRecord = 0xE313; - break; - case STABILITY_CLASSIFIER: - reportMetaRecord = 0xE317; - break; - case STEP_DETECTOR: - reportMetaRecord = 0xE314; - break; - case STEP_COUNTER: - reportMetaRecord = 0xE315; - break; - case SIGNIFICANT_MOTION: - reportMetaRecord = 0xE316; - break; - case SHAKE_DETECTOR: - reportMetaRecord = 0xE318; - break; - } + // first, convert the report into the correct FRS record ID for that report's metadata + // data from SH-2 section 5.1 + switch(report) { + case TOTAL_ACCELERATION: + reportMetaRecord = 0xE301; + break; + case LINEAR_ACCELERATION: + reportMetaRecord = 0xE303; + break; + case GRAVITY_ACCELERATION: + reportMetaRecord = 0xE304; + break; + case GYROSCOPE: + reportMetaRecord = 0xE306; + break; + case MAG_FIELD: + reportMetaRecord = 0xE309; + break; + case MAG_FIELD_UNCALIBRATED: + reportMetaRecord = 0xE30A; + break; + case ROTATION: + reportMetaRecord = 0xE30B; + break; + case GEOMAGNETIC_ROTATION: + reportMetaRecord = 0xE30D; + break; + case GAME_ROTATION: + reportMetaRecord = 0xE30C; + break; + case TAP_DETECTOR: + reportMetaRecord = 0xE313; + break; + case STABILITY_CLASSIFIER: + reportMetaRecord = 0xE317; + break; + case STEP_DETECTOR: + reportMetaRecord = 0xE314; + break; + case STEP_COUNTER: + reportMetaRecord = 0xE315; + break; + case SIGNIFICANT_MOTION: + reportMetaRecord = 0xE316; + break; + case SHAKE_DETECTOR: + reportMetaRecord = 0xE318; + break; + } - // if we already have that data stored, everything's OK - if(bufferMetadataRecord == reportMetaRecord) - { - return true; - } + // if we already have that data stored, everything's OK + if(bufferMetadataRecord == reportMetaRecord) { + return true; + } - // now, load the metadata into the buffer - if(!readFRSRecord(reportMetaRecord, metadataRecord, METADATA_BUFFER_LEN)) - { - // clear this so future calls won't try to use the cached version - bufferMetadataRecord = 0; + // now, load the metadata into the buffer + if(!readFRSRecord(reportMetaRecord, metadataRecord, METADATA_BUFFER_LEN)) { + // clear this so future calls won't try to use the cached version + bufferMetadataRecord = 0; - return false; - } + return false; + } - bufferMetadataRecord = reportMetaRecord; + bufferMetadataRecord = reportMetaRecord; - return true; + return true; }