CompFilter

void ComplementaryFilter(float *d, float *comp)
{
#define ACCELEROMETER_SENSITIVITY 8192.0
#define GYROSCOPE_SENSITIVITY 65.536
 
#define M_PI 3.14159265359      
 
#define dt 0.01                                             // 10 ms sample rate!    
    
    float pitchAcc, rollAcc;               
 
    // Integrate the gyroscope data -> int(angularSpeed) = angle
    *(comp+0) += (d[0] / GYROSCOPE_SENSITIVITY) * dt;      // Angle around the X-axis
    *(comp+1) -= (d[1] / GYROSCOPE_SENSITIVITY) * dt;      // Angle around the Y-axis
    
    // Compensate for drift with accelerometer data if in accepted range
    // Sensitivity = -2 to 2 G at 16Bit -> 2G = 32768 && 0.5G = 8192    //Change so sensitivity of +/- 4G
    int forceMagnitudeApprox = abs(d[3]) + abs(d[4]) + abs(d[5]);
    if (forceMagnitudeApprox > 8192 && forceMagnitudeApprox < 32768)
    {
    // Turning around the X axis results in a vector on the Y-axis
        pitchAcc = atan2f(d[4], d[5]) * 180 / M_PI;
        *(comp+0) = *(comp+0) * 0.98 + pitchAcc * 0.02;
 
    // Turning around the Y axis results in a vector on the X-axis
        rollAcc = atan2f(d[3], d[5]) * 180 / M_PI;
        *(comp+1) = *(comp+1) * 0.98 + rollAcc * 0.02;
    }
}