diff --git a/examples/Example16-GyroIntegratedRotationVector/Example16-GyroIntegratedRotationVector.ino b/examples/Example16-GyroIntegratedRotationVector/Example16-GyroIntegratedRotationVector.ino new file mode 100644 index 0000000..589cf32 --- /dev/null +++ b/examples/Example16-GyroIntegratedRotationVector/Example16-GyroIntegratedRotationVector.ino @@ -0,0 +1,90 @@ +/* + Using the BNO080 IMU + By: Nathan Seidle + SparkFun Electronics + Date: December 21st, 2017 + License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license). + + Feel like supporting our work? Buy a board from SparkFun! + https://www.sparkfun.com/products/14586 + + This example shows how to output the i/j/k/real parts of the rotation vector. + https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation + + It takes about 1ms at 400kHz I2C to read a record from the sensor, but we are polling the sensor continually + between updates from the sensor. Use the interrupt pin on the BNO080 breakout to avoid polling. + + Hardware Connections: + Attach the Qwiic Shield to your Arduino/Photon/ESP32 or other + Plug the sensor onto the shield + Serial.print it out at 9600 baud to serial monitor. +*/ + +#include + +#include "SparkFun_BNO080_Arduino_Library.h" +BNO080 myIMU; + +void setup() +{ + Serial.begin(9600); + Serial.println(); + Serial.println("BNO080 Read Example"); + + Wire.begin(); + + //Are you using a ESP? Check this issue for more information: https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library/issues/16 +// //================================= +// delay(100); // Wait for BNO to boot +// // Start i2c and BNO080 +// Wire.flush(); // Reset I2C +// IMU.begin(BNO080_DEFAULT_ADDRESS, Wire); +// Wire.begin(4, 5); +// Wire.setClockStretchLimit(4000); +// //================================= + + if (myIMU.begin() == false) + { + Serial.println("BNO080 not detected at default I2C address. Check your jumpers and the hookup guide. Freezing..."); + while (1); + } + + Wire.setClock(400000); //Increase I2C data rate to 400kHz + + myIMU.enableGyroIntegratedRotationVector(50); //Send data update every 50ms + + Serial.println(F("Gyro integrated rotation vector enabled")); + Serial.println(F("Output in form i, j, k, real, gyroX, gyroY, gyroZ")); +} + +void loop() +{ + //Look for reports from the IMU + if (myIMU.dataAvailable() == true) + { + float quatI = myIMU.getQuatI(); + float quatJ = myIMU.getQuatJ(); + float quatK = myIMU.getQuatK(); + float quatReal = myIMU.getQuatReal(); + float gyroX = myIMU.getFastGyroX(); + float gyroY = myIMU.getFastGyroY(); + float gyroZ = myIMU.getFastGyroZ(); + + + Serial.print(quatI, 2); + Serial.print(F(",")); + Serial.print(quatJ, 2); + Serial.print(F(",")); + Serial.print(quatK, 2); + Serial.print(F(",")); + Serial.print(quatReal, 2); + Serial.print(F(",")); + Serial.print(gyroX, 2); + Serial.print(F(",")); + Serial.print(gyroY, 2); + Serial.print(F(",")); + Serial.print(gyroZ, 2); + + Serial.println(); + } +} diff --git a/keywords.txt b/keywords.txt index 17b52d8..94e125b 100644 --- a/keywords.txt +++ b/keywords.txt @@ -41,6 +41,7 @@ enableLinearAccelerometer KEYWORD2 enableRawAccelerometer KEYWORD2 enableRawGyro KEYWORD2 enableRawMagnetometer KEYWORD2 +enableGyroIntegratedRotationVector KEYWORD2 dataAvailable KEYWORD2 parseInputReport KEYWORD2 @@ -63,6 +64,11 @@ getGyroY KEYWORD2 getGyroZ KEYWORD2 getGyroAccuracy KEYWORD2 +getFastGyroX KEYWORD2 +getFastGyroY KEYWORD2 +getFastGyroZ KEYWORD2 + + getMagX KEYWORD2 getMagY KEYWORD2 getMagZ KEYWORD2 diff --git a/src/SparkFun_BNO080_Arduino_Library.cpp b/src/SparkFun_BNO080_Arduino_Library.cpp index c706996..3d34a1f 100644 --- a/src/SparkFun_BNO080_Arduino_Library.cpp +++ b/src/SparkFun_BNO080_Arduino_Library.cpp @@ -149,7 +149,7 @@ bool BNO080::dataAvailable(void) if (digitalRead(_int) == HIGH) return (false); } - + if (receivePacket() == true) { //Check to see if this packet is a sensor reporting its data to us @@ -163,6 +163,11 @@ bool BNO080::dataAvailable(void) parseCommandReport(); //This will update responses to commands, calibrationStatus, etc. return (true); } + else if(shtpHeader[2] == CHANNEL_GYRO) + { + parseInputReport(); //This will update the rawAccelX, etc variables depending on which feature report is found + return (true); + } } return (false); } @@ -227,11 +232,24 @@ void BNO080::parseInputReport(void) int16_t dataLength = ((uint16_t)shtpHeader[1] << 8 | shtpHeader[0]); dataLength &= ~(1 << 15); //Clear the MSbit. This bit indicates if this package is a continuation of the last. //Ignore it for now. TODO catch this as an error and exit - + dataLength -= 4; //Remove the header bytes from the data count timeStamp = ((uint32_t)shtpData[4] << (8 * 3)) | ((uint32_t)shtpData[3] << (8 * 2)) | ((uint32_t)shtpData[2] << (8 * 1)) | ((uint32_t)shtpData[1] << (8 * 0)); + // The gyro-integrated input reports are sent via the special gyro channel and do no include the usual ID, sequence, and status fields + if(shtpHeader[2] == CHANNEL_GYRO) { + rawQuatI = (uint16_t)shtpData[1] << 8 | shtpData[0]; + rawQuatJ = (uint16_t)shtpData[3] << 8 | shtpData[2]; + rawQuatK = (uint16_t)shtpData[5] << 8 | shtpData[4]; + rawQuatReal = (uint16_t)shtpData[7] << 8 | shtpData[6]; + rawFastGyroX = (uint16_t)shtpData[9] << 8 | shtpData[8]; + rawFastGyroY = (uint16_t)shtpData[11] << 8 | shtpData[10]; + rawFastGyroZ = (uint16_t)shtpData[13] << 8 | shtpData[12]; + + return; + } + uint8_t status = shtpData[5 + 2] & 0x03; //Get status bits uint16_t data1 = (uint16_t)shtpData[5 + 5] << 8 | shtpData[5 + 4]; uint16_t data2 = (uint16_t)shtpData[5 + 7] << 8 | shtpData[5 + 6]; @@ -492,6 +510,27 @@ uint8_t BNO080::getMagAccuracy() return (magAccuracy); } +// Return the high refresh rate gyro component +float BNO080::getFastGyroX() +{ + float gyro = qToFloat(rawFastGyroX, angular_velocity_Q1); + return (gyro); +} + +// Return the high refresh rate gyro component +float BNO080::getFastGyroY() +{ + float gyro = qToFloat(rawFastGyroY, angular_velocity_Q1); + return (gyro); +} + +// Return the high refresh rate gyro component +float BNO080::getFastGyroZ() +{ + float gyro = qToFloat(rawFastGyroZ, angular_velocity_Q1); + return (gyro); +} + //Return the step count uint16_t BNO080::getStepCount() { @@ -793,6 +832,12 @@ void BNO080::enableMagnetometer(uint16_t timeBetweenReports) setFeatureCommand(SENSOR_REPORTID_MAGNETIC_FIELD, timeBetweenReports); } +//Sends the packet to enable the high refresh-rate gyro-integrated rotation vector +void BNO080::enableGyroIntegratedRotationVector(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR, timeBetweenReports); +} + //Sends the packet to enable the step counter void BNO080::enableStepCounter(uint16_t timeBetweenReports) { @@ -1077,13 +1122,13 @@ boolean BNO080::receivePacket(void) uint8_t packetMSB = _spiPort->transfer(0); uint8_t channelNumber = _spiPort->transfer(0); uint8_t sequenceNumber = _spiPort->transfer(0); //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] = sequenceNumber; - + //Calculate the number of data bytes in this packet uint16_t dataLength = ((uint16_t)packetMSB << 8 | packetLSB); dataLength &= ~(1 << 15); //Clear the MSbit. @@ -1105,7 +1150,9 @@ boolean BNO080::receivePacket(void) } digitalWrite(_cs, HIGH); //Release BNO080 + _spiPort->endTransaction(); + //printPacket(); } else //Do I2C { diff --git a/src/SparkFun_BNO080_Arduino_Library.h b/src/SparkFun_BNO080_Arduino_Library.h index 5627532..9bb07c0 100644 --- a/src/SparkFun_BNO080_Arduino_Library.h +++ b/src/SparkFun_BNO080_Arduino_Library.h @@ -87,6 +87,7 @@ const byte CHANNEL_GYRO = 5; #define SENSOR_REPORTID_GRAVITY 0x06 #define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08 #define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09 +#define SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR 0x2A #define SENSOR_REPORTID_TAP_DETECTOR 0x10 #define SENSOR_REPORTID_STEP_COUNTER 0x11 #define SENSOR_REPORTID_STABILITY_CLASSIFIER 0x13 @@ -156,6 +157,7 @@ class BNO080 void enableRawAccelerometer(uint16_t timeBetweenReports); void enableRawGyro(uint16_t timeBetweenReports); void enableRawMagnetometer(uint16_t timeBetweenReports); + void enableGyroIntegratedRotationVector(uint16_t timeBetweenReports); bool dataAvailable(void); void parseInputReport(void); //Parse sensor readings out of report @@ -183,6 +185,10 @@ class BNO080 float getGyroZ(); uint8_t getGyroAccuracy(); + float getFastGyroX(); + float getFastGyroY(); + float getFastGyroZ(); + float getMagX(); float getMagY(); float getMagZ(); @@ -258,6 +264,7 @@ class BNO080 uint16_t rawGyroX, rawGyroY, rawGyroZ, gyroAccuracy; uint16_t rawMagX, rawMagY, rawMagZ, magAccuracy; uint16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy; + uint16_t rawFastGyroX, rawFastGyroY, rawFastGyroZ; uint16_t stepCount; uint32_t timeStamp; uint8_t stabilityClassifier; @@ -275,4 +282,5 @@ class BNO080 int16_t linear_accelerometer_Q1 = 8; int16_t gyro_Q1 = 9; int16_t magnetometer_Q1 = 4; + int16_t angular_velocity_Q1 = 10; };