File content as of revision 0:b1f2c9e88e32:

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

// Class constructor
AttitudeEstimator::AttitudeEstimator() : imu(PC_9,PA_8)
{
}

// Initialize class 
void AttitudeEstimator::init()
{
    // Initialize IMU sensor object
    imu.init();
    // Calibrate gyroscope
    calibrate_gyro();
}

// Estimate euler angles (rad) and angular velocity (rad/s)
void AttitudeEstimator::estimate()
{
    // Read IMU sensor data
    imu.read();
    // Estimate Euler angles (rad) and angular velocities (rad/s) from accelerometer and gyroscope data
    estimate_accel();
    estimate_gyro();
    // 
    phi = phi_accel*rho + phi_gyro*(1.0f-rho);
    theta = theta_accel*rho + theta_gyro*(1.0f-rho);
}

// Estimate Euler angles (rad) from accelerometer data
void AttitudeEstimator::estimate_accel()
{
    // Estimate Euler angles (rad)
    phi_accel = atan2(-imu.ay,-imu.az);
    theta_accel = atan2(imu.ax,-imu.az);
}

// Estimate Euler angles (rad) and angular velocities (rad/s) from gyroscope data
void AttitudeEstimator::estimate_gyro()
{
    // Estimate angular velocities (rad/s)
    p = imu.gx - p_bias;
    q = imu.gy - q_bias;
    r = imu.gz - r_bias;
    // Estimate Euler angles (rad)
    phi_gyro = phi + p*dt;
    theta_gyro = theta + q*dt;
    psi = psi + r*dt;
}

// Calibrates gyroscope by calculating angular velocity bias (rad/s)
void AttitudeEstimator::calibrate_gyro()
{
    // Reser angular velocites bias (rad/s)
    p_bias = 0.0f;
    q_bias = 0.0f;
    r_bias = 0.0f;
    // Calculate angular velocities bias (rad/s) by averaging 200 samples in 1 second
    for(int i = 0; i<200;i++)
    {
        imu.read();
        p_bias += imu.gx/200.0f;
        q_bias += imu.gy/200.0f;
        r_bias += imu.gz/200.0f;
        wait(0.005);
    }
}