AttitudeEstimator.cpp

00001 #include "mbed.h"
00002 #include "AttitudeEstimator.h"
00003 #include "Library.h"
00004 
00005 
00006 // Class constructor
00007 AttitudeEstimator :: AttitudeEstimator () : imu(PC_9 , PA_8 )
00008 {
00009     
00010 }
00011 // Initialize class
00012 void AttitudeEstimator::init()
00013 {
00014     imu.init();
00015     calibrate_gyro();
00016     phi = 0;
00017     theta = 0;
00018     psi = 0;
00019 }
00020 // Calibrates gyroscope by calculating angular velocity bias (rad/s)
00021 void AttitudeEstimator::calibrate_gyro()
00022 {
00023     int taxa = 200; //taxa em Hz de leitura
00024     int count = 0;
00025     p_bias = 0.0f;
00026     q_bias = 0.0f;
00027     r_bias = 0.0f;
00028 
00029     for(count = 1; count <= taxa; ++count) 
00030     {
00031         imu.read();
00032         p_bias += imu.gx/200.0f;
00033         q_bias += imu.gy/200.0f;
00034         r_bias += imu.gz/200.0f;
00035         wait(0.005);
00036     }
00037 }
00038 
00039 // Estimate Euler angles (rad ) from accelerometer data
00040 void AttitudeEstimator::estimate_accel ()
00041 {
00042     
00043     phi_accel = atan2(-imu.ay,-imu.az);    
00044     if (imu.az!=0) 
00045     {
00046         theta_accel = atan2( imu.ax,( -(imu.az/abs(imu.az))* sqrt(pow(imu.ay,2) + pow(imu.az,2))  ) );
00047     }
00048     else 
00049     {
00050         theta_accel = atan2( imu.ax,(- sqrt(pow(imu.ay,2) + pow (imu.az,2)) ) );
00051     }
00052     
00053     //theta_accel = atan2(imu.gx,((imu.gz/(abs(imu.gz)+0.00001)*(pow(imu.gy, 2)+pow(imu.gz, 2)))
00054 
00055 }
00056 
00057 // Estimate Euler angles (rad ) and angular velocities ( rad /s) from gyroscope data
00058 void AttitudeEstimator::estimate_gyro ()
00059 {
00060     p = imu.gx - p_bias;
00061     q = imu.gy - q_bias;
00062     r = imu.gz - r_bias;
00063 
00064     phi_gyro = phi + (p + (sin(phi)*tan(theta))*q + (cos(phi)*tan(theta))*r)*dt;
00065     if (phi_gyro < -pi) {
00066         phi_gyro += 2*pi;
00067     }
00068     if (phi_gyro > pi) {
00069         phi_gyro -= 2*pi;
00070     }   
00071     
00072     theta_gyro = theta + (q*(cos(phi)) - (sin(phi))*r)*dt;
00073     if (theta_gyro < -pi) {
00074         theta_gyro += 2*pi;
00075     }
00076     if (theta_gyro > pi) {
00077         theta_gyro -= 2*pi ;
00078     }
00079     
00080     psi_gyro   = psi + (sin(phi)*(1/cos(theta))*q + cos(phi)*(1/cos(theta))*r)*dt;
00081     if (psi_gyro < -pi) {
00082         psi_gyro += 2*pi;
00083     }
00084     if (psi_gyro > pi) {
00085         psi_gyro -= 2*pi;
00086     }    
00087 }                        
00088 
00089 // Estimate euler angles (rad ) and angular velocity (rad/s)
00090 void AttitudeEstimator::estimate ()
00091 {
00092     imu.read();
00093     estimate_accel();
00094     estimate_gyro();
00095     phi = rho*phi_accel + (1-rho)*phi_gyro;
00096     theta = rho*theta_accel + (1-rho)*theta_gyro;
00097     psi = psi_gyro;
00098 }