It has been a while since my last post and I know that some of you are waiting for some very informative posts about gyroscopes and magnetometers, but today is not that day. I want to talk a wee bit about my life in the past few months, since I joined SeeByte and moved to Edinburgh (As you may recall, I was born and raised in Las Palmas de G.C.).

Even though I haven’t written much in this blog, if you go back about a year ago you will see that I was already working on some interesting UUV projects, so SeeByte seemed like the right place for me, since above all I’m a developer/programmer/software engineer/computer scientist. The work I’m doing is very interesting, but unfortunately I’m not allowed to talk about it, suffice it to say it is related to ROVs, as was my thesis, although the level of complexity is much higher.

Three years ago, in 2009, I came to Edinburgh with my sister and thought it would be a great place to live in, and now that I live here I can certainly agree with my past self. If you come from a hot place, like the Canary Islands, the Scottish weather may not agree with you, but I have to say that I really do like the cold, those of you who know me probably know that already. All right, to be fair I sometimes miss the Sun and the heat.

Aside from my job and my personal life, I have also been dedicating some time to my projects and in doing so I’ve learned quite a lot about electronics. The first of the projects I completed was a GPS datalogger but for that one I will dedicate a full post which is already half written. The rest of the projects are not that interesting but I’m quite proud of two of them, one is a Sound Meter (also known as VU Meter) and the other one is a variable power supply.

The variable power supply uses a few voltage regulators in order to achieve fixed 5v and a variable voltage dependent on the input voltage, which can be anything between 7v and 36v  if I’m not mistaken, and the value of the potentiometer, this voltage can then be set between the input voltage and 1.25v. I also added an LCD voltage meter I bought a while ago from ebay. The end result is a very useful device which I can use to power the rest of my projects with a few standard AA batteries.

The sound meter was just an idea I had to learn about LED matrices and shift registers, but it ended up being a lot of fun. In this project I also included an Attiny85, which is a very small microcontroller similar to the ones you can find on the Arduino. In order to program the Attiny, I used the Arduino itself and the Arduino IDE.

The basic idea behind the sound meter is to sample the output of a standard microphone and extract from it some sort of volume level. I didn’t want to spend much time with the programming part of the project so the algorithm I implemented is very simple and it is probably not as good as some others you can find on other VU meters.

Once the volume level has been obtained, the shift register is used in order to activate the necessary rows of the LED matrix. In the following video you can see an example of the sound meter working when it was just a prototype on a breadboard, the code I was using is quite different from the latest version and the result is much nicer now, but I was too lazy to make another video.

I think that’s all I wanted to say for now, on my next post I will talk about the GPS Datalogger. Sorry for those who are waiting for the gyro stuff, but you will have to wait a bit more, spoiler alert, gyros are not very useful on their own.

# Arduino IMU: Pitch & Roll from an Accelerometer

One day, looking for cheap sensors on ebay, I found this interesting board which contained everything I was looking for. It basically consists of a 3-axis accelerometer (ADXL345), a 3-axis magnetometer (HMC5883L), a 3-axis gyroscope (L3G4200D) and a barometric pressure sensor (BMP085). My plan is to build an Inertial Measurement Unit (IMU) (or maybe I should call it Attitude and heading reference system (AHRS)) and in the process learn how to interact and interpret the information all of this sensors provide. The fact is I have some experience using IMUs since I used one on my master thesis and another one on the Avora AUV, but the fact is they come preprogrammed and there is not much point in working with the raw sensor data unless you want to improve the measurement or give it another use.

For this project I am also using an Arduino Duemilanove, for that reason I wanted to call it ArduIMU, but there is already another project with the same name, so I will have to find another name (suggestions would be appreciated). Connecting the sensor board to the Arduino is pretty straightforward, every sensor has an I²C interface so you can access each of them using the Arduino Wire Library. The drawing was done using fritzing, on which I created the corresponding custom part for this board, although I did something wrong and it does not conform to the fritzing graphic standards.

This will be the first of a series of posts I plan to write about this project, since there are several steps I need to take in order to fully understand each sensor and several more to combine them in order to improve accuracy. In this post I want to talk about the accelerometer and how to obtain the roll and pitch angles from it, which is a process that can also be called tilt sensing.

Accelerometers are devices that are capable of measuring the acceleration they experience relative to free-fall,  the same acceleration living beings feel. As a consequence, accelerometers are incapable of measuring the acceleration of gravity, but can be used to measure the upwards acceleration that counters gravity when at rest. This acceleration is measured as $1g$ on the z-axis, when both pitch and roll angles are zero, but when the sensor is tilted either the x-axis or the y-axis experiences a component of the upward acceleration, whose magnitude depends on the tilt angle.

### Pitch & Roll estimation

Obtaining the pitch and roll angles is then a matter of being able to read the accelerometer, convert these readings to the g unit (1g = 9.8 m/s²), and apply the corresponding equations. The process of obtaining and converting the accelerometer readings depends on the accelerometer you are using, in my case, the ADXL345 in its basic configuration, provides 10-bit resolution for ±2g, but has several other ranges (±2g, ±4g, ±8g, ±16g)  and resolutions (from 10 to 13 bits depending on the range) . Generalizing, the formula used to calculate the acceleration from the accelerometer readings is:

$G_{Accel} = Raw_{Accel} \cdot \dfrac{Range}{2^{Resolution - 1}}$

Once we have the correct acceleration components, we can proceed to calculate the different angles using the following equations:

$pitch = \arctan{\left(\dfrac{G_y}{\sqrt{G_{x}^2 + G_{z}^2}}\right)}$     $roll =\arctan{\left( \dfrac{-G_x}{ G_{z}}\right)}$

For more information about where these equations come from, you can read the documentation I include at the end of this post. As you can see, the denominator of the pitch equation is defined to be always positive, so the equation itself only provides $[-90, 90]$ range, which is exactly what is expected for the pitch angle. In contrast, the roll equation provides $[-180, 180]$ range. It is important to take into account that when the pitch angle is 90º, the surge axis (roll) is directly aligned with the gravity vector, thus we cannot measure the roll angle anymore, this is what is called Gimbal Lock.

Also, be aware that the roll equation is undefined when both $G_x$ and $G_z$ are equal to zero, and that for each possible value of the calculation done inside the arctan function there are two valid solutions, not only on the roll but also on the pitch equation. These problems can be easily solved in code by using the function atan2, which eliminates the angle calculation ambiguity by taking into account the quadrant.

### Removing short-term fluctuations using a Low-Pass filter

At this point we already have a fully functional pitch & roll estimation system, but if we experiment with it we will discover that the readings fluctuate quite a bit and this may be very annoying for some applications. Removing these short-term fluctuations can be achieved by means of what is called a Low-Pass filter. This type of filter attenuates the higher frequencies of the signal, thus providing a smoother reading. The Low-Pass filter is easily implemented by using the following equation:

$y_{t} = \alpha \cdot x_{t} + (1 - \alpha) \cdot y_{t - 1}$

Where $y_t$ is our filtered signal, $y_{t-1}$ the previous filtered signal, $x_t$ the accelerometer reading and $\alpha$ the smoothing factor. It probably may seem obvious, but filtering should be done to the accelerometer readings before calculating the angles, instead of to the angles themselves. Regarding the smoothing factor, the lower we set it, the more it will take for the angle to stabilize, so we should not set it too low because then we could lose real-time behaviour. With this I mean that the reading will not correspond to the real angle until it stabilizes, and this could take some time.

### The source code & the ADXL345 library

I developed a small library to interface with the accelerometer, even though at the moment I have only implemented the basic functionality, I plan on supporting all of the device features. You can find it in my github account, where you can also find the processing code I used for the video example below. Thanks to the library, the code is pretty straightforward. It just reads the sensor accelerations which are already converted into gs by the library, applies the Low-Pass filter and then uses the roll and pitch equations to calculate the angles.

#include <Wire.h>

const float alpha = 0.5;

double fXg = 0;
double fYg = 0;
double fZg = 0;

void setup()
{
acc.begin();
Serial.begin(9600);
delay(100);
}

void loop()
{
double pitch, roll, Xg, Yg, Zg;

//Low Pass Filter
fXg = Xg * alpha + (fXg * (1.0 - alpha));
fYg = Yg * alpha + (fYg * (1.0 - alpha));
fZg = Zg * alpha + (fZg * (1.0 - alpha));

//Roll & Pitch Equations
roll  = (atan2(-fYg, fZg)*180.0)/M_PI;
pitch = (atan2(fXg, sqrt(fYg*fYg + fZg*fZg))*180.0)/M_PI;

Serial.print(pitch);
Serial.print(":");
Serial.println(roll);

delay(10);
}


### The result

For a more interactive visualization of the data, I also developed an example using processing, which consists on a rotating 3D cube. You can see the results in the following video.

In the next post about my Arduino IMU, I will talk about how gyroscopes work and how to interpret the information they provide.