Category Archives: Arduino 9-Axis IMU Totorials

In this lesson we show how to use quaternions from the BNO055 to create a visualization in Vpython. The visualization is a complete 3D free body rotation of a rigid body. To build this project you will need an Arduino Nano, and an Adafruit BNO055 Inertial Measurement Sensor.

This is the code we developed in the video posted here for your convenience. This code is for demo purposes only and should not be used in real applications. It is for educational purposes only.

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>


#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);
}

void loop() {
  // put your main code here, to run repeatedly:
uint8_t system, gyro, accel, mg = 0;
myIMU.getCalibration(&system, &gyro, &accel, &mg);

imu::Quaternion quat=myIMU.getQuat();

Serial.print(quat.w());
Serial.print(",");
Serial.print(quat.x());
Serial.print(",");
Serial.print(quat.y());
Serial.print(",");
Serial.print(quat.z());
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.println(system);

delay(BNO055_SAMPLERATE_DELAY_MS);
}

#include <Wire.h>

#include <Adafruit_Sensor.h>

#include <Adafruit_BNO055.h>

#include <utility/imumaths.h>

#include <math.h>

#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);

}

void loop() {

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

uint8_t system, gyro, accel, mg = 0;

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

imu::Quaternion quat=myIMU.getQuat();

Serial.print(quat.w());

Serial.print(",");

Serial.print(quat.x());

Serial.print(",");

Serial.print(quat.y());

Serial.print(",");

Serial.print(quat.z());

Serial.print(",");

Serial.print(accel);

Serial.print(",");

Serial.print(gyro);

Serial.print(",");

Serial.print(mg);

Serial.print(",");

Serial.println(system);

delay(BNO055_SAMPLERATE_DELAY_MS);

}

This is the code we developed on the python side to do the visualization from the passed quaternions.

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

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

scene.width=1200
scene.height=1080

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):
    try:
        while (ad.inWaiting()==0):
            pass
        dataPacket=ad.readline()
        dataPacket=str(dataPacket,'utf-8')
        splitPacket=dataPacket.split(",")
        q0=float(splitPacket[0])
        q1=float(splitPacket[1])
        q2=float(splitPacket[2])
        q3=float(splitPacket[3])

        roll=-math.atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2))
        pitch=math.asin(2*(q0*q2-q3*q1))
        yaw=-math.atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))-np.pi/2

        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)
        vrot=v*cos(roll)+cross(k,v)*sin(roll)

        frontArrow.axis=k
        sideArrow.axis=cross(k,vrot)
        upArrow.axis=vrot
        myObj.axis=k
        myObj.up=vrot
        sideArrow.length=2
        frontArrow.length=4
        upArrow.length=1
    except:
        pass

from vpython import *

from time import *

import numpy as np

import math

import serial

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

sleep(1)

scene.range=5

scene.background=color.yellow

toRad=2*np.pi/360

toDeg=1/toRad

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

scene.width=1200

scene.height=1080

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):

try:

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

pass

dataPacket=ad.readline()

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

splitPacket=dataPacket.split(",")

q0=float(splitPacket[0])

q1=float(splitPacket[1])

q2=float(splitPacket[2])

q3=float(splitPacket[3])

roll=-math.atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2))

pitch=math.asin(2*(q0*q2-q3*q1))

yaw=-math.atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))-np.pi/2

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)

vrot=v*cos(roll)+cross(k,v)*sin(roll)

frontArrow.axis=k

sideArrow.axis=cross(k,vrot)

upArrow.axis=vrot

myObj.axis=k

myObj.up=vrot

sideArrow.length=2

frontArrow.length=4

upArrow.length=1

except:

pass

Arduino, Arduino 9-Axis IMU Totorials

9-Axis IMU LESSON 20: Vpython Visualization of Roll, Pitch, and Yaw

December 26, 2019 admin

<span data-mce-type="bookmark" style="display: inline-block; width: 0px; overflow: hidden; line-height: 0;" class="mce_SELRES_start"></span>
This is the arduino code we developed in this lesson to approximate roll, pitch and yaw over small ranges.

#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(phi);
Serial.print(",");
Serial.print(theta);
Serial.print(",");
Serial.print(psi);
Serial.print(",");
Serial.print(accel);
Serial.print(",");
Serial.print(gyro);
Serial.print(",");
Serial.print(mg);
Serial.print(",");
Serial.println(system);


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(phi);

Serial.print(",");

Serial.print(theta);

Serial.print(",");

Serial.print(psi);

Serial.print(",");

Serial.print(accel);

Serial.print(",");

Serial.print(gyro);

Serial.print(",");

Serial.print(mg);

Serial.print(",");

Serial.println(system);

phiFold=phiFnew;

thetaFold=thetaFnew;

delay(BNO055_SAMPLERATE_DELAY_MS);

}

This is the python code we developed to visualize the 3 dimensional rotation of a rigid body.

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)
    vrot=v*cos(roll)+cross(k,v)*sin(roll)

    frontArrow.axis=k
    sideArrow.axis=cross(k,vrot)
    upArrow.axis=vrot
    myObj.axis=k
    myObj.up=vrot
    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