Estimating orientation from acceleration

Three-axis accelerometers measure the linear acceleration of a body. However, you can calculate the roll and pitch angle when you know the acceleration along each axis thanks to trigonometry.

Basic idea

The force of gravity always acts perpendicular to the earth’s surface. When an object is tilted at an angle, part of that force acts along the X axis of the object, and part acts along the Y axis.

Let’s consider the following problem in which the acceleration is produced by the gravitational force that the earth exerts on the body. The sensor is tilted

Place the sensor with the X axis oriented upward and press ‘xm’. The analog value will be saved.
Place the sensor with the X axis oriented downward and press ‘xM’.
Place the sensor with the Y axis oriented upward and press ‘ym’.
Place the sensor with the Y axis oriented downward and press ‘yM’.
Place the sensor with the Z axis oriented upward and press ‘zm’.
Place the sensor with the Z axis oriented downward and press ‘zM’.
Press ‘v’ to show the Min and max values for each axes.

We will discuss the main functions of this sketch. Let’s have a look at the code.

Scaling raw values

The analog values are read from 3 analog pins on the Arduino board. The Analog to Digital Converter has a resolution of 0.049 Volts/Unit. It maps input voltages between 0 and 5 volts into values between 0 and 1023. Let’s create a scaleAccs() function that maps the analog values to the corresponding number of G.

  float xScaled = map(xRead, xRawMin, xRawMax, -1000, 1000);
  float yScaled = map(yRead, yRawMin, yRawMax, -1000, 1000);
  float zScaled = map(zRead, zRawMin, zRawMax, -1000, 1000);

Calibrating the sensor

We can control our program using the serial interface. We would like to store the raw value when the sensor measures one G of acceleration. Using the Serial.read() function we can associate actions to key pressed. The following code reads the char in the serial buffer and saves the current analog value received from the sensor to a variable that will be used for calibration purposes.

if (Serial.available())
{
   char t = Serial.read();
   Serial.println(t);
   if (t == 'x')
   {   
     char sec = Serial.read();
     if (sec == 'm')
     {
      xRawMin = xRead;
     }
     else if (sec == 'M')
     {
       xRawMax = xRead;
     }
   }
}

Complete Arduino code

So our sketch will be composed by three main functions.
As before we need some code that reads the analog values from the sensor. Then a Serial routine will wait for the right combination of keys to be pressed to store the reference values used for the calibration. And finally the scale function will be called to convert the raw values to acceleration ones.

//Analog read pins
const int xPin = 0;
const int yPin = 1;
const int zPin = 2;

int xRead,yRead,zRead;

//to hold the caculated values
double x;
double y;
double z;

float xRaw,yRaw,zRaw;  
float xAccel,yAccel,zAccel;

int xRawMin = 402;
int xRawMax = 267;
 
int yRawMin = 397;
int yRawMax = 263;
 
int zRawMin = 413;
int zRawMax = 281;


// angle estimated from kalman
float angleXAcc;
float angleYAcc;

boolean processing = false;
boolean printAngles = false;

void setup()
{
  Serial.begin(9600);
  Serial.println("Occhio");
  pinMode(zPin,INPUT); 
  printCalVals();
}

void loop()
{
  //read the analog values from the accelerometer
  readAcc();
  serialRoutine();
  
  scaleAccs();
  
  printVals();

  delay(500);
}

void scaleAccs()
{
  float xScaled = map(xRead, xRawMin, xRawMax, -1000, 1000);
  float yScaled = map(yRead, yRawMin, yRawMax, -1000, 1000);
  float zScaled = map(zRead, zRawMin, zRawMax, -1000, 1000);
  
  // re-scale to fractional Gs
  xAccel = xScaled / 1000.0;
  yAccel = yScaled / 1000.0;
  zAccel = zScaled / 1000.0; 
}

void readAcc()
{
  xRead = analogRead(xPin);
  yRead = analogRead(yPin);
  zRead = analogRead(zPin); 
}

void printVals()
{
  Serial.print("xG: ");
  Serial.print(xAccel);
  Serial.print(" | yG: ");
  Serial.print(yAccel);
  Serial.print(" | zG: ");
  Serial.print(zAccel);
  Serial.print("      X: ");
  Serial.print(xRead);
  Serial.print(" | Y: ");
  Serial.print(yRead);
  Serial.print(" | Z: ");
  Serial.print(zRead);
  Serial.println();
}

void printCalVals()
{
  Serial.print("X _ min max: ");
  Serial.print(xRawMin);
  Serial.print(" : ");
  Serial.println(xRawMax);
  Serial.print("Y _ min max: ");
  Serial.print(yRawMin);
  Serial.print(" : ");
  Serial.println(yRawMax);
  Serial.print("Z _ min max: ");
  Serial.print(zRawMin);
  Serial.print(" : ");
  Serial.println(zRawMax);  
}


void serialRoutine()
{
 if (Serial.available())
 {
   char t = Serial.read();
   Serial.println(t);
   if (t == 'x')
   {   
     char sec = Serial.read();
     if (sec == 'm')
     {
      xRawMin = xRead;
       Serial.print("Min X: ");
       Serial.println(xRawMin);
     }
     else if (sec == 'M')
     {
       xRawMax = xRead;
       Serial.print("Max X: ");
       Serial.println(xRawMax);
     }
   }
   else if (t == 'y')
   {   
     char sec = Serial.read();
     if (sec == 'm')
     {
      yRawMin = yRead;
       Serial.print("Min Y: ");
       Serial.println(yRawMin);
     }
     else if (sec == 'M')
     {
       yRawMax = yRead;
       Serial.print("Max Y: ");
       Serial.println(yRawMax);
     }
   }
   else if (t == 'z')
   {   
     char sec = Serial.read();
     if (sec == 'm')
     {
      zRawMin = zRead;
       Serial.print("Min Z: ");
       Serial.println(zRawMin);
     }
     else if (sec == 'M')
     {
       zRawMax = zRead;
       Serial.print("Max Z: ");
       Serial.println(zRawMax);
     }
   }
   else if (t=='v')
   {
     Serial.print("Min x: ");
     Serial.println(xRawMin);
     Serial.print("Max x: ");
     Serial.println(xRawMax);
     Serial.print("Min Y: ");
     Serial.println(yRawMin);
     Serial.print("Max Y: ");
     Serial.println(yRawMax);
     Serial.print("Min Z: ");
     Serial.println(zRawMin);
     Serial.print("Max Z: ");
     Serial.println(zRawMax);
   }
 } 
}

Adxl335 Basics