Yaw, Pitch and Roll. From Values to Visual Interface

Intro

Hi!

So I come here with another little project. In this one, I want to highlight the use of processing as a visual interface for sensors output and a bit of basics on using gyroscope, accelarometer and magnetometer to calculate the yaw, pitch and roll of a platform.

In this project, I used particularly a STM32 board, an IAR workbench IDE and the standard peripheral library but you can use a Arduino Uno or pretty much any Arduino for this purpose.

I made this project in the hopes to learn how to use all that was listed above and the make a self-balancing robot, so let's see how that turns out. The processing interface was made with the purpose to showcase to younger students that usually visit the robotics club and the university.

You can see here the final result in a quick video:

Processing (V3)

If you use the Arduino, you will probably like processing.

Not only is the Arduino based in processing but there are plenty of examples and libraries out there, and it's pretty easy to program. From what I've read, it was created to aid artists bringing their ideas to reality with little programming knowledge - so it's pretty good to quickly make visual interfaces (and also command your project while your at it), though for time graphs I still prefer Matlab for now.

First is first, using the sensor data

In my project, I used a MPU6050 which includes a gyroscope and an accelerometer and a HMC5883L. I will not explain how to use these particular sensors, just how to use generic data.

If you aren't interested in this and came for the processing, skip ahead.

• Yaw using magnetometer - easy mode

Getting a Yaw from a 3-axis magnetometer can be easy - getting a correct yaw is hard.

The magnetometer detects any magnetic field and considering a perfect environment, only existing the earths magnetic field is ideal for this purpose. Usually it's also considered the field to be parallel to the ground for calculations and then a bit correction are made to get the true north (versus the magnetic north) - these corrections depend on the year and localization.

• First case - Z-axis perpendicular to the ground

If you have the sensor like that, the Z will be considered irrelevant. This way you can easily get the angle that the X-axis is making with the magnetic north just from the X and Y. Remember trigonometry? ArcTan will do the trick. Just do arctan(y/x) and done. How you do this in programming? Well you need:

float magnetic_heading = atan2(y,x); //returns heading in radians

Why atan2 instead of atan ? Because we want all 4 quadrants (check out the definition of atan2 and atan for more info). Test this out, to convert to degrees:

int Degrees = magnetic_heading * 180 / PI;

Easy right? But this is considering a no interference environment and no tilting (Z always perpendicular to the ground!).

I did implement some tilt compensation and a bit of hard metal and soft metal interference (look, already some concepts to be aware of) but it did not work good enough, 90º turns we're more like 80º turns you 110º. So for this project I will leave it here just to show a bit of yaw on the interface. For more info I think "Using LSM303DLH for a tilt compensated electronic compass" from ST is pretty good.

The gyroscope can be used to help out in a similar manner that will be shown for pitch and roll but I had problems with the turning point from 1º to 359º.

• Pitch and Roll

Pitch and roll we're much more reliable to calculate. For this you use both the accelerometer and the gyroscope to compensate each others problems.

First the accelerometer.

It measures accelerations. Stand still (or just keep it moving at constant speed) and only gravity will be detected - which points always to the same direction: down. To get the angles I really liked ST's application note approach. Being X and Y the values from the accelerometer axes:

Pitch = asin(-X);
Roll = asin( Y / cos(Pitch)  );

Be careful, if the pitch module is 90º. The cosine will be 0 (dividing by 0 is not a good idea), so you should avoid doing the Roll math if that's the case and just set Roll to 0º. The problem with this is, quick movements will throw off the calculations, since there will be acceleration besides gravity. That's where the gyroscope will help.

Okay, now the gyroscope alone. The gyro measures angular velocity. Very well, if you remember calculus, just integrate angular velocity to get the angle in a time interval. Well we can't integrate but we can do sums, that's pretty close right?

If you have a cycle doing this math, and the time between measurements (can be defined by the sensor measure rate) times the angular velocity will give you the angular displacement between measurements, right? Keep adding that and you will get a approximation of the integral (with a error of course, the smaller intervals, the better, but for example the MPU6050 can do more than 8KHz).

int pitch =0;
while(1){
G_Y = Get_Y_measurement(); //returns in º/s
pitch += 0,1 * G_Y;
(100); //wait 100ms, let's imagine this is the time between measurements
}

Now remember, for roll you use the X-axis of the gyroscope.

And with this you got Roll and pitch on the gyroscope. You can get in a similar way the yaw using the Z-axis.

But is this enough? Better than the accelerometer? Well it's pretty precise... for a while. After a minute or so you will probably see a considerable error. Remember the math? We wanted an integral, we approximated. That means there's error accumulation.

• How to solve this

You use filters. There's for example the Kalman filter (pretty complicated) and the complementary filter - the one I used.

The complementary filter sums both the calculations from the accelerometer and the gyro with different weights. The gyro which is great for short quick movements, will have the biggest weight, like 98%, while the accelerometer will have just 2% (this could/should be adjusted).

Something like this:

Final_Roll = G_Roll * 0,98 + A_Roll * 0,02

Where G_Roll is the roll calculated from the gyro and A_Roll is from the accelerometer.

How will it work? Well, the gyro will give near 0 readings when standing still - meaning the total contribution will be very tiny - while the Acc will be in the situation it excels: standing still - bringing the Roll to a pretty accurate value, eliminating accumulation errors from the gyro.

When moving the gyro will overwhelm the Acc error from the extra acceleration, of course this works best with short rotations, if you rotate for a minute the gyro error will start being evident on the final roll.

Do something similar to the Pitch and done. Of course all this didn't compensate for individual errors from the sensors like offset values.

Yay! Processing now! No more math!

So time for some programming in processing. The code will be below as I think that, complemented with comments, is best to show how it was done. Copying it to processing IDE + do CTRL+T to index is a good idea to make it more legible.

First here is a little video of it working:

/*
 Rotation visual interface
 To use you require a microcontroler sending yaw, pich and roll in this order:
   yaw least significant byte
   yaw most significant byte
   Pitch (1 byte)
   Roll (1 byte)
 This in a cycle. Be carefull to not send it too fast or it will start lagging.
 Made by: Luís Afonso
*/
import processing.serial.*;
Serial myPort;  // Create object from Serial class
PFont f;
int Yaw_H;      // Data received from the serial port
int Yaw_L = 0;
int Pitch_H;      // Data received from the serial port
int Pitch_L = 0;
int Roll_H;      // Data received from the serial port
int Roll_L = 0;
float r;
// Angle and angular velocity, accleration
float F_Yaw;
float F_Pitch;
float F_Roll;
int xi = 0, yi=0;
void setup() 
{
 //size(1300, 720);
 fullScreen();
 // Initialize all values
 r = height * 0.45;
 // I know that the first port in the serial list on my mac
 // is always my  FTDI adaptor, so I open Serial.list()[0].
 // On Windows machines, this generally opens COM1.
 // Open whatever port is the one you're using.
 String portName = Serial.list()[0];
 myPort = new Serial(this, portName, 115200);
 /* 
     Change the text font to make it bigger
     You need to go to "Tools->Create font" to be able to use this. Just comment these 2 lines out
   if you don't feel like it.
 */
 f = createFont("SourceCodePro-Regular.vlw", 20);
 textFont(f);
}
void draw()
{
 /* 
   Wait for at least 4 values to be available to receive.
   It should be in order:
   yaw least significant byte
   yaw most significant byte
   Pitch 
   Roll 
   Pitch and Roll can be just 1 byte since they go in a 180º range, while yaw is 360º (360 doesn't fit in 8bit number)
 */
 if (myPort.available() >= 4) {  // If data is available,
   Yaw_L = myPort.read();         // read it and store it in val
   Yaw_H = myPort.read();         // read it and store it in val
   Pitch_L = myPort.read();         // read it and store it in val
   //Pitch_H = myPort.read();         // read it and store it in val
   Roll_L = myPort.read();         // read it and store it in val
   //Roll_H = myPort.read();         // read it and store it in val
   /* Just for debug purposes, prints in text console */
   print(Yaw_L);
   print(" ");
   print(Yaw_H);
   print(" ");
   print(Pitch_L);
   print(" ");
   print(Roll_L);
   println(" ");
 }
 //Set background to black (will erase previous drawing that was there in each cycle)
 background(0);
 // Translate the origin point to the center of the screen
 translate(width/2, height/2);
 int Yaw;
 Yaw = (Yaw_H < int Pitch = 1*(Pitch_L-90); int Roll = 1*(Roll_L-90); F_Yaw = ((Yaw+90)*PI/180.0); // Convert polar to cartesian float x = -r * cos(F_Yaw); float y = r * sin(F_Yaw); // Draw the ellipse at the cartesian coordinate ellipseMode(CENTER); noStroke(); fill(0, 255, 255); ellipse(x, y, 32, 32); fill(200); ellipse(0, 0, 10, 10); float F_Roll = Roll * PI /180; y = sin(F_Roll) * 300; x = cos(F_Roll)*300; stroke(0, 255, 0); line(xi, yi, x+xi, y+yi); line(xi, yi, xi-x, yi-y); float F_Pitch = Pitch * PI /180; float  y_P=  yi + sin(F_Pitch)*100; stroke(255, 0, 0); line(0, y_P, 300, y_P); line(0, y_P, -300, y_P); textAlign(RIGHT); fill(0, 255, 0); text("Roll:", 300, 200); text(Roll, 350, 200); fill(255, 0, 0); text("Pitch:", 300, 240); text(Pitch, 350, 240); fill(0, 100, 255); text("Yaw:", 300, 280); text(Yaw, 350, 280);}