Skip to content

Commit d34b4f1

Browse files
PaulZCPaulZC
authored
Merge pull request #43 from sparkfun/Euler-angles
Added the Euler Angles plus Examples
2 parents 263c274 + 1cbcd23 commit d34b4f1

File tree

5 files changed

+276
-1
lines changed

5 files changed

+276
-1
lines changed

‎examples/Example17-EulerAngles/Example17-EulerAngles.ino

Lines changed: 86 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
1+
/*
2+
Using the BNO080 IMU
3+
4+
Example : Euler Angles
5+
By: Paul Clark
6+
Date: April 28th, 2020
7+
8+
Based on: Example1-RotationVector
9+
By: Nathan Seidle
10+
SparkFun Electronics
11+
Date: December 21st, 2017
12+
License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).
13+
14+
Feel like supporting our work? Buy a board from SparkFun!
15+
https://www.sparkfun.com/products/14586
16+
17+
This example shows how to output the Euler angles: roll, pitch and yaw.
18+
The yaw (compass heading) is tilt-compensated, which is nice.
19+
https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
20+
https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/issues/5#issuecomment-306509440
21+
22+
It takes about 1ms at 400kHz I2C to read a record from the sensor, but we are polling the sensor continually
23+
between updates from the sensor. Use the interrupt pin on the BNO080 breakout to avoid polling.
24+
25+
Hardware Connections:
26+
Attach the Qwiic Shield to your Arduino/Photon/ESP32 or other
27+
Plug the sensor onto the shield
28+
Serial.print it out at 115200 baud to serial monitor.
29+
*/
30+
31+
#include <Wire.h>
32+
33+
#include "SparkFun_BNO080_Arduino_Library.h"
34+
BNO080 myIMU;
35+
36+
void setup()
37+
{
38+
Serial.begin(115200);
39+
Serial.println();
40+
Serial.println("BNO080 Read Example");
41+
42+
Wire.begin();
43+
44+
//Are you using a ESP? Check this issue for more information: https://github.com/sparkfun/SparkFun_BNO080_Arduino_Library/issues/16
45+
// //=================================
46+
// delay(100); // Wait for BNO to boot
47+
// // Start i2c and BNO080
48+
// Wire.flush(); // Reset I2C
49+
// IMU.begin(BNO080_DEFAULT_ADDRESS, Wire);
50+
// Wire.begin(4, 5);
51+
// Wire.setClockStretchLimit(4000);
52+
// //=================================
53+
54+
if (myIMU.begin() == false)
55+
{
56+
Serial.println(F("BNO080 not detected at default I2C address. Check your jumpers and the hookup guide. Freezing..."));
57+
while (1)
58+
;
59+
}
60+
61+
Wire.setClock(400000); //Increase I2C data rate to 400kHz
62+
63+
myIMU.enableRotationVector(50); //Send data update every 50ms
64+
65+
Serial.println(F("Rotation vector enabled"));
66+
Serial.println(F("Output in form roll, pitch, yaw"));
67+
}
68+
69+
void loop()
70+
{
71+
//Look for reports from the IMU
72+
if (myIMU.dataAvailable() == true)
73+
{
74+
float roll = (myIMU.getRoll()) * 180.0 / PI; // Convert roll to degrees
75+
float pitch = (myIMU.getPitch()) * 180.0 / PI; // Convert pitch to degrees
76+
float yaw = (myIMU.getYaw()) * 180.0 / PI; // Convert yaw / heading to degrees
77+
78+
Serial.print(roll, 1);
79+
Serial.print(F(","));
80+
Serial.print(pitch, 1);
81+
Serial.print(F(","));
82+
Serial.print(yaw, 1);
83+
84+
Serial.println();
85+
}
86+
}

‎examples/SPI/Example17-EulerAngles/Example17-EulerAngles.ino

Lines changed: 105 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
/*
2+
Using the BNO080 IMU
3+
4+
Example : Euler Angles
5+
By: Paul Clark
6+
Date: April 28th, 2020
7+
8+
Based on: Example1-RotationVector
9+
By: Nathan Seidle
10+
SparkFun Electronics
11+
Date: July 27th, 2018
12+
License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).
13+
14+
Feel like supporting our work? Buy a board from SparkFun!
15+
https://www.sparkfun.com/products/14686
16+
17+
This example shows how to output the Euler angles: roll, pitch and yaw.
18+
The yaw (compass heading) is tilt-compensated, which is nice.
19+
https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
20+
https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/issues/5#issuecomment-306509440
21+
22+
This example shows how to use the SPI interface on the BNO080. It's fairly involved
23+
and requires 7 comm wires (plus 2 power), soldering the PS1 jumper, and clearing
24+
the I2C jumper. We recommend using the Qwiic I2C interface, but if you need speed
25+
SPI is the way to go.
26+
27+
Hardware modifications:
28+
The PS1 jumper must be closed
29+
The PS0 jumper must be open. PS0/WAKE is connected and the WAK pin is used to bring the IC out of sleep.
30+
The I2C pull up jumper must be cleared/open
31+
32+
Hardware Connections:
33+
Don't hook the BNO080 to a normal 5V Uno! Either use the Qwiic system or use a
34+
microcontroller that runs at 3.3V.
35+
Arduino 13 = BNO080 SCK
36+
12 = SO
37+
11 = SI
38+
10 = !CS
39+
9 = WAK
40+
8 = !INT
41+
7 = !RST
42+
3.3V = 3V3
43+
GND = GND
44+
*/
45+
46+
#include <SPI.h>
47+
48+
#include "SparkFun_BNO080_Arduino_Library.h"
49+
BNO080 myIMU;
50+
51+
//These pins can be any GPIO
52+
byte imuCSPin = 10;
53+
byte imuWAKPin = 9;
54+
byte imuINTPin = 8;
55+
byte imuRSTPin = 7;
56+
57+
void setup()
58+
{
59+
Serial.begin(115200);
60+
Serial.println();
61+
Serial.println(F("BNO080 SPI Read Example"));
62+
63+
myIMU.enableDebugging(Serial); //Pipe debug messages to Serial port
64+
65+
if(myIMU.beginSPI(imuCSPin, imuWAKPin, imuINTPin, imuRSTPin) == false)
66+
{
67+
Serial.println(F("BNO080 over SPI not detected. Are you sure you have all 6 connections? Freezing..."));
68+
while(1)
69+
;
70+
}
71+
72+
//You can also call begin with SPI clock speed and SPI port hardware
73+
//myIMU.beginSPI(imuCSPin, imuWAKPin, imuINTPin, imuRSTPin, 1000000);
74+
//myIMU.beginSPI(imuCSPin, imuWAKPin, imuINTPin, imuRSTPin, 1000000, SPI1);
75+
76+
//The IMU is now connected over SPI
77+
//Please see the other examples for library functions that you can call
78+
79+
myIMU.enableRotationVector(50); //Send data update every 50ms
80+
81+
Serial.println(F("Rotation vector enabled"));
82+
Serial.println(F("Output in form roll, pitch, yaw"));
83+
}
84+
85+
void loop()
86+
{
87+
delay(10); //You can do many other things. We spend most of our time printing and delaying.
88+
89+
//Look for reports from the IMU
90+
if (myIMU.dataAvailable() == true)
91+
{
92+
float roll = (myIMU.getRoll()) * 180.0 / PI; // Convert roll to degrees
93+
float pitch = (myIMU.getPitch()) * 180.0 / PI; // Convert pitch to degrees
94+
float yaw = (myIMU.getYaw()) * 180.0 / PI; // Convert yaw / heading to degrees
95+
96+
Serial.print(roll, 1);
97+
Serial.print(F(","));
98+
Serial.print(pitch, 1);
99+
Serial.print(F(","));
100+
Serial.print(yaw, 1);
101+
102+
Serial.println();
103+
}
104+
105+
}

‎keywords.txt

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
#######################################
2-
# Syntax Coloring Map
2+
# Syntax Coloring Map
33
#######################################
44

55
#######################################
@@ -115,6 +115,10 @@ getRawMagX KEYWORD2
115115
getRawMagY KEYWORD2
116116
getRawMagZ KEYWORD2
117117

118+
getRoll KEYWORD2
119+
getPitch KEYWORD2
120+
getYaw KEYWORD2
121+
118122

119123
#######################################
120124
# Constants (LITERAL1)

‎src/SparkFun_BNO080_Arduino_Library.cpp

Lines changed: 76 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -341,6 +341,82 @@ void BNO080::parseInputReport(void)
341341
//TODO additional feature reports may be strung together. Parse them all.
342342
}
343343

344+
// Quaternion to Euler conversion
345+
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
346+
// https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/issues/5#issuecomment-306509440
347+
// Return the roll (rotation around the x-axis) in Radians
348+
float BNO080::getRoll()
349+
{
350+
float dqw = getQuatReal();
351+
float dqx = getQuatI();
352+
float dqy = getQuatJ();
353+
float dqz = getQuatK();
354+
355+
float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz);
356+
dqw = dqw/norm;
357+
dqx = dqx/norm;
358+
dqy = dqy/norm;
359+
dqz = dqz/norm;
360+
361+
float ysqr = dqy * dqy;
362+
363+
// roll (x-axis rotation)
364+
float t0 = +2.0 * (dqw * dqx + dqy * dqz);
365+
float t1 = +1.0 - 2.0 * (dqx * dqx + ysqr);
366+
float roll = atan2(t0, t1);
367+
368+
return (roll);
369+
}
370+
371+
// Return the pitch (rotation around the y-axis) in Radians
372+
float BNO080::getPitch()
373+
{
374+
float dqw = getQuatReal();
375+
float dqx = getQuatI();
376+
float dqy = getQuatJ();
377+
float dqz = getQuatK();
378+
379+
float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz);
380+
dqw = dqw/norm;
381+
dqx = dqx/norm;
382+
dqy = dqy/norm;
383+
dqz = dqz/norm;
384+
385+
float ysqr = dqy * dqy;
386+
387+
// pitch (y-axis rotation)
388+
float t2 = +2.0 * (dqw * dqy - dqz * dqx);
389+
t2 = t2 > 1.0 ? 1.0 : t2;
390+
t2 = t2 < -1.0 ? -1.0 : t2;
391+
float pitch = asin(t2);
392+
393+
return (pitch);
394+
}
395+
396+
// Return the yaw / heading (rotation around the z-axis) in Radians
397+
float BNO080::getYaw()
398+
{
399+
float dqw = getQuatReal();
400+
float dqx = getQuatI();
401+
float dqy = getQuatJ();
402+
float dqz = getQuatK();
403+
404+
float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz);
405+
dqw = dqw/norm;
406+
dqx = dqx/norm;
407+
dqy = dqy/norm;
408+
dqz = dqz/norm;
409+
410+
float ysqr = dqy * dqy;
411+
412+
// yaw (z-axis rotation)
413+
float t3 = +2.0 * (dqw * dqz + dqx * dqy);
414+
float t4 = +1.0 - 2.0 * (ysqr + dqz * dqz);
415+
float yaw = atan2(t3, t4);
416+
417+
return (yaw);
418+
}
419+
344420
//Return the rotation vector quaternion I
345421
float BNO080::getQuatI()
346422
{

‎src/SparkFun_BNO080_Arduino_Library.h

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -215,6 +215,10 @@ class BNO080
215215
int16_t getRawMagY();
216216
int16_t getRawMagZ();
217217

218+
float getRoll();
219+
float getPitch();
220+
float getYaw();
221+
218222
void setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports);
219223
void setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig);
220224
void sendCommand(uint8_t command);

0 commit comments

Comments
 (0)