Click here to Skip to main content
15,884,629 members
Please Sign up or sign in to vote.
0.00/5 (No votes)
See more:
C++
Data.Roll -= ((float)Data.Gyro_Raw[0] / GYROSCOPE_SENSITIVITY) * dt;    // Angle around the Y-axis

Data.Pitch += ((float)Data.Gyro_Raw[1] / GYROSCOPE_SENSITIVITY) * dt; // Angle around the X-axis

Data.Yaw += ((float)Data.Gyro_Raw[2] / GYROSCOPE_SENSITIVITY) * dt;

									 
roll =     sqrt(((float)Data.Accel_Raw[0]*(float)Data.Accel_Raw[0])+((float)Data.Accel_Raw[2]*(float)Data.Accel_Raw[2]));

pitch = sqrt(((float)Data.Accel_Raw[1]*(float)Data.Accel_Raw[1])+((float)Data.Accel_Raw[2]*(float)Data.Accel_Raw[2]));

yaw = sqrt(((float)Data.Accel_Raw[0]*(float)Data.Accel_Raw[0])+((float)Data.Accel_Raw[1]*(float)Data.Accel_Raw[1]));
									
int forceMagnitudeApprox = abs(Data.Accel_Raw[0]) + abs(Data.Accel_Raw[1]) + abs(Data.Accel_Raw[2]);
if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
	{

// Turning around the Y axis results in a vector on the X-axis
rollAcc = atan2f((float)Data.Accel_Raw[1], roll) * 180 / M_PI;
Data.Roll = (Data.Roll) * 0.98f + rollAcc * 0.02f;
cout <<Data.Roll<<"\t";
				
// Turning around the X axis results in a vector on the Y-axis
pitchAcc = atan2f((float)Data.Accel_Raw[0], pitch) * 180 / M_PI;
Data.Pitch = (Data.Pitch) * 0.98f + pitchAcc * 0.02f;
cout <<Data.Pitch<<"\t";

yawAcc = atan2f(yaw, (float)Data.Accel_Raw[2]) * 180 / M_PI;
Data.Yaw = (Data.Yaw) * 0.98f + yawAcc * 0.02f;
cout <<Data.Yaw<<"\t\n";
				
}


Accelerometer sensor has sensitivity 1 mg/LSB with +/- 2g
Posted
Updated 2-Apr-14 23:04pm
v3
Comments
Rage 3-Apr-14 5:05am    
What is not working ? What have debugged ?
Pervez03 6-Apr-14 12:05pm    
Roll pitch Yaw FilRoll FilPitch FilYaw
23 6 7 19 6 7
24 7 11 18.5 6 8
25 9 16 19 7 10


it increases the yaw value ...not in a range like other ...

This content, along with any associated source code and files, is licensed under The Code Project Open License (CPOL)

  Print Answers RSS
Top Experts
Last 24hrsThis month


CodeProject, 20 Bay Street, 11th Floor Toronto, Ontario, Canada M5J 2N8 +1 (416) 849-8900