File content as of revision 15:155ca63b7884:

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

// Class constructor
AttitudeEstimator::AttitudeEstimator() : imu(PC_9,PA_8)
{
    phi = 0.0f;
    theta = 0.0f;
    psi = 0.0f;
    p = 0.0f;
    q = 0.0f;
    r = 0.0f; 
    phi_accel = 0.0f;
    theta_accel = 0.0f; 
    phi_gyro = 0.0f;
    theta_gyro = 0.0f;
    psi_gyro = 0.0f;
    p_bias = 0.0f;
    q_bias = 0.0f;
    r_bias = 0.0f;  
}

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

// Calibrates gyroscope by calculating angular velocity bias (rad/s)
void AttitudeEstimator::calibrate_gyro()
{
    // Calculate angular velocities bias (rad/s) by averaging 600 samples during 2 seconds
    for(int i = 0; i<600;i++)
    {
        imu.read();
        p_bias += imu.gx/600.0f;
        q_bias += imu.gy/600.0f;
        r_bias += imu.gz/600.0f;
        wait(dt);
    }
}

// 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();
    // Ponderate Euler angles (rad) estimation from accelerometer and gyroscope
    phi = phi_accel*rho_att + phi_gyro*(1.0f-rho_att);
    theta = theta_accel*rho_att + theta_gyro*(1.0f-rho_att);
    psi = psi_gyro;
}

// 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>0)-(imu.az<0))*sqrt(pow(imu.ay,2)+pow(imu.az,2)));
}

// 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+q*sin(phi)*tan(theta)+r*cos(phi)*tan(theta))*dt;
    theta_gyro = theta + (q*cos(phi)-r*sin(phi))*dt;
    psi_gyro = psi + (q*sin(phi)/cos(theta)+r*cos(phi)/cos(theta))*dt;
    // Adjust Euler angles (rad) to be in the interval of +/- pi
    if(phi_gyro > PI)
    {
        phi_gyro = phi_gyro - 2*PI;
    }
    else if(phi_gyro < -PI)
    {
        phi_gyro = phi_gyro + 2*PI;
    }
    
    if(theta_gyro > PI)
    {
        theta_gyro = theta_gyro - 2*PI;
    }
    else if(theta_gyro < -PI)
    {
        theta_gyro = theta_gyro + 2*PI;
    }
    if(psi_gyro > PI)
    {
        psi_gyro = psi_gyro - 2*PI;
    }
    else if(psi_gyro < -PI)
    {
        psi_gyro = psi_gyro + 2*PI;
    }
}