Hi everyone,
Present i am working with BMX160, Till i am able to read Three sensor's raw data(Acc, Gyro, Mag) and convert them into Gravity, Radiuns/sec,uT respectively and To converting these readings iinto roll, pitch, yaw i am using below section of code(i got this from google)like below.
#define ACCELEROMETER_SENSITIVITY 16384.0
#define GYROSCOPE_SENSITIVITY 16.4 //from bmx160 datasheet pg no:9,it is the value form range FS2000
#define M_PI 3.14159265359
#define dt 0.01 // 10 ms sample rate!
void ComplementaryFilter(int16_t *accData, int16_t *gyrData, float *pitch, float *roll)
{
float pitchAcc, rollAcc, yaw = 0.0;
float *yaw_ptr;
// Integrate the gyroscope data -> int(angularSpeed) = angle
*pitch = ( (float)gyro.x / GYROSCOPE_SENSITIVITY ) * dt; // Angle around the X-axis
*roll = ( (float)gyro.y / GYROSCOPE_SENSITIVITY ) * dt; // Angle around the Y-axis
yaw = yaw + ((float)gyro.z) * dt; // Angle around the Z-axis
yaw_ptr = &yaw;
// Compensate for drift with accelerometer data if !bull**bleep**
// Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192
int forceMagnitudeApprox = abs(accel.x) + abs(accel.y) + abs(accel.z);
if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
{
// Turning around the X axis results in a vector on the Y-axis
pitchAcc = atan2f((float)accel.y, (float)accel.z) * 180 / M_PI;
*pitch = *pitch * 0.98 + pitchAcc * 0.02;
// Turning around the Y axis results in a vector on the X-axis
rollAcc = atan2f((float)accel.x, (float)accel.z) * 180 / M_PI;
*roll = *roll * 0.98 + rollAcc * 0.02;
}
PRINTF("pitch\t%0.5f\troll\t%0.5f\tyaw\t%0.5f\t\r\n", *pitch,*roll, yaw);
}
Can you please review it once.
If it is wrong to proceede so please correct me and suggeat me right formula for converting it.
Thank you inadvance,
amarr