File content as of revision 8:1ad52489f6f3:

#include "mbed.h"
#include "AttitudeEstimator.h"
#include "Library.h"


// Class constructor
AttitudeEstimator :: AttitudeEstimator () : imu(PC_9 , PA_8 )
{
    
}
// Initialize class
void AttitudeEstimator::init()
{
    imu.init();
    calibrate_gyro();
    phi = 0;
    theta = 0;
    psi = 0;
}
// Calibrates gyroscope by calculating angular velocity bias (rad/s)
void AttitudeEstimator::calibrate_gyro()
{
    int taxa = 200; //taxa em Hz de leitura
    int count = 0;
    p_bias = 0.0f;
    q_bias = 0.0f;
    r_bias = 0.0f;

    for(count = 1; count <= taxa; ++count) 
    {
        imu.read();
        p_bias += imu.gx/200.0f;
        q_bias += imu.gy/200.0f;
        r_bias += imu.gz/200.0f;
        wait(0.005);
    }
}

// Estimate Euler angles (rad ) from accelerometer data
void AttitudeEstimator::estimate_accel ()
{
    
    phi_accel = atan2(-imu.ay,-imu.az);    
    if (imu.az!=0) 
    {
        theta_accel = atan2( imu.ax,( -(imu.az/abs(imu.az))* sqrt(pow(imu.ay,2) + pow(imu.az,2))  ) );
    }
    else 
    {
        theta_accel = atan2( imu.ax,(- sqrt(pow(imu.ay,2) + pow (imu.az,2)) ) );
    }
    
    //theta_accel = atan2(imu.gx,((imu.gz/(abs(imu.gz)+0.00001)*(pow(imu.gy, 2)+pow(imu.gz, 2)))

}

// Estimate Euler angles (rad ) and angular velocities ( rad /s) from gyroscope data
void AttitudeEstimator::estimate_gyro ()
{
    p = imu.gx - p_bias;
    q = imu.gy - q_bias;
    r = imu.gz - r_bias;

    phi_gyro = phi + (p + (sin(phi)*tan(theta))*q + (cos(phi)*tan(theta))*r)*dt;
    if (phi_gyro < -pi) {
        phi_gyro += 2*pi;
    }
    if (phi_gyro > pi) {
        phi_gyro -= 2*pi;
    }   
    
    theta_gyro = theta + (q*(cos(phi)) - (sin(phi))*r)*dt;
    if (theta_gyro < -pi) {
        theta_gyro += 2*pi;
    }
    if (theta_gyro > pi) {
        theta_gyro -= 2*pi ;
    }
    
    psi_gyro   = psi + (sin(phi)*(1/cos(theta))*q + cos(phi)*(1/cos(theta))*r)*dt;
    if (psi_gyro < -pi) {
        psi_gyro += 2*pi;
    }
    if (psi_gyro > pi) {
        psi_gyro -= 2*pi;
    }    
}                        

// Estimate euler angles (rad ) and angular velocity (rad/s)
void AttitudeEstimator::estimate ()
{
    imu.read();
    estimate_accel();
    estimate_gyro();
    phi = rho*phi_accel + (1-rho)*phi_gyro;
    theta = rho*theta_accel + (1-rho)*theta_gyro;
    psi = psi_gyro;
}