Yuri De Stefani / CrazyflieController_final

programa final

AttitudeEstimator/AttitudeEstimator.cpp@8:1ad52489f6f3, 2018-11-30 (annotated)

Committer:
yurindes
Date:
Fri Nov 30 19:23:29 2018 +0000
Branch:
yuri
Revision:
8:1ad52489f6f3
Parent:
1:579511e9f0b8 
final;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yvesyuzo 1:579511e9f0b8 1 #include "mbed.h"
yvesyuzo 1:579511e9f0b8 2 #include "AttitudeEstimator.h"
yvesyuzo 1:579511e9f0b8 3 #include "Library.h"
yvesyuzo 1:579511e9f0b8 4
yvesyuzo 1:579511e9f0b8 5
yvesyuzo 1:579511e9f0b8 6 // Class constructor
yvesyuzo 1:579511e9f0b8 7 AttitudeEstimator :: AttitudeEstimator () : imu(PC_9 , PA_8 )
yvesyuzo 1:579511e9f0b8 8 {
yvesyuzo 1:579511e9f0b8 9
yvesyuzo 1:579511e9f0b8 10 }
yvesyuzo 1:579511e9f0b8 11 // Initialize class
yvesyuzo 1:579511e9f0b8 12 void AttitudeEstimator::init()
yvesyuzo 1:579511e9f0b8 13 {
yvesyuzo 1:579511e9f0b8 14 imu.init();
yvesyuzo 1:579511e9f0b8 15 calibrate_gyro();
yvesyuzo 1:579511e9f0b8 16 phi = 0;
yvesyuzo 1:579511e9f0b8 17 theta = 0;
yvesyuzo 1:579511e9f0b8 18 psi = 0;
yvesyuzo 1:579511e9f0b8 19 }
yvesyuzo 1:579511e9f0b8 20 // Calibrates gyroscope by calculating angular velocity bias (rad/s)
yvesyuzo 1:579511e9f0b8 21 void AttitudeEstimator::calibrate_gyro()
yvesyuzo 1:579511e9f0b8 22 {
yvesyuzo 1:579511e9f0b8 23 int taxa = 200; //taxa em Hz de leitura
yvesyuzo 1:579511e9f0b8 24 int count = 0;
yvesyuzo 1:579511e9f0b8 25 p_bias = 0.0f;
yvesyuzo 1:579511e9f0b8 26 q_bias = 0.0f;
yvesyuzo 1:579511e9f0b8 27 r_bias = 0.0f;
yvesyuzo 1:579511e9f0b8 28
yvesyuzo 1:579511e9f0b8 29 for(count = 1; count <= taxa; ++count)
yvesyuzo 1:579511e9f0b8 30 {
yvesyuzo 1:579511e9f0b8 31 imu.read();
yvesyuzo 1:579511e9f0b8 32 p_bias += imu.gx/200.0f;
yvesyuzo 1:579511e9f0b8 33 q_bias += imu.gy/200.0f;
yvesyuzo 1:579511e9f0b8 34 r_bias += imu.gz/200.0f;
yvesyuzo 1:579511e9f0b8 35 wait(0.005);
yvesyuzo 1:579511e9f0b8 36 }
yvesyuzo 1:579511e9f0b8 37 }
yvesyuzo 1:579511e9f0b8 38
yvesyuzo 1:579511e9f0b8 39 // Estimate Euler angles (rad ) from accelerometer data
yvesyuzo 1:579511e9f0b8 40 void AttitudeEstimator::estimate_accel ()
yvesyuzo 1:579511e9f0b8 41 {
yvesyuzo 1:579511e9f0b8 42
yvesyuzo 1:579511e9f0b8 43 phi_accel = atan2(-imu.ay,-imu.az);
yvesyuzo 1:579511e9f0b8 44 if (imu.az!=0)
yvesyuzo 1:579511e9f0b8 45 {
yvesyuzo 1:579511e9f0b8 46 theta_accel = atan2( imu.ax,( -(imu.az/abs(imu.az))* sqrt(pow(imu.ay,2) + pow(imu.az,2)) ) );
yvesyuzo 1:579511e9f0b8 47 }
yvesyuzo 1:579511e9f0b8 48 else
yvesyuzo 1:579511e9f0b8 49 {
yvesyuzo 1:579511e9f0b8 50 theta_accel = atan2( imu.ax,(- sqrt(pow(imu.ay,2) + pow (imu.az,2)) ) );
yvesyuzo 1:579511e9f0b8 51 }
yvesyuzo 1:579511e9f0b8 52
yvesyuzo 1:579511e9f0b8 53 //theta_accel = atan2(imu.gx,((imu.gz/(abs(imu.gz)+0.00001)*(pow(imu.gy, 2)+pow(imu.gz, 2)))
yvesyuzo 1:579511e9f0b8 54
yvesyuzo 1:579511e9f0b8 55 }
yvesyuzo 1:579511e9f0b8 56
yvesyuzo 1:579511e9f0b8 57 // Estimate Euler angles (rad ) and angular velocities ( rad /s) from gyroscope data
yvesyuzo 1:579511e9f0b8 58 void AttitudeEstimator::estimate_gyro ()
yvesyuzo 1:579511e9f0b8 59 {
yvesyuzo 1:579511e9f0b8 60 p = imu.gx - p_bias;
yvesyuzo 1:579511e9f0b8 61 q = imu.gy - q_bias;
yvesyuzo 1:579511e9f0b8 62 r = imu.gz - r_bias;
yvesyuzo 1:579511e9f0b8 63
yvesyuzo 1:579511e9f0b8 64 phi_gyro = phi + (p + (sin(phi)*tan(theta))*q + (cos(phi)*tan(theta))*r)*dt;
yvesyuzo 1:579511e9f0b8 65 if (phi_gyro < -pi) {
yvesyuzo 1:579511e9f0b8 66 phi_gyro += 2*pi;
yvesyuzo 1:579511e9f0b8 67 }
yvesyuzo 1:579511e9f0b8 68 if (phi_gyro > pi) {
yvesyuzo 1:579511e9f0b8 69 phi_gyro -= 2*pi;
yvesyuzo 1:579511e9f0b8 70 }
yvesyuzo 1:579511e9f0b8 71
yvesyuzo 1:579511e9f0b8 72 theta_gyro = theta + (q*(cos(phi)) - (sin(phi))*r)*dt;
yvesyuzo 1:579511e9f0b8 73 if (theta_gyro < -pi) {
yvesyuzo 1:579511e9f0b8 74 theta_gyro += 2*pi;
yvesyuzo 1:579511e9f0b8 75 }
yvesyuzo 1:579511e9f0b8 76 if (theta_gyro > pi) {
yvesyuzo 1:579511e9f0b8 77 theta_gyro -= 2*pi ;
yvesyuzo 1:579511e9f0b8 78 }
yvesyuzo 1:579511e9f0b8 79
yvesyuzo 1:579511e9f0b8 80 psi_gyro = psi + (sin(phi)*(1/cos(theta))*q + cos(phi)*(1/cos(theta))*r)*dt;
yvesyuzo 1:579511e9f0b8 81 if (psi_gyro < -pi) {
yvesyuzo 1:579511e9f0b8 82 psi_gyro += 2*pi;
yvesyuzo 1:579511e9f0b8 83 }
yvesyuzo 1:579511e9f0b8 84 if (psi_gyro > pi) {
yvesyuzo 1:579511e9f0b8 85 psi_gyro -= 2*pi;
yvesyuzo 1:579511e9f0b8 86 }
yvesyuzo 1:579511e9f0b8 87 }
yvesyuzo 1:579511e9f0b8 88
yvesyuzo 1:579511e9f0b8 89 // Estimate euler angles (rad ) and angular velocity (rad/s)
yvesyuzo 1:579511e9f0b8 90 void AttitudeEstimator::estimate ()
yvesyuzo 1:579511e9f0b8 91 {
yvesyuzo 1:579511e9f0b8 92 imu.read();
yvesyuzo 1:579511e9f0b8 93 estimate_accel();
yvesyuzo 1:579511e9f0b8 94 estimate_gyro();
yvesyuzo 1:579511e9f0b8 95 phi = rho*phi_accel + (1-rho)*phi_gyro;
yvesyuzo 1:579511e9f0b8 96 theta = rho*theta_accel + (1-rho)*theta_gyro;
yvesyuzo 1:579511e9f0b8 97 psi = psi_gyro;
yurindes 8:1ad52489f6f3 98 }