In the previous blog we setup the xtrinsic board to read accelerometer registers.
However the raw register values doesn't mean anything by itself. It needs to be converted to suitable human readable format to be useful in measurement applications.
In this guide we shall use a proper logic to calculate the elevation and bank, other wise called Pitch and Roll of xtrinsic mma8491q accelerometer.
A self-explanatory image of Pitch, Roll & Yaw.
Pitch:
Pitch is the measurement of elevation of a surface on which an accelerometer is mounted.
It helps to calculate the magnitude of ascent, descent and inclination against the 3-dimensional axis.
Roll:
This property helps to determine if the object is spinning/rotate around itself.
Check this : http://howthingsfly.si.edu/flight-dynamics/roll-pitch-and-yaw
What is g-value & how to find one ??
Earth's gravity by itself acts downwards.
In case of accelerometer the gravity is experienced on either/all of the axis when movement happens.
Below is the register bitmap from mma8491q data sheet.
According to above we will get 8bits in MSB & 6bits in LSB, so total 14bits to read for.
so max value that can be read is (2^14 -1) = 16383.
Any value obtained from the registers above is a signed value.
We will consider the values between 0 to 8191 is positive & ones between 8192 to 16383 is the negative (opposite direction.)
Differentiating the values will help us determine which direction the xtrinsic mma8491q is titled.
g-value during pitch
When xtrinsic is held flat on surface level with ground, gvalue across X-axis is near 0.
Increasing the tilt across X-axis, the head is pointing up, gvalue across X-axis increase from 0 to 1g.
Similarly, decreasing the tilt across X-axis, the head is pointing down, gvalue across X-axis decrease from 0 to -1g.
g-value during roll
When xtrinsic is held flat on surface level with ground, gvalue across Y-axis is near 0.
Lowering the board across Y-axis, the sideways goes down, g-value across Y-axis increase from 0 to 1g.
Similarly, Lowering the board in opposite direction on Y-axis, g-value across Y-axis decrease from 0 to -1g.
The pitch & roll values can be determined in 3 steps.
- convert register readings to g value.
- convert g-value to axis based angle.
- convert the measured angle to Pitch & Roll.
We will be using the math.h library to calculate the required values.
Convert register readings to g-value.
double register_to_gvalue(unsigned char msb,unsigned char lsb) //we get unsigned register values
{
signed int data1;
double data2;
data1 = (signed int)((msb<<6)|(lsb>>2)); //convert values to signed data
if (data1 > 16383/2 )
data1 = data1-16383;
data2 = (double)data1/1000.0; //convert signed data to g-value
return data2;
}
Convert g-value to angle
Next we need to convert the gvalue in double data-type to an angle against an axis.
we use arcsine function from math.h library to get this done.
double gvalue_to_angle(double gvalue) //we got the gvalue form previous calculation
{double data1;
data1 = asin(gvalue/1)*(180/PI); //convert gvalue to angle against an axis
return data1;
}
Convert the measured angle to Pitch & Roll
we use arctan2 and multiple the resulting value to get an approximate tilt value for Pitch & Roll.
pitch = (atan2(gvalue_x, sqrt(gvalue_y*gvalue_y + gvalue_z*gvalue_z))*180.0)/M_PI;
roll = (atan2(-gvalue_y, gvalue_z)*180.0)/M_PI;
The complete code for the above setup :
#include <stdio.h>
#include <stdint.h>
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
#include <fcntl.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <string.h>
#include <math.h>
#define I2CFILE "/dev/i2c-3"
#define PI 3.14159
static int write_register(int file,unsigned char address,unsigned char reg,unsigned char data)
{
unsigned char output_buffer[2];
struct i2c_rdwr_ioctl_data packets;
struct i2c_msg messages[1];
messages[0].addr = address;
messages[0].flags = 0;
messages[0].len = sizeof(output_buffer);
messages[0].buf = output_buffer;
output_buffer[0] = reg;
output_buffer[1] = data;
packets.msgs = messages;
packets.nmsgs = 1;
if(ioctl(file, I2C_RDWR, &packets) < 0) {
perror("Error sending data");
return 1;
}
return 0;
}
static int read_register(int file, unsigned char address, unsigned char reg, unsigned char *data)
{
unsigned char input_buffer, output_buffer;
struct i2c_rdwr_ioctl_data packets;
struct i2c_msg messages[2];
output_buffer = reg;
messages[0].addr = address;
messages[0].flags = 0;
messages[0].len = sizeof(output_buffer);
messages[0].buf = &output_buffer;
messages[1].addr = address;
messages[1].flags = I2C_M_RD;
messages[1].len = sizeof(input_buffer);
messages[1].buf = &input_buffer;
packets.msgs = messages;
packets.nmsgs = 2;
if(ioctl(file, I2C_RDWR, &packets) < 0) {
perror("Error Receiving data");
return 1;
}
*data = input_buffer;
return 0;
}
double gvalue_to_angle(double gvalue)
{
double data1;
data1 = asin(gvalue/1)*(180/PI);
return data1;
}
double register_to_gvalue(unsigned char msb,unsigned char lsb)
{
signed int data1;
double data2;
data1 = (signed int)((msb<<6)|(lsb>>2)); //convert
if (data1 > 16383/2 )
data1 = data1-16383;
data2 = (double)data1/1000.0;
return data2;
}
int main() {
int file, i;
unsigned char data_m;
unsigned char data_l;
double gvalue_x;
double gvalue_y;
double gvalue_z;
double angle_x;
double angle_y;
double angle_z;
double pitch;
double roll;
if ((file = open(I2CFILE, O_RDWR)) < 0)
{
perror("Error openning file!");
exit(1);
}
else
printf("%s opened\n",I2CFILE);
while(1)
{
//accel status
if(read_register(file, 0x55, 0x00, &data_m))
exit(1);
else
printf("\n\n\nAcc Status: [0x%02X]: 0x%02X\n" , 00 , data_m);
//read accel MSB/LSB - X axis
if(read_register(file, 0x55, 0x01, &data_m))
exit(1);
if(read_register(file, 0x55, 0x02, &data_l))
exit(1);
/* printf("X-Register : 0x%02X:0x%02X\n" , data_m, data_l);*/
gvalue_x = register_to_gvalue(data_m,data_l);
printf("\nX-gvalue : %.02lfg\n" , gvalue_x);
angle_x = gvalue_to_angle(gvalue_x);
printf("X-angle : %.02lf\n\n" , angle_x);
//read accel MSB/LSB - Y axis
if(read_register(file, 0x55, 0x03, &data_m))
exit(1);
if(read_register(file, 0x55, 0x04, &data_l))
exit(1);
/* printf("Y-Register : 0x%02X:0x%02X\n" , data_m, data_l);*/
gvalue_y = register_to_gvalue(data_m,data_l);
printf("Y-gvalue : %.02lfg\n" , gvalue_y);
angle_y = gvalue_to_angle(gvalue_y);
printf("Y-angle : %.02lf\n\n" , angle_y);
//read accel MSB/LSB - Z axis
if(read_register(file, 0x55, 0x05, &data_m))
exit(1);
if(read_register(file, 0x55, 0x06, &data_l))
exit(1);
/* printf("Z-Register : 0x%02X:0x%02X\n" , data_m, data_l);*/
gvalue_z = register_to_gvalue(data_m,data_l);
printf("Z-gvalue : %.02lfg\n" , gvalue_z);
angle_z = gvalue_to_angle(gvalue_z);
printf("Z-angle : %.02lf\n\n" , angle_z);
pitch = (atan2(gvalue_x, sqrt(gvalue_y*gvalue_y + gvalue_z*gvalue_z))*180.0)/M_PI;
roll = (atan2(-gvalue_y, gvalue_z)*180.0)/M_PI;
printf("pitch :%lf:\n", pitch);
printf("roll :%lf:\n", roll);
system("echo out > /sys/class/gpio/gpio112/direction");
system("echo 1 > /sys/class/gpio/gpio112/value");
usleep(1000000);
}
close(file);
return 0;
}
Start the Application, you will get something similar as below:
I got the below readings with board resting on table.
In the Next blog we shall detect Accelerometer tilts & configure GPIO pins for same.
