how to calculate angle using accelerometer

[irangathathsara]

i am using pololu MMA7361L accelerometer and i used the following code in mikroC. ADC values changes with rotation but the angle output of the program is zero. Can any body help me to sort this up. Thanx

int Acc_X;
int Acc_Y;
int Acc_Z;
int Acc_REF;
int diff_X;
int diff_Y;
int diff_Z;
double acceleration_X;
double acceleration_Y;
double acceleration_Z;
double force_vector;
double rotate_X;
double rotate_Y;
double rotate_Z;


double force_vector_calculation()
{
acceleration_X= Acc_X*0.001;
acceleration_Y= Acc_Y*0.001;
acceleration_Z= Acc_Z*0.001;

force_vector=sqrt(pow(acceleration_X,2)+pow(acceleration_Y,2)+pow(acceleration_Z,2));

return force_vector;
}

int angle_X()
{

rotate_X=acos(acceleration_X/force_vector);
return rotate_X;
}

int angle_Y()
{

rotate_Y=acos(acceleration_Y/force_vector);
return rotate_Y;
}

int angle_Z()
{

rotate_Z=acos(acceleration_Z/force_vector);
return rotate_Z;
}


void main()
{
trisa=0xff;
ADCON1=0b00000001;
UsART_Init(9600);
while(1)
{
Acc_X=Adc_read(1);
Acc_Y=Adc_read(2);
Acc_Z=Adc_read(3);
Acc_REF=Adc_Read(4);


delay_ms(5);
force_vector_calculation();
angle_X();
angle_Y();
angle_Z();


Usart_Write( rotate_X);
Usart_Write( rotate_Y);
Usart_Write( rotate_Z);
}

 

[doraemon]

Hello!

First for your question: try to follow with the debugger what's going on.
We cannot do that for you.
Then, is this a microcontroller algorithm?
In this case, you should try to avoid float (or double) calculations.
One integer multiplication takes only a few clock cycles, but a double
multiplication takes a few hundreds.
Also you should avoid sqrt, acos, etc...
Hint: use lookup tables!

Dora.

 

[enjunear]

Much agreed with Dora; lookup tables are the way to go. Generally, accelerometer vendors will supply an app note that talks about how to set them up, or even supply the table values, and possibly an equation that does a rough curve-fit for your particular device.

At all costs, avoid doing complex math functions on your microcontroller... it'll take FOREVER, and simply induce long delays between samples (slowing down your sampling rate, and reducing your bandwidth... very bad if using this in a feedback control loop).

 

[irangathathsara]

Thanx enjunear and doraemon for ur quick reply.
i want to know whether the algorythm i used to calculate angle from accelerometer is correct?

 

[FvM]

i want to know whether the algorythm i used to calculate angle from accelerometer is correct?

I don't see an obvious fault. As suggested, check the intermediate results. There may be problems with the math library, it's also unclear, what Usart_Write() exactly does.

My comment on the considerations about not using float calculations and similar: Depends on. Nothing has been said about speed requirements yet.

 

[irangathathsara]

Thanx FvM, Usart_Write is send data to computer via RS232 serial communication. I don't understand what you mean by speed requirements.

 

[FvM]

I don't understand what you mean by speed requirements.

I was referring to the comments about slow calculation of float and complex math functions.