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.

// ====================================================================
// DISCLAIMER:
// This code is provided as-is for educational and experimental 
// purposes only. The author makes no representations or warranties of 
// any kind concerning the safety, suitability, or accuracy of this 
// code. Use at your own risk. The author assumes no liability for any 
// damages, system failures, security breaches, or network issues 
// resulting from the use or implementation of this script.
// ====================================================================
 
#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

109

110

111

112

113

114

115

116

117

118

// ====================================================================

// DISCLAIMER:

// This code is provided as-is for educational and experimental

// purposes only. The author makes no representations or warranties of

// any kind concerning the safety, suitability, or accuracy of this

// code. Use at your own risk. The author assumes no liability for any

// damages, system failures, security breaches, or network issues

// resulting from the use or implementation of this script.

// ====================================================================

#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, Tutorial

Arduino Tutorial 32: Understanding and Using Joysticks in a Project

October 15, 2019 admin

In this lesson we will explain how to hook up a Joystick. The easiest way to think of a joystick is to think of it as two independent potentiometer. Moving the joystick left and right changes one potentiometer, and moving the joystick up and down changes the other potentiometer. Also, pressing the knob on the joystick will activate a simple on/off switch. In this video we show you how to hook the joystick up, and then show you code that will allow you to read from the potentiometers and the switch.

If you want to follow along at home, an official Arduino Uno R3 is available HERE. In this new series of lessons, I will be using the sensor and other components found in this KIT.

Typically, the servos in electronics kits are not the best ones, but are suitable to learn with. Heads up that in Lesson 33 we will be using a joystick to control two servos. If you want to get ready for that lesson, go ahead and order your HiTEC Servos.

This is the code that we developed in the video above.

int Xpin=A0;
int Ypin=A1;
int Spin=2;
int Xval;
int Yval;
int Sval;
int dt=200;
void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);
pinMode(Xpin,INPUT);
pinMode(Ypin,INPUT);
pinMode(Spin,INPUT);
digitalWrite(Spin,HIGH);
}

void loop() {
 Xval=analogRead(Xpin);
 Yval=analogRead(Ypin);
 Sval=digitalRead(Spin);
 delay(dt);
 Serial.print("X Value = ");
 Serial.print(Xval);
 Serial.print(" Y Value = ");
 Serial.print(Yval);
 Serial.print(" Switch State is ");
 Serial.println(Sval);
}

int Xpin=A0;

int Ypin=A1;

int Spin=2;

int Xval;

int Yval;

int Sval;

int dt=200;

void setup() {

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

Serial.begin(9600);

pinMode(Xpin,INPUT);

pinMode(Ypin,INPUT);

pinMode(Spin,INPUT);

digitalWrite(Spin,HIGH);

}

void loop() {

Xval=analogRead(Xpin);

Yval=analogRead(Ypin);

Sval=digitalRead(Spin);

delay(dt);

Serial.print("X Value = ");

Serial.print(Xval);

Serial.print(" Y Value = ");

Serial.print(Yval);

Serial.print(" Switch State is ");

Serial.println(Sval);

}

Jetson Nano

Starting the Raspberry Pi Camera, or a WEB Camera on the Jetson Nano

October 14, 2019 admin

For those of you playing along at home as we learn Artificial Intelligence on the Jetson Nano, the code below is what we do to launch a camera using OpenCV. We include code that will launch either the Raspberry Pi Camera, or a WEB cam like the Logitech 920. Check out our Youtube Jetson Nano Channel to follow along the full tutorial. The Jetson Nano tutorial Play List can be found HERE:

The code below is the starting point for most of the lessons:

import cv2
print(cv2.__version__)
dispW=640
dispH=480
flip=2
#Uncomment These next Two Line for Pi Camera
#camSet='nvarguscamerasrc !  video/x-raw(memory:NVMM), width=3264, height=2464, format=NV12, framerate=21/1 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(dispW)+', height='+str(dispH)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! appsink'
#cam= cv2.VideoCapture(camSet)

#Or, if you have a WEB cam, uncomment the next line
#(If it does not work, try setting to '1' instead of '0')
#cam=cv2.VideoCapture(0)
while True:
    ret, frame = cam.read()
    cv2.imshow('nanoCam',frame)
    if cv2.waitKey(1)==ord('q'):
        break
cam.release()
cv2.destroyAllWindows()

import cv2

print(cv2.__version__)

dispW=640

dispH=480

flip=2

#Uncomment These next Two Line for Pi Camera

#camSet='nvarguscamerasrc ! video/x-raw(memory:NVMM), width=3264, height=2464, format=NV12, framerate=21/1 ! nvvidconv flip-method='+str(flip)+' ! video/x-raw, width='+str(dispW)+', height='+str(dispH)+', format=BGRx ! videoconvert ! video/x-raw, format=BGR ! appsink'

#cam= cv2.VideoCapture(camSet)

#Or, if you have a WEB cam, uncomment the next line

#(If it does not work, try setting to '1' instead of '0')

#cam=cv2.VideoCapture(0)

while True:

ret, frame = cam.read()

cv2.imshow('nanoCam',frame)

if cv2.waitKey(1)==ord('q'):

break

cam.release()

cv2.destroyAllWindows()

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.

// ====================================================================
// DISCLAIMER:
// This code is provided as-is for educational and experimental 
// purposes only. The author makes no representations or warranties of 
// any kind concerning the safety, suitability, or accuracy of this 
// code. Use at your own risk. The author assumes no liability for any 
// damages, system failures, security breaches, or network issues 
// resulting from the use or implementation of this script.
// ====================================================================
 
#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);
}

100

101

// ====================================================================

// DISCLAIMER:

// This code is provided as-is for educational and experimental

// purposes only. The author makes no representations or warranties of

// any kind concerning the safety, suitability, or accuracy of this

// code. Use at your own risk. The author assumes no liability for any

// damages, system failures, security breaches, or network issues

// resulting from the use or implementation of this script.

// ====================================================================

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

}

Arduino, Tutorial

Arduino Tutorial 31: Using Servo in a Simple Project

October 8, 2019 admin

In this lesson we demonstrate a simple project that reads the brightness in a room, and then displays the brightness on a device consisting of a servo with an arrow pointing at vaiouse visual indications of brightness. This project demonstrates how to use Algebra and the equation of a line to take the data read from the light sensor, and calculate the desired angle value to send to the servo.

If you want to follow along at home, an official Arduino Uno R3 is available HERE. In this new series of lessons, I will be using the sensor and other components found in this KIT.

Typically, the servos in electronics kits are not the best ones, but are suitable to learn with. If you want a more stable and better quality servo, this is the one I user in more of my projects: HiTEC

Technology Tutorials

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

Arduino Tutorial 32: Understanding and Using Joysticks in a Project

Starting the Raspberry Pi Camera, or a WEB Camera on the Jetson Nano

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

Arduino Tutorial 31: Using Servo in a Simple Project

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