Merge pull request #41 from badVibes--/master

Added gyro integrated rotation vector support

Author: PaulZC

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 <Wire.h>
+
+#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();
+  }
+}

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

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
 	{

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;
@@ -276,4 +283,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;
 };