Published August 21, 2019 © MIT

Calculate Pitch and Roll Using Hexabitz Accelerometer

Using Hexabitz IMU and digital compass (H0BR40) module and Processing IDE I calculated pitch and roll angles and visualized them in 3D.

IntermediateShowcase (no instructions)12 minutes1,058

Calculate Pitch and Roll Using Hexabitz Accelerometer

Things used in this project

Hardware components

Hexabitz 3-axis IMU and 3-axis Compass Module (H0BR40)

Software apps and online services

The Processing Foundation Processing

Hand tools and fabrication machines

USB-UART Prototype Cable

Wired Kelvin clamp

Story

IMU and Digital Compass (H0BR40) is a 3-axis Inertial Measurement Unit (IMU) + 3-axis Digital Compass, which means it can be use as accelerometer, gyroscope and magnetometer.

I used just the accelerometer to calculate two of the three aircraft principal axes angles which were roll and pitch, duo to some physics I couldn't get yaw from accelerometer.

First of all, I needed to get a stream of filtered and calibrated accelerometer values in g. according to the module's factsheet you can get that by using the CLI commands.

Once we get the Acc values it would be something like this:

 Acc(G) | X: 0.20, Y: 0.98, Z: 0.06

Which would be a string now I used processing to get the values from this string.

After that using physics and math magic I get roll and pitch.

Below you can see three videos the first one is testing just roll - you can do that by adding // before rotateX(YaPiRo[1]); - the second one is testing pitch - again you can do that by adding // before rotateZ(YaPiRo[0]); - finally the complete thing:

Processing Code

import processing.serial.*;
import processing.opengl.*;
import toxi.geom.*;
import toxi.processing.*; 

Serial myPort;  // Create object from Serial class
String portName = "COM35"; //change this
String output;

float [] acceleration = new float [3]; //AccX,AccY,AccZ
float [] YaPiRo = new float [3]; //Yaw/Pitch/Roll angles

public void settings() {
  size(300, 300, P3D);
}

void setup()
  {
  myPort = new Serial(this, portName, 921600);
  myPort.write("\r");
  delay(20);
  myPort.clear();
  sendMSG("stream acc 200 5000000");
  delay(20);
  myPort.clear();
  }

void draw()
  {
    getCordinate();
    acc2PiRo();
    
    background(0);
    // translate everything to the middle of the viewport
    pushMatrix();
    translate(width / 2, height / 2);
    //rotateY(YaPiRo[0]);
    rotateZ(YaPiRo[2]);
    rotateX(YaPiRo[1]);
    fill(255, 0, 0, 200);
    box(10, 10, 200);
    
    // draw front-facing tip in blue
    fill(0, 0, 255, 200);
    pushMatrix();
    translate(0, 0, -120);
    rotateX(PI/2);
    drawCylinder(0, 20, 20, 8);
    popMatrix();
    
    // draw wings and tail fin in green
    fill(0, 255, 0, 200);
    beginShape(TRIANGLES);
    vertex(-100,  2, 30); vertex(0,  2, -80); vertex(100,  2, 30);  // wing top layer
    vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30);  // wing bottom layer
    vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70);  // tail left layer
    vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70);  // tail right layer
    endShape();
    beginShape(QUADS);
    vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(  0, -2, -80); vertex(  0, 2, -80);
    vertex( 100, 2, 30); vertex( 100, -2, 30); vertex(  0, -2, -80); vertex(  0, 2, -80);
    vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2,  30); vertex(100, 2,  30);
    vertex(-2,   0, 98); vertex(2,   0, 98); vertex(2, -30, 98); vertex(-2, -30, 98);
    vertex(-2,   0, 98); vertex(2,   0, 98); vertex(2,   0, 70); vertex(-2,   0, 70);
    vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2,   0, 70); vertex(-2,   0, 70);
    endShape();
    
    popMatrix();
    
        
      
  }
void sendMSG(String msg){
  int strLenght = msg.length();
  for (int i = 0; i<strLenght; i++)
  {
    myPort.write(msg.charAt(i));
    delay(20);
  }
  myPort.write("\r");
}

void getCordinate()
{
      if (myPort.available()>1)
    {
      output = myPort.readStringUntil(10);
    }
    if(output != null)
    {
      if(output.charAt(0) == 'A'){
      float sign = 1.00;
      int Index = output.indexOf('X');
      if(Index!=-1){
      if(output.charAt(Index+3) == '-') {sign = -1; Index++;} else {sign = 1;}
      String temp = output.substring(Index+3,Index+7);
      acceleration[0] = sign*float(temp);
      Index = output.indexOf('Y');
      if(output.charAt(Index+3) == '-') {sign = -1; Index++;} else {sign = 1;}
      temp = output.substring(Index+3,Index+7);
      acceleration[1] = sign*float(temp);
      Index = output.indexOf('Z');
      if(output.charAt(Index+3) == '-') {sign = -1; Index++;} else {sign = 1;}
      temp = output.substring(Index+3,Index+7);
      acceleration[2] = sign*float(temp);
    print(output);
  }}
}
}

void acc2PiRo()
{
    YaPiRo[1] = atan2(acceleration[1] , acceleration[2]);
    YaPiRo[2] = atan2(-acceleration[0] , acceleration[2]);
}
void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) {
    float angle = 0;
    float angleIncrement = TWO_PI / sides;
    beginShape(QUAD_STRIP);
    for (int i = 0; i < sides + 1; ++i) {
        vertex(topRadius*cos(angle), 0, topRadius*sin(angle));
        vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle));
        angle += angleIncrement;
    }
    endShape();
    
    // If it is not a cone, draw the circular top cap
    if (topRadius != 0) {
        angle = 0;
        beginShape(TRIANGLE_FAN);
        
        // Center point
        vertex(0, 0, 0);
        for (int i = 0; i < sides + 1; i++) {
            vertex(topRadius * cos(angle), 0, topRadius * sin(angle));
            angle += angleIncrement;
        }
        endShape();
    }
  
    // If it is not a cone, draw the circular bottom cap
    if (bottomRadius != 0) {
        angle = 0;
        beginShape(TRIANGLE_FAN);
    
        // Center point
        vertex(0, tall, 0);
        for (int i = 0; i < sides + 1; i++) {
            vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle));
            angle += angleIncrement;
        }
        endShape();
    }
}