Tag Archives: BNO055

To play along at home, you will need an Arduino Nano, and an Adafruit BNO055 Inertial Measurement Sensor. In this lesson we create a live visual where a 3D model rotates in space mimicking the pitch and yaw of the breadboard in the real world. We have not yet derived and implemented the math to incorporate roll into the simulation but that will ab done in the next lesson.

This is the code on the arduino side we developed in the video:

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;

float thetaG=0;
float phiG=0;

float theta;
float phi;

float thetaRad;
float phiRad;

float Xm;
float Ym;
float psi;


float dt;
unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
millisOld=millis();
}

void loop() {
  // put your main code here, to run repeatedly:
uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);
imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
imu::Vector<3> mag =myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiFnew=.95*phiFold+.05*phiM;
thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;
millisOld=millis();
theta=(theta+gyr.y()*dt)*.95+thetaM*.05;
phi=(phi-gyr.x()*dt)*.95+ phiM*.05;
thetaG=thetaG+gyr.y()*dt;
phiG=phiG-gyr.x()*dt;

phiRad=phi/360*(2*3.14);
thetaRad=theta/360*(2*3.14);

Xm=mag.x()*cos(thetaRad)-mag.y()*sin(phiRad)*sin(thetaRad)+mag.z()*cos(phiRad)*sin(thetaRad);
Ym=mag.y()*cos(phiRad)+mag.z()*sin(phiRad);

psi=atan2(Ym,Xm)/(2*3.14)*360;

Serial.print(acc.x()/9.8);
Serial.print(",");
Serial.print(acc.y()/9.8);
Serial.print(",");
Serial.print(acc.z()/9.8);
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.print(system);
Serial.print(",");
Serial.print(thetaM);
Serial.print(",");
Serial.print(phiM);
Serial.print(",");
Serial.print(thetaFnew);
Serial.print(",");
Serial.print(phiFnew);
Serial.print(",");
Serial.print(thetaG);
Serial.print(",");
Serial.print(phiG);
Serial.print(",");
Serial.print(theta);
Serial.print(",");
Serial.print(phi);
Serial.print(",");
Serial.println(psi);

phiFold=phiFnew;
thetaFold=thetaFnew;

 
delay(BNO055_SAMPLERATE_DELAY_MS);
}

100

101

102

103

104

105

106

107

108

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

float thetaM;

float phiM;

float thetaFold=0;

float thetaFnew;

float phiFold=0;

float phiFnew;

float thetaG=0;

float phiG=0;

float theta;

float phi;

float thetaRad;

float phiRad;

float Xm;

float Ym;

float psi;

float dt;

unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

myIMU.begin();

delay(1000);

int8_t temp=myIMU.getTemp();

myIMU.setExtCrystalUse(true);

millisOld=millis();

}

void loop() {

// put your main code here, to run repeatedly:

uint8_t system, gyro, accel, mg = 0;

myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);

imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

imu::Vector<3> mag =myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);

thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiFnew=.95*phiFold+.05*phiM;

thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;

millisOld=millis();

theta=(theta+gyr.y()*dt)*.95+thetaM*.05;

phi=(phi-gyr.x()*dt)*.95+ phiM*.05;

thetaG=thetaG+gyr.y()*dt;

phiG=phiG-gyr.x()*dt;

phiRad=phi/360*(2*3.14);

thetaRad=theta/360*(2*3.14);

Xm=mag.x()*cos(thetaRad)-mag.y()*sin(phiRad)*sin(thetaRad)+mag.z()*cos(phiRad)*sin(thetaRad);

Ym=mag.y()*cos(phiRad)+mag.z()*sin(phiRad);

psi=atan2(Ym,Xm)/(2*3.14)*360;

Serial.print(acc.x()/9.8);

Serial.print(",");

Serial.print(acc.y()/9.8);

Serial.print(",");

Serial.print(acc.z()/9.8);

Serial.print(",");

Serial.print(accel);

Serial.print(",");

Serial.print(gyro);

Serial.print(",");

Serial.print(mg);

Serial.print(",");

Serial.print(system);

Serial.print(",");

Serial.print(thetaM);

Serial.print(",");

Serial.print(phiM);

Serial.print(",");

Serial.print(thetaFnew);

Serial.print(",");

Serial.print(phiFnew);

Serial.print(",");

Serial.print(thetaG);

Serial.print(",");

Serial.print(phiG);

Serial.print(",");

Serial.print(theta);

Serial.print(",");

Serial.print(phi);

Serial.print(",");

Serial.println(psi);

phiFold=phiFnew;

thetaFold=thetaFnew;

delay(BNO055_SAMPLERATE_DELAY_MS);

}

This is the code on the Python side we developed in the video:

from vpython import *
from time import *
import numpy as np
import math
import serial
ad=serial.Serial('com5',115200)
sleep(1)


scene.range=5
toRad=2*np.pi/360
toDeg=1/toRad
scene.forward=vector(-1,-1,-1)

scene.width=600
scene.height=600

xarrow=arrow(lenght=2, shaftwidth=.1, color=color.red,axis=vector(1,0,0))
yarrow=arrow(lenght=2, shaftwidth=.1, color=color.green,axis=vector(0,1,0))
zarrow=arrow(lenght=4, shaftwidth=.1, color=color.blue,axis=vector(0,0,1))

frontArrow=arrow(length=4,shaftwidth=.1,color=color.purple,axis=vector(1,0,0))
upArrow=arrow(length=1,shaftwidth=.1,color=color.magenta,axis=vector(0,1,0))
sideArrow=arrow(length=2,shaftwidth=.1,color=color.orange,axis=vector(0,0,1))

bBoard=box(length=6,width=2,height=.2,opacity=.8,pos=vector(0,0,0,))
bn=box(length=1,width=.75,height=.1, pos=vector(-.5,.1+.05,0),color=color.blue)
nano=box(lenght=1.75,width=.6,height=.1,pos=vector(-2,.1+.05,0),color=color.green)
myObj=compound([bBoard,bn,nano])
while (True):
    while (ad.inWaiting()==0):
        pass
    dataPacket=ad.readline()
    dataPacket=str(dataPacket,'utf-8')
    splitPacket=dataPacket.split(",")
    roll=float(splitPacket[0])*toRad
    pitch=float(splitPacket[1])*toRad
    yaw=float(splitPacket[2])*toRad+np.pi
    print("Roll=",roll*toDeg," Pitch=",pitch*toDeg,"Yaw=",yaw*toDeg)
    rate(50)
    k=vector(cos(yaw)*cos(pitch), sin(pitch),sin(yaw)*cos(pitch))
    y=vector(0,1,0)
    s=cross(k,y)
    v=cross(s,k)

    frontArrow.axis=k
    sideArrow.axis=s
    upArrow.axis=v
    myObj.axis=k
    myObj.up=v
    sideArrow.length=2
    frontArrow.length=4
    upArrow.length=1

from vpython import *

from time import *

import numpy as np

import math

import serial

ad=serial.Serial('com5',115200)

sleep(1)

scene.range=5

toRad=2*np.pi/360

toDeg=1/toRad

scene.forward=vector(-1,-1,-1)

scene.width=600

scene.height=600

xarrow=arrow(lenght=2, shaftwidth=.1, color=color.red,axis=vector(1,0,0))

yarrow=arrow(lenght=2, shaftwidth=.1, color=color.green,axis=vector(0,1,0))

zarrow=arrow(lenght=4, shaftwidth=.1, color=color.blue,axis=vector(0,0,1))

frontArrow=arrow(length=4,shaftwidth=.1,color=color.purple,axis=vector(1,0,0))

upArrow=arrow(length=1,shaftwidth=.1,color=color.magenta,axis=vector(0,1,0))

sideArrow=arrow(length=2,shaftwidth=.1,color=color.orange,axis=vector(0,0,1))

bBoard=box(length=6,width=2,height=.2,opacity=.8,pos=vector(0,0,0,))

bn=box(length=1,width=.75,height=.1, pos=vector(-.5,.1+.05,0),color=color.blue)

nano=box(lenght=1.75,width=.6,height=.1,pos=vector(-2,.1+.05,0),color=color.green)

myObj=compound([bBoard,bn,nano])

while (True):

while (ad.inWaiting()==0):

pass

dataPacket=ad.readline()

dataPacket=str(dataPacket,'utf-8')

splitPacket=dataPacket.split(",")

roll=float(splitPacket[0])*toRad

pitch=float(splitPacket[1])*toRad

yaw=float(splitPacket[2])*toRad+np.pi

print("Roll=",roll*toDeg," Pitch=",pitch*toDeg,"Yaw=",yaw*toDeg)

rate(50)

k=vector(cos(yaw)*cos(pitch), sin(pitch),sin(yaw)*cos(pitch))

y=vector(0,1,0)

s=cross(k,y)

v=cross(s,k)

frontArrow.axis=k

sideArrow.axis=s

upArrow.axis=v

myObj.axis=k

myObj.up=v

sideArrow.length=2

frontArrow.length=4

upArrow.length=1

Arduino, Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 17: Review of Basic Trigonometry

December 5, 2019 admin

In this video we review the basic trigonometry needed in order to get the Vpython visualization to work. With this quick trig tutorial, you should have the math you need to finish the project.

Arduino, Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 12: Passing Data From Arduino to Python

October 31, 2019 admin

In this lesson we show how to pass data from Arduino to Python using a Com Port. This is important for our 9-Axis IMU project as we want to take advantage of the processing power and 3D graphics capabilities of Python. Our goal is to get the date from Arduino to Python, and then create a dynamic 3D visualization of our system. The first step in this goal is to pass the data from arduino to Python.

In order to do this, a first step is to install the pyserial library. If you followed our python installation tutorial in lesson 11, then it is easy to install pyserial by just opening a windows command prompt, and then typing:

pip install pyserial

If this does not work, likely you did not install python according to the instruction in lesson 11.

In order to show a simple demonstration of passing data, we can use the following code on the arduino side, which just generates x, y, and z numbers and passes them to Python.

int x=0;
int y=0;
int z=0;
void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
}

void loop() {
  // put your main code here, to run repeatedly:
x=x+1;
y=y+2;
z=z+4;

Serial.print(x);
Serial.print(",");
Serial.print(y);
Serial.print(",");
Serial.println(z);
delay(100);


}

int x=0;

int y=0;

int z=0;

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

}

void loop() {

// put your main code here, to run repeatedly:

x=x+1;

y=y+2;

z=z+4;

Serial.print(x);

Serial.print(",");

Serial.print(y);

Serial.print(",");

Serial.println(z);

delay(100);

}

We can grab these numbers from the Com port on the Python side with the following code. Note that you should use the com port your arduino is on, which likely will not be the same as mine (which was ‘com5’).

import time
import serial
arduinoData=serial.Serial('com5',115200)
time.sleep(1)
while (True):
    while (arduinoData.inWaiting()==0):
        pass
    dataPacket = arduinoData.readline() #reply
    dataPacket=str(dataPacket,'utf-8')
    print(dataPacket)
    splitPacket=dataPacket.split(",")
    print (splitPacket)
    X=float(splitPacket[0])
    Y=float(splitPacket[1])
    Z=float(splitPacket[2])
    print ("X=",X," Y=",Y," Z=",Z)

import time

import serial

arduinoData=serial.Serial('com5',115200)

time.sleep(1)

while (True):

while (arduinoData.inWaiting()==0):

pass

dataPacket = arduinoData.readline() #reply

dataPacket=str(dataPacket,'utf-8')

print(dataPacket)

splitPacket=dataPacket.split(",")

print (splitPacket)

X=float(splitPacket[0])

Y=float(splitPacket[1])

Z=float(splitPacket[2])

print ("X=",X," Y=",Y," Z=",Z)

The above example is just a simple method for passing different channels from Arduino to Python.

For our IMU project, we want to use the code we left off with Lesson 10. However, note we can scale back on the number of data channels, because we just want the calibration data and then the final roll, pitch and yaw numbers. This is the arduino code that will pass those parameters.

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;

float thetaG=0;
float phiG=0;

float theta;
float phi;

float thetaRad;
float phiRad;

float Xm;
float Ym;
float psi;

float dt;
unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
millisOld=millis();
}

void loop() {
  // put your main code here, to run repeatedly:
uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);
imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
imu::Vector<3> mag =myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiFnew=.95*phiFold+.05*phiM;
thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;
millisOld=millis();
theta=(theta+gyr.y()*dt)*.95+thetaM*.05;
phi=(phi-gyr.x()*dt)*.95+ phiM*.05;
thetaG=thetaG+gyr.y()*dt;
phiG=phiG-gyr.x()*dt;

phiRad=phi/360*(2*3.14);
thetaRad=theta/360*(2*3.14);

Xm=mag.x()*cos(thetaRad)-mag.y()*sin(phiRad)*sin(thetaRad)+mag.z()*cos(phiRad)*sin(thetaRad);
Ym=mag.y()*cos(phiRad)+mag.z()*sin(phiRad);

psi=atan2(Ym,Xm)/(2*3.14)*360;

Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.print(system);
Serial.print(",");
Serial.print(theta);
Serial.print(",");
Serial.print(phi);
Serial.print(",");
Serial.println(psi);

phiFold=phiFnew;
thetaFold=thetaFnew;

 
delay(BNO055_SAMPLERATE_DELAY_MS);
}

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

float thetaM;

float phiM;

float thetaFold=0;

float thetaFnew;

float phiFold=0;

float phiFnew;

float thetaG=0;

float phiG=0;

float theta;

float phi;

float thetaRad;

float phiRad;

float Xm;

float Ym;

float psi;

float dt;

unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

myIMU.begin();

delay(1000);

int8_t temp=myIMU.getTemp();

myIMU.setExtCrystalUse(true);

millisOld=millis();

}

void loop() {

// put your main code here, to run repeatedly:

uint8_t system, gyro, accel, mg = 0;

myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);

imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

imu::Vector<3> mag =myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);

thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiFnew=.95*phiFold+.05*phiM;

thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;

millisOld=millis();

theta=(theta+gyr.y()*dt)*.95+thetaM*.05;

phi=(phi-gyr.x()*dt)*.95+ phiM*.05;

thetaG=thetaG+gyr.y()*dt;

phiG=phiG-gyr.x()*dt;

phiRad=phi/360*(2*3.14);

thetaRad=theta/360*(2*3.14);

Xm=mag.x()*cos(thetaRad)-mag.y()*sin(phiRad)*sin(thetaRad)+mag.z()*cos(phiRad)*sin(thetaRad);

Ym=mag.y()*cos(phiRad)+mag.z()*sin(phiRad);

psi=atan2(Ym,Xm)/(2*3.14)*360;

Serial.print(accel);

Serial.print(",");

Serial.print(gyro);

Serial.print(",");

Serial.print(mg);

Serial.print(",");

Serial.print(system);

Serial.print(",");

Serial.print(theta);

Serial.print(",");

Serial.print(phi);

Serial.print(",");

Serial.println(psi);

phiFold=phiFnew;

thetaFold=thetaFnew;

delay(BNO055_SAMPLERATE_DELAY_MS);

}

Then, on the Python side we can grab and parse the data with this code.

import serial
import time
arduinoData=serial.Serial('com5',115200)
time.sleep(1)
while (1==1):
    while (arduinoData.inWaiting()==0):
        pass
    dataPacket=arduinoData.readline()
    
    dataPacket=str(dataPacket,'utf-8')
    splitPacket=dataPacket.split(',')
    Acal=float(splitPacket[0])
    Gcal=float(splitPacket[1])
    Mcal=float(splitPacket[2])
    Scal=float(splitPacket[3])
    Pitch=float(splitPacket[4])
    Roll=float(splitPacket[5])
    Yaw=float(splitPacket[6])
    print("Acal=",Acal,"Gcal=",Gcal,"Mcal=",Mcal,"Scal",Scal, "Pitch=",Pitch,"Roll=",Roll,"Yaw=",Yaw)

import serial

import time

arduinoData=serial.Serial('com5',115200)

time.sleep(1)

while (1==1):

while (arduinoData.inWaiting()==0):

pass

dataPacket=arduinoData.readline()

dataPacket=str(dataPacket,'utf-8')

splitPacket=dataPacket.split(',')

Acal=float(splitPacket[0])

Gcal=float(splitPacket[1])

Mcal=float(splitPacket[2])

Scal=float(splitPacket[3])

Pitch=float(splitPacket[4])

Roll=float(splitPacket[5])

Yaw=float(splitPacket[6])

print("Acal=",Acal,"Gcal=",Gcal,"Mcal=",Mcal,"Scal",Scal, "Pitch=",Pitch,"Roll=",Roll,"Yaw=",Yaw)

In the next lesson we will install Vpython and begin building our code to create dynamic 3D visualizations of our system.

Arduino, Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 10: Making a Tilt Compensated Compass with Arduino

October 17, 2019 admin

In this lesson we show you how to build a demo tilt compensated compass using the BNO055 9-axis sensor. We go through some trigonometry to help you understand conceptually how the device works.

To play along at home, you will need an Arduino Nano, and an Adafruit BNO055 Inertial Measurement Sensor.

The code below is provided for your convenience. It is intended only for bench top demos, and should not be used in real applications. Just for fun, not for drones, or other actual control applications.

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;

float thetaG=0;
float phiG=0;

float theta;
float phi;

float thetaRad;
float phiRad;

float Xm;
float Ym;
float psi;


float dt;
unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
millisOld=millis();
}

void loop() {
  // put your main code here, to run repeatedly:
uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);
imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
imu::Vector<3> mag =myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiFnew=.95*phiFold+.05*phiM;
thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;
millisOld=millis();
theta=(theta+gyr.y()*dt)*.95+thetaM*.05;
phi=(phi-gyr.x()*dt)*.95+ phiM*.05;
thetaG=thetaG+gyr.y()*dt;
phiG=phiG-gyr.x()*dt;

phiRad=phi/360*(2*3.14);
thetaRad=theta/360*(2*3.14);

Xm=mag.x()*cos(thetaRad)-mag.y()*sin(phiRad)*sin(thetaRad)+mag.z()*cos(phiRad)*sin(thetaRad);
Ym=mag.y()*cos(phiRad)+mag.z()*sin(phiRad);

psi=atan2(Ym,Xm)/(2*3.14)*360;

Serial.print(acc.x()/9.8);
Serial.print(",");
Serial.print(acc.y()/9.8);
Serial.print(",");
Serial.print(acc.z()/9.8);
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.print(system);
Serial.print(",");
Serial.print(thetaM);
Serial.print(",");
Serial.print(phiM);
Serial.print(",");
Serial.print(thetaFnew);
Serial.print(",");
Serial.print(phiFnew);
Serial.print(",");
Serial.print(thetaG);
Serial.print(",");
Serial.print(phiG);
Serial.print(",");
Serial.print(theta);
Serial.print(",");
Serial.print(phi);
Serial.print(",");
Serial.println(psi);

phiFold=phiFnew;
thetaFold=thetaFnew;

 
delay(BNO055_SAMPLERATE_DELAY_MS);
}

100

101

102

103

104

105

106

107

108

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

float thetaM;

float phiM;

float thetaFold=0;

float thetaFnew;

float phiFold=0;

float phiFnew;

float thetaG=0;

float phiG=0;

float theta;

float phi;

float thetaRad;

float phiRad;

float Xm;

float Ym;

float psi;

float dt;

unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

myIMU.begin();

delay(1000);

int8_t temp=myIMU.getTemp();

myIMU.setExtCrystalUse(true);

millisOld=millis();

}

void loop() {

// put your main code here, to run repeatedly:

uint8_t system, gyro, accel, mg = 0;

myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);

imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

imu::Vector<3> mag =myIMU.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);

thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiFnew=.95*phiFold+.05*phiM;

thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;

millisOld=millis();

theta=(theta+gyr.y()*dt)*.95+thetaM*.05;

phi=(phi-gyr.x()*dt)*.95+ phiM*.05;

thetaG=thetaG+gyr.y()*dt;

phiG=phiG-gyr.x()*dt;

phiRad=phi/360*(2*3.14);

thetaRad=theta/360*(2*3.14);

Xm=mag.x()*cos(thetaRad)-mag.y()*sin(phiRad)*sin(thetaRad)+mag.z()*cos(phiRad)*sin(thetaRad);

Ym=mag.y()*cos(phiRad)+mag.z()*sin(phiRad);

psi=atan2(Ym,Xm)/(2*3.14)*360;

Serial.print(acc.x()/9.8);

Serial.print(",");

Serial.print(acc.y()/9.8);

Serial.print(",");

Serial.print(acc.z()/9.8);

Serial.print(",");

Serial.print(accel);

Serial.print(",");

Serial.print(gyro);

Serial.print(",");

Serial.print(mg);

Serial.print(",");

Serial.print(system);

Serial.print(",");

Serial.print(thetaM);

Serial.print(",");

Serial.print(phiM);

Serial.print(",");

Serial.print(thetaFnew);

Serial.print(",");

Serial.print(phiFnew);

Serial.print(",");

Serial.print(thetaG);

Serial.print(",");

Serial.print(phiG);

Serial.print(",");

Serial.print(theta);

Serial.print(",");

Serial.print(phi);

Serial.print(",");

Serial.println(psi);

phiFold=phiFnew;

thetaFold=thetaFnew;

delay(BNO055_SAMPLERATE_DELAY_MS);

}

Arduino, Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 9: Accurate and Stable Tilt Using Accelerometers, Gyros and a Complimentary Filter

October 10, 2019 admin

In this lesson we show how a complimentary filter can be used to create the best of both worlds in approximating tilt from data coming from the accelerometers and gyros. The goal of our complimentary filter is to weight our overall result to take advantage the short term accuracy of the gyro measurement, and combine it with the long term stability of the accelerometer. In effect the combination is applying a high pass filter to the gyro and a low pass filter to the accelerometer. Remember that the angles are approximate, and were developed assuming roll and pitch stay under 45 degrees. The trigonometry becomes more difficult when considering higher tilt angles.

To play along at home, you will need an Arduino Nano, and an Adafruit BNO055 Inertial Measurement Sensor.

The code below is for demo purposes only, and should not be used in any real applications. It just demonstrates how a complimentary filter works. Of course, any real systems need to go into much more depth than the simple demo project below.

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>
float thetaM;
float phiM;
float thetaFold=0;
float thetaFnew;
float phiFold=0;
float phiFnew;

float thetaG=0;
float phiG=0;

float theta;
float phi;

float dt;
unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {
  // put your setup code here, to run once:
Serial.begin(115200);
myIMU.begin();
delay(1000);
int8_t temp=myIMU.getTemp();
myIMU.setExtCrystalUse(true);
millisOld=millis();
}

void loop() {
  // put your main code here, to run repeatedly:
uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);
imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;
phiFnew=.95*phiFold+.05*phiM;
thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;
millisOld=millis();
theta=(theta+gyr.y()*dt)*.95+thetaM*.05;
phi=(phi-gyr.x()*dt)*.95+ phiM*.05;
thetaG=thetaG+gyr.y()*dt;
phiG=phiG-gyr.x()*dt;

Serial.print(acc.x()/9.8);
Serial.print(",");
Serial.print(acc.y()/9.8);
Serial.print(",");
Serial.print(acc.z()/9.8);
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.print(system);
Serial.print(",");
Serial.print(thetaM);
Serial.print(",");
Serial.print(phiM);
Serial.print(",");
Serial.print(thetaFnew);
Serial.print(",");
Serial.print(phiFnew);

Serial.print(",");
Serial.print(thetaG);
Serial.print(",");
Serial.print(phiG);
Serial.print(",");
Serial.print(theta);
Serial.print(",");
Serial.println(phi);

phiFold=phiFnew;
thetaFold=thetaFnew;

 
delay(BNO055_SAMPLERATE_DELAY_MS);
}

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

float thetaM;

float phiM;

float thetaFold=0;

float thetaFnew;

float phiFold=0;

float phiFnew;

float thetaG=0;

float phiG=0;

float theta;

float phi;

float dt;

unsigned long millisOld;

#define BNO055_SAMPLERATE_DELAY_MS (100)

Adafruit_BNO055 myIMU = Adafruit_BNO055();

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

myIMU.begin();

delay(1000);

int8_t temp=myIMU.getTemp();

myIMU.setExtCrystalUse(true);

millisOld=millis();

}

void loop() {

// put your main code here, to run repeatedly:

uint8_t system, gyro, accel, mg = 0;

myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Vector<3> acc =myIMU.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);

imu::Vector<3> gyr =myIMU.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

thetaM=-atan2(acc.x()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiM=-atan2(acc.y()/9.8,acc.z()/9.8)/2/3.141592654*360;

phiFnew=.95*phiFold+.05*phiM;

thetaFnew=.95*thetaFold+.05*thetaM;

dt=(millis()-millisOld)/1000.;

millisOld=millis();

theta=(theta+gyr.y()*dt)*.95+thetaM*.05;

phi=(phi-gyr.x()*dt)*.95+ phiM*.05;

thetaG=thetaG+gyr.y()*dt;

phiG=phiG-gyr.x()*dt;

Serial.print(acc.x()/9.8);

Serial.print(",");

Serial.print(acc.y()/9.8);

Serial.print(",");

Serial.print(acc.z()/9.8);

Serial.print(",");

Serial.print(accel);

Serial.print(",");

Serial.print(gyro);

Serial.print(",");

Serial.print(mg);

Serial.print(",");

Serial.print(system);

Serial.print(",");

Serial.print(thetaM);

Serial.print(",");

Serial.print(phiM);

Serial.print(",");

Serial.print(thetaFnew);

Serial.print(",");

Serial.print(phiFnew);

Serial.print(",");

Serial.print(thetaG);

Serial.print(",");

Serial.print(phiG);

Serial.print(",");

Serial.print(theta);

Serial.print(",");

Serial.println(phi);

phiFold=phiFnew;

thetaFold=thetaFnew;

delay(BNO055_SAMPLERATE_DELAY_MS);

}

Technology Tutorials

Tag Archives: BNO055

9-Axis IMU LESSON 19: Vpython Visualization of Pitch and Yaw

9-Axis IMU LESSON 17: Review of Basic Trigonometry

9-Axis IMU LESSON 12: Passing Data From Arduino to Python

9-Axis IMU LESSON 10: Making a Tilt Compensated Compass with Arduino

9-Axis IMU LESSON 9: Accurate and Stable Tilt Using Accelerometers, Gyros and a Complimentary Filter

Making The World a Better Place One High Tech Project at a Time. Enjoy!