diff --git a/examples/Example17-EulerAngles/Example17-EulerAngles.ino b/examples/Example17-EulerAngles/Example17-EulerAngles.ino new file mode 100644 index 0000000..deeba06 --- /dev/null +++ b/examples/Example17-EulerAngles/Example17-EulerAngles.ino @@ -0,0 +1,86 @@ +/* + Using the BNO080 IMU + + Example : Euler Angles + By: Paul Clark + Date: April 28th, 2020 + + Based on: Example1-RotationVector + 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 Euler angles: roll, pitch and yaw. + The yaw (compass heading) is tilt-compensated, which is nice. + https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/issues/5#issuecomment-306509440 + + 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" +BNO080 myIMU; + +void setup() +{ + Serial.begin(115200); + 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(F("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.enableRotationVector(50); //Send data update every 50ms + + Serial.println(F("Rotation vector enabled")); + Serial.println(F("Output in form roll, pitch, yaw")); +} + +void loop() +{ + //Look for reports from the IMU + if (myIMU.dataAvailable() == true) + { + float roll = (myIMU.getRoll()) * 180.0 / PI; // Convert roll to degrees + float pitch = (myIMU.getPitch()) * 180.0 / PI; // Convert pitch to degrees + float yaw = (myIMU.getYaw()) * 180.0 / PI; // Convert yaw / heading to degrees + + Serial.print(roll, 1); + Serial.print(F(",")); + Serial.print(pitch, 1); + Serial.print(F(",")); + Serial.print(yaw, 1); + + Serial.println(); + } +} diff --git a/examples/SPI/Example17-EulerAngles/Example17-EulerAngles.ino b/examples/SPI/Example17-EulerAngles/Example17-EulerAngles.ino new file mode 100644 index 0000000..251dbe6 --- /dev/null +++ b/examples/SPI/Example17-EulerAngles/Example17-EulerAngles.ino @@ -0,0 +1,105 @@ +/* + Using the BNO080 IMU + + Example : Euler Angles + By: Paul Clark + Date: April 28th, 2020 + + Based on: Example1-RotationVector + By: Nathan Seidle + SparkFun Electronics + Date: July 27th, 2018 + 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/14686 + + This example shows how to output the Euler angles: roll, pitch and yaw. + The yaw (compass heading) is tilt-compensated, which is nice. + https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/issues/5#issuecomment-306509440 + + This example shows how to use the SPI interface on the BNO080. It's fairly involved + and requires 7 comm wires (plus 2 power), soldering the PS1 jumper, and clearing + the I2C jumper. We recommend using the Qwiic I2C interface, but if you need speed + SPI is the way to go. + + Hardware modifications: + The PS1 jumper must be closed + The PS0 jumper must be open. PS0/WAKE is connected and the WAK pin is used to bring the IC out of sleep. + The I2C pull up jumper must be cleared/open + + Hardware Connections: + Don't hook the BNO080 to a normal 5V Uno! Either use the Qwiic system or use a + microcontroller that runs at 3.3V. + Arduino 13 = BNO080 SCK + 12 = SO + 11 = SI + 10 = !CS + 9 = WAK + 8 = !INT + 7 = !RST + 3.3V = 3V3 + GND = GND +*/ + +#include + +#include "SparkFun_BNO080_Arduino_Library.h" +BNO080 myIMU; + +//These pins can be any GPIO +byte imuCSPin = 10; +byte imuWAKPin = 9; +byte imuINTPin = 8; +byte imuRSTPin = 7; + +void setup() +{ + Serial.begin(115200); + Serial.println(); + Serial.println(F("BNO080 SPI Read Example")); + + myIMU.enableDebugging(Serial); //Pipe debug messages to Serial port + + if(myIMU.beginSPI(imuCSPin, imuWAKPin, imuINTPin, imuRSTPin) == false) + { + Serial.println(F("BNO080 over SPI not detected. Are you sure you have all 6 connections? Freezing...")); + while(1) + ; + } + + //You can also call begin with SPI clock speed and SPI port hardware + //myIMU.beginSPI(imuCSPin, imuWAKPin, imuINTPin, imuRSTPin, 1000000); + //myIMU.beginSPI(imuCSPin, imuWAKPin, imuINTPin, imuRSTPin, 1000000, SPI1); + + //The IMU is now connected over SPI + //Please see the other examples for library functions that you can call + + myIMU.enableRotationVector(50); //Send data update every 50ms + + Serial.println(F("Rotation vector enabled")); + Serial.println(F("Output in form roll, pitch, yaw")); +} + +void loop() +{ + delay(10); //You can do many other things. We spend most of our time printing and delaying. + + //Look for reports from the IMU + if (myIMU.dataAvailable() == true) + { + float roll = (myIMU.getRoll()) * 180.0 / PI; // Convert roll to degrees + float pitch = (myIMU.getPitch()) * 180.0 / PI; // Convert pitch to degrees + float yaw = (myIMU.getYaw()) * 180.0 / PI; // Convert yaw / heading to degrees + + Serial.print(roll, 1); + Serial.print(F(",")); + Serial.print(pitch, 1); + Serial.print(F(",")); + Serial.print(yaw, 1); + + Serial.println(); + } + +} diff --git a/keywords.txt b/keywords.txt index 17b52d8..06e022c 100644 --- a/keywords.txt +++ b/keywords.txt @@ -1,5 +1,5 @@ ####################################### -# Syntax Coloring Map +# Syntax Coloring Map ####################################### ####################################### @@ -115,6 +115,10 @@ getRawMagX KEYWORD2 getRawMagY KEYWORD2 getRawMagZ KEYWORD2 +getRoll KEYWORD2 +getPitch KEYWORD2 +getYaw KEYWORD2 + ####################################### # Constants (LITERAL1) diff --git a/src/SparkFun_BNO080_Arduino_Library.cpp b/src/SparkFun_BNO080_Arduino_Library.cpp index c706996..04dc99f 100644 --- a/src/SparkFun_BNO080_Arduino_Library.cpp +++ b/src/SparkFun_BNO080_Arduino_Library.cpp @@ -341,6 +341,82 @@ void BNO080::parseInputReport(void) //TODO additional feature reports may be strung together. Parse them all. } +// Quaternion to Euler conversion +// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles +// https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/issues/5#issuecomment-306509440 +// Return the roll (rotation around the x-axis) in Radians +float BNO080::getRoll() +{ + float dqw = getQuatReal(); + float dqx = getQuatI(); + float dqy = getQuatJ(); + float dqz = getQuatK(); + + float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz); + dqw = dqw/norm; + dqx = dqx/norm; + dqy = dqy/norm; + dqz = dqz/norm; + + float ysqr = dqy * dqy; + + // roll (x-axis rotation) + float t0 = +2.0 * (dqw * dqx + dqy * dqz); + float t1 = +1.0 - 2.0 * (dqx * dqx + ysqr); + float roll = atan2(t0, t1); + + return (roll); +} + +// Return the pitch (rotation around the y-axis) in Radians +float BNO080::getPitch() +{ + float dqw = getQuatReal(); + float dqx = getQuatI(); + float dqy = getQuatJ(); + float dqz = getQuatK(); + + float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz); + dqw = dqw/norm; + dqx = dqx/norm; + dqy = dqy/norm; + dqz = dqz/norm; + + float ysqr = dqy * dqy; + + // pitch (y-axis rotation) + float t2 = +2.0 * (dqw * dqy - dqz * dqx); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + float pitch = asin(t2); + + return (pitch); +} + +// Return the yaw / heading (rotation around the z-axis) in Radians +float BNO080::getYaw() +{ + float dqw = getQuatReal(); + float dqx = getQuatI(); + float dqy = getQuatJ(); + float dqz = getQuatK(); + + float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz); + dqw = dqw/norm; + dqx = dqx/norm; + dqy = dqy/norm; + dqz = dqz/norm; + + float ysqr = dqy * dqy; + + // yaw (z-axis rotation) + float t3 = +2.0 * (dqw * dqz + dqx * dqy); + float t4 = +1.0 - 2.0 * (ysqr + dqz * dqz); + float yaw = atan2(t3, t4); + + return (yaw); +} + //Return the rotation vector quaternion I float BNO080::getQuatI() { diff --git a/src/SparkFun_BNO080_Arduino_Library.h b/src/SparkFun_BNO080_Arduino_Library.h index 5627532..2de5306 100644 --- a/src/SparkFun_BNO080_Arduino_Library.h +++ b/src/SparkFun_BNO080_Arduino_Library.h @@ -215,6 +215,10 @@ class BNO080 int16_t getRawMagY(); int16_t getRawMagZ(); + float getRoll(); + float getPitch(); + float getYaw(); + void setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports); void setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig); void sendCommand(uint8_t command);