controlador de atitude
AttitudeEstimator/AttitudeEstimator.cpp
- Committer:
- yvesyuzo
- Date:
- 2018-10-10
- Revision:
- 1:579511e9f0b8
- Child:
- 8:c96125e9ac70
File content as of revision 1:579511e9f0b8:
#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; }