Sheila Pham
start the wrapper burrito
Diff: BNO080.cpp
- Revision:
- 6:546e4553cf6d
- Parent:
- 5:7e2cd0f351b2
- Child:
- 7:050dc348e690
--- 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;
}