Yuri De Stefani / CrazyflieController

controlador de atitude

AttitudeEstimator/AttitudeEstimator.cpp@8:c96125e9ac70, 2018-11-21 (annotated)

Committer:
yurindes
Date:
Wed Nov 21 10:07:00 2018 +0000
Branch:
yuri
Revision:
8:c96125e9ac70
Parent:
1:579511e9f0b8 
teste controlador atitude;

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
yurindes 8:c96125e9ac70 29 for(count = 1; count <= taxa; count++)
yvesyuzo 1:579511e9f0b8 30 {
yvesyuzo 1:579511e9f0b8 31 imu.read();
yurindes 8:c96125e9ac70 32 p_bias += imu.gx;
yurindes 8:c96125e9ac70 33 q_bias += imu.gy;
yurindes 8:c96125e9ac70 34 r_bias += imu.gz;
yvesyuzo 1:579511e9f0b8 35 wait(0.005);
yvesyuzo 1:579511e9f0b8 36 }
yurindes 8:c96125e9ac70 37 p_bias = p_bias/200.0f;
yurindes 8:c96125e9ac70 38 q_bias = q_bias/200.0f;
yurindes 8:c96125e9ac70 39 r_bias = r_bias/200.0f;
yvesyuzo 1:579511e9f0b8 40 }
yvesyuzo 1:579511e9f0b8 41
yvesyuzo 1:579511e9f0b8 42 // Estimate Euler angles (rad ) from accelerometer data
yvesyuzo 1:579511e9f0b8 43 void AttitudeEstimator::estimate_accel ()
yvesyuzo 1:579511e9f0b8 44 {
yvesyuzo 1:579511e9f0b8 45
yvesyuzo 1:579511e9f0b8 46 phi_accel = atan2(-imu.ay,-imu.az);
yvesyuzo 1:579511e9f0b8 47 if (imu.az!=0)
yvesyuzo 1:579511e9f0b8 48 {
yvesyuzo 1:579511e9f0b8 49 theta_accel = atan2( imu.ax,( -(imu.az/abs(imu.az))* sqrt(pow(imu.ay,2) + pow(imu.az,2)) ) );
yvesyuzo 1:579511e9f0b8 50 }
yvesyuzo 1:579511e9f0b8 51 else
yvesyuzo 1:579511e9f0b8 52 {
yvesyuzo 1:579511e9f0b8 53 theta_accel = atan2( imu.ax,(- sqrt(pow(imu.ay,2) + pow (imu.az,2)) ) );
yvesyuzo 1:579511e9f0b8 54 }
yvesyuzo 1:579511e9f0b8 55
yvesyuzo 1:579511e9f0b8 56 //theta_accel = atan2(imu.gx,((imu.gz/(abs(imu.gz)+0.00001)*(pow(imu.gy, 2)+pow(imu.gz, 2)))
yvesyuzo 1:579511e9f0b8 57
yvesyuzo 1:579511e9f0b8 58 }
yvesyuzo 1:579511e9f0b8 59
yvesyuzo 1:579511e9f0b8 60 // Estimate Euler angles (rad ) and angular velocities ( rad /s) from gyroscope data
yvesyuzo 1:579511e9f0b8 61 void AttitudeEstimator::estimate_gyro ()
yvesyuzo 1:579511e9f0b8 62 {
yvesyuzo 1:579511e9f0b8 63 p = imu.gx - p_bias;
yvesyuzo 1:579511e9f0b8 64 q = imu.gy - q_bias;
yvesyuzo 1:579511e9f0b8 65 r = imu.gz - r_bias;
yvesyuzo 1:579511e9f0b8 66
yvesyuzo 1:579511e9f0b8 67 phi_gyro = phi + (p + (sin(phi)*tan(theta))*q + (cos(phi)*tan(theta))*r)*dt;
yvesyuzo 1:579511e9f0b8 68 if (phi_gyro < -pi) {
yvesyuzo 1:579511e9f0b8 69 phi_gyro += 2*pi;
yvesyuzo 1:579511e9f0b8 70 }
yvesyuzo 1:579511e9f0b8 71 if (phi_gyro > pi) {
yvesyuzo 1:579511e9f0b8 72 phi_gyro -= 2*pi;
yvesyuzo 1:579511e9f0b8 73 }
yvesyuzo 1:579511e9f0b8 74
yvesyuzo 1:579511e9f0b8 75 theta_gyro = theta + (q*(cos(phi)) - (sin(phi))*r)*dt;
yvesyuzo 1:579511e9f0b8 76 if (theta_gyro < -pi) {
yvesyuzo 1:579511e9f0b8 77 theta_gyro += 2*pi;
yvesyuzo 1:579511e9f0b8 78 }
yvesyuzo 1:579511e9f0b8 79 if (theta_gyro > pi) {
yvesyuzo 1:579511e9f0b8 80 theta_gyro -= 2*pi ;
yvesyuzo 1:579511e9f0b8 81 }
yvesyuzo 1:579511e9f0b8 82
yvesyuzo 1:579511e9f0b8 83 psi_gyro = psi + (sin(phi)*(1/cos(theta))*q + cos(phi)*(1/cos(theta))*r)*dt;
yvesyuzo 1:579511e9f0b8 84 if (psi_gyro < -pi) {
yvesyuzo 1:579511e9f0b8 85 psi_gyro += 2*pi;
yvesyuzo 1:579511e9f0b8 86 }
yvesyuzo 1:579511e9f0b8 87 if (psi_gyro > pi) {
yvesyuzo 1:579511e9f0b8 88 psi_gyro -= 2*pi;
yvesyuzo 1:579511e9f0b8 89 }
yvesyuzo 1:579511e9f0b8 90 }
yvesyuzo 1:579511e9f0b8 91
yvesyuzo 1:579511e9f0b8 92 // Estimate euler angles (rad ) and angular velocity (rad/s)
yvesyuzo 1:579511e9f0b8 93 void AttitudeEstimator::estimate ()
yvesyuzo 1:579511e9f0b8 94 {
yvesyuzo 1:579511e9f0b8 95 imu.read();
yvesyuzo 1:579511e9f0b8 96 estimate_accel();
yvesyuzo 1:579511e9f0b8 97 estimate_gyro();
yurindes 8:c96125e9ac70 98 phi = rho*phi_accel + (1.0f-rho)*phi_gyro;
yurindes 8:c96125e9ac70 99 theta = rho*theta_accel + (1.0f-rho)*theta_gyro;
yvesyuzo 1:579511e9f0b8 100 psi = psi_gyro;
yurindes 8:c96125e9ac70 101 }