From b1ab503c89bbf7f0681277d603102cc7b346c17a Mon Sep 17 00:00:00 2001 From: bastienboudet Date: Thu, 3 Feb 2022 10:21:49 +0100 Subject: [PATCH 1/2] addition of uncalibrated gyroscope reports Add the uncalibrated gyroscope report with the full set of related functions --- src/SparkFun_BNO080_Arduino_Library.cpp | 81 ++++++++++++++++++++++++- src/SparkFun_BNO080_Arduino_Library.h | 13 ++++ 2 files changed, 93 insertions(+), 1 deletion(-) diff --git a/src/SparkFun_BNO080_Arduino_Library.cpp b/src/SparkFun_BNO080_Arduino_Library.cpp index 1ebdfe7..a6c510c 100644 --- a/src/SparkFun_BNO080_Arduino_Library.cpp +++ b/src/SparkFun_BNO080_Arduino_Library.cpp @@ -291,6 +291,7 @@ uint16_t BNO080::parseInputReport(void) uint16_t data3 = (uint16_t)shtpData[5 + 9] << 8 | shtpData[5 + 8]; uint16_t data4 = 0; uint16_t data5 = 0; //We would need to change this to uin32_t to capture time stamp value on Raw Accel/Gyro/Mag reports + uint16_t data6 = 0; if (dataLength - 5 > 9) { @@ -300,6 +301,11 @@ uint16_t BNO080::parseInputReport(void) { data5 = (uint16_t)shtpData[5 + 13] << 8 | shtpData[5 + 12]; } + if (dataLength - 5 > 13) + { + data6 = (uint16_t)shtpData[5 + 15] << 8 | shtpData[5 + 14]; + } + //Store these generic values to their proper global variable if (shtpData[5] == SENSOR_REPORTID_ACCELEROMETER) @@ -317,12 +323,22 @@ uint16_t BNO080::parseInputReport(void) rawLinAccelZ = data3; } else if (shtpData[5] == SENSOR_REPORTID_GYROSCOPE) - { + { gyroAccuracy = status; rawGyroX = data1; rawGyroY = data2; rawGyroZ = data3; } + else if (shtpData[5] == SENSOR_REPORTID_UNCALIBRATED_GYRO) + { + UncalibGyroAccuracy = status; + rawUncalibGyroX = data1; + rawUncalibGyroY = data2; + rawUncalibGyroZ = data3; + rawBiasX = data4; + rawBiasY = data5; + rawBiasZ = data6; + } else if (shtpData[5] == SENSOR_REPORTID_MAGNETIC_FIELD) { magAccuracy = status; @@ -690,6 +706,62 @@ uint8_t BNO080::getGyroAccuracy() return (gyroAccuracy); } +//Gets the full uncalibrated gyro vector +//x,y,z,bx,by,bz output floats +void BNO080::getUncalibratedGyro(float &x, float &y, float &z, float &bx, float &by, float &bz, uint8_t &accuracy) +{ + x = qToFloat(rawUncalibGyroX, gyro_Q1); + y = qToFloat(rawUncalibGyroY, gyro_Q1); + z = qToFloat(rawUncalibGyroZ, gyro_Q1); + bx = qToFloat(rawBiasX, gyro_Q1); + by = qToFloat(rawBiasY, gyro_Q1); + bz = qToFloat(rawBiasZ, gyro_Q1); + accuracy = UncalibGyroAccuracy; +} +//Return the gyro component +float BNO080::getUncalibratedGyroX() +{ + float gyro = qToFloat(rawUncalibGyroX, gyro_Q1); + return (gyro); +} +//Return the gyro component +float BNO080::getUncalibratedGyroY() +{ + float gyro = qToFloat(rawUncalibGyroY, gyro_Q1); + return (gyro); +} +//Return the gyro component +float BNO080::getUncalibratedGyroZ() +{ + float gyro = qToFloat(rawUncalibGyroZ, gyro_Q1); + return (gyro); +} +//Return the gyro component +float BNO080::getUncalibratedGyroBiasX() +{ + float gyro = qToFloat(rawBiasX, gyro_Q1); + return (gyro); +} +//Return the gyro component +float BNO080::getUncalibratedGyroBiasY() +{ + float gyro = qToFloat(rawBiasY, gyro_Q1); + return (gyro); +} +//Return the gyro component +float BNO080::getUncalibratedGyroBiasZ() +{ + float gyro = qToFloat(rawBiasZ, gyro_Q1); + return (gyro); +} + +//Return the gyro component +uint8_t BNO080::getUncalibratedGyroAccuracy() +{ + return (UncalibGyroAccuracy); +} + + //Gets the full mag vector //x,y,z output floats void BNO080::getMag(float &x, float &y, float &z, uint8_t &accuracy) @@ -1071,6 +1143,7 @@ uint8_t BNO080::resetReason() //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); @@ -1118,6 +1191,12 @@ void BNO080::enableGyro(uint16_t timeBetweenReports) setFeatureCommand(SENSOR_REPORTID_GYROSCOPE, timeBetweenReports); } +//Sends the packet to enable the uncalibrated gyro +void BNO080::enableUncalibratedGyro(uint16_t timeBetweenReports) +{ + setFeatureCommand(SENSOR_REPORTID_UNCALIBRATED_GYRO, timeBetweenReports); +} + //Sends the packet to enable the magnetometer void BNO080::enableMagnetometer(uint16_t timeBetweenReports) { diff --git a/src/SparkFun_BNO080_Arduino_Library.h b/src/SparkFun_BNO080_Arduino_Library.h index 0b1c811..c24811e 100644 --- a/src/SparkFun_BNO080_Arduino_Library.h +++ b/src/SparkFun_BNO080_Arduino_Library.h @@ -80,6 +80,7 @@ const byte CHANNEL_GYRO = 5; #define SENSOR_REPORTID_LINEAR_ACCELERATION 0x04 #define SENSOR_REPORTID_ROTATION_VECTOR 0x05 #define SENSOR_REPORTID_GRAVITY 0x06 +#define SENSOR_REPORTID_UNCALIBRATED_GYRO 0x07 #define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08 #define SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR 0x09 #define SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR 0x2A @@ -156,6 +157,7 @@ class BNO080 void enableAccelerometer(uint16_t timeBetweenReports); void enableLinearAccelerometer(uint16_t timeBetweenReports); void enableGyro(uint16_t timeBetweenReports); + void enableUncalibratedGyro(uint16_t timeBetweenReports); void enableMagnetometer(uint16_t timeBetweenReports); void enableTapDetector(uint16_t timeBetweenReports); void enableStepCounter(uint16_t timeBetweenReports); @@ -197,6 +199,16 @@ class BNO080 float getGyroZ(); uint8_t getGyroAccuracy(); + void getUncalibratedGyro(float &x, float &y, float &z, float &bx, float &by, float &bz, uint8_t &accuracy); + float getUncalibratedGyroX(); + float getUncalibratedGyroY(); + float getUncalibratedGyroZ(); + float getUncalibratedGyroBiasX(); + float getUncalibratedGyroBiasY(); + float getUncalibratedGyroBiasZ(); + uint8_t getUncalibratedGyroAccuracy(); + + void getFastGyro(float &x, float &y, float &z); float getFastGyroX(); float getFastGyroY(); @@ -283,6 +295,7 @@ class BNO080 uint16_t rawAccelX, rawAccelY, rawAccelZ, accelAccuracy; uint16_t rawLinAccelX, rawLinAccelY, rawLinAccelZ, accelLinAccuracy; uint16_t rawGyroX, rawGyroY, rawGyroZ, gyroAccuracy; + uint16_t rawUncalibGyroX, rawUncalibGyroY, rawUncalibGyroZ, rawBiasX, rawBiasY, rawBiasZ, UncalibGyroAccuracy; uint16_t rawMagX, rawMagY, rawMagZ, magAccuracy; uint16_t rawQuatI, rawQuatJ, rawQuatK, rawQuatReal, rawQuatRadianAccuracy, quatAccuracy; uint16_t rawFastGyroX, rawFastGyroY, rawFastGyroZ; From b4b3bacf842901f4978616efa10b1fd5ced97ad8 Mon Sep 17 00:00:00 2001 From: bastienboudet Date: Thu, 3 Feb 2022 10:52:23 +0100 Subject: [PATCH 2/2] update keyword + add example addition of uncalibrated gyro functions --- .../Example3-Gyro.ino | 74 +++++++++++++++++++ keywords.txt | 11 +++ 2 files changed, 85 insertions(+) create mode 100644 examples/Example22-UncalibratedGyro/Example3-Gyro.ino diff --git a/examples/Example22-UncalibratedGyro/Example3-Gyro.ino b/examples/Example22-UncalibratedGyro/Example3-Gyro.ino new file mode 100644 index 0000000..9dd27f2 --- /dev/null +++ b/examples/Example22-UncalibratedGyro/Example3-Gyro.ino @@ -0,0 +1,74 @@ +/* + Using the BNO080 IMU + By: Bastien Boudet + Date: February 3rd, 2022 + SparkFun code, firmware, and software is released under the MIT License. + Please see LICENSE.md for further details. + + Feel like supporting our work? Buy a board from SparkFun! + https://www.sparkfun.com/products/14586 + + This example shows how to output the parts of the uncalibrated gyro. + + 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 115200 baud to serial monitor. +*/ + +#include + +#include "SparkFun_BNO080_Arduino_Library.h" // Click here to get the library: http://librarymanager/All#SparkFun_BNO080 +BNO080 myIMU; + +void setup() +{ + Serial.begin(115200); + Serial.println(); + Serial.println("BNO080 Read Example"); + + Wire.begin(); + + myIMU.begin(); + + Wire.setClock(400000); //Increase I2C data rate to 400kHz + + myIMU.enableUncalibratedGyro(50); //Send data update every 50ms + + Serial.println(F("Uncalibrated Gyro enabled")); + Serial.println(F("Output in form x, y, z, bx, by, bz in radians per second")); +} + +void loop() +{ + //Look for reports from the IMU + if (myIMU.dataAvailable() == true) + { + float x = myIMU.getUncalibratedGyroX(); + float y = myIMU.getUncalibratedGyroY(); + float z = myIMU.getUncalibratedGyroZ(); + float bx = myIMU.getUncalibratedGyroBiasX(); + float by = myIMU.getUncalibratedGyroBiasY(); + float bz = myIMU.getUncalibratedGyroBiasZ(); + + + Serial.print(x, 2); + Serial.print(F(",")); + Serial.print(y, 2); + Serial.print(F(",")); + Serial.print(z, 2); + Serial.print(F(",")); + + Serial.print(bx, 2); + Serial.print(F(",")); + Serial.print(by, 2); + Serial.print(F(",")); + Serial.print(bz, 2); + Serial.print(F(",")); + + Serial.println(); + } +} diff --git a/keywords.txt b/keywords.txt index 318ed33..9ca6541 100644 --- a/keywords.txt +++ b/keywords.txt @@ -39,6 +39,7 @@ enableARVRStabilizedRotationVector KEYWORD2 enableARVRStabilizedGameRotationVector KEYWORD2 enableAccelerometer KEYWORD2 enableGyro KEYWORD2 +enableUncalibratedGyro KEYWORD2 enableMagnetometer KEYWORD2 enableTapDetector KEYWORD2 enableStepCounter KEYWORD2 @@ -75,6 +76,16 @@ getGyroY KEYWORD2 getGyroZ KEYWORD2 getGyroAccuracy KEYWORD2 +getUncalibratedGyro KEYWORD2 +getUncalibratedGyroX KEYWORD2 +getUncalibratedGyroY KEYWORD2 +getUncalibratedGyroZ KEYWORD2 +getUncalibratedGyroAccuracy KEYWORD2 +getUncalibratedGyroBiasX KEYWORD2 +getUncalibratedGyroBiasY KEYWORD2 +getUncalibratedGyroBiasZ KEYWORD2 + + getFastGyro KEYWORD2 getFastGyroX KEYWORD2 getFastGyroY KEYWORD2