Fabio Bobrow / CrazyflieController

Library containing Crazyflie 2.0 controller classes: - Attitude estimator - Horizontal estimator - Vertical estimator - Attitude controller - Horizontal controller - Vertical controller - Mixer

AttitudeEstimator/AttitudeEstimator.cpp@24:7b9e3beb61d5, 2018-12-06 (annotated)

Committer:
fbob
Date:
Thu Dec 06 16:44:40 2018 +0000
Revision:
24:7b9e3beb61d5
Parent:
21:169cc2b1d2ff 
Adjust

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fbob 0:b1f2c9e88e32 1 #include "mbed.h"
fbob 0:b1f2c9e88e32 2 #include "AttitudeEstimator.h"
fbob 0:b1f2c9e88e32 3
fbob 0:b1f2c9e88e32 4 // Class constructor
fbob 17:f682b4a5686d 5 AttitudeEstimator::AttitudeEstimator() : imu(PC_9,PA_8), led_3_blue(PD_2)
fbob 0:b1f2c9e88e32 6 {
fbob 15:155ca63b7884 7 phi = 0.0f;
fbob 15:155ca63b7884 8 theta = 0.0f;
fbob 15:155ca63b7884 9 psi = 0.0f;
fbob 15:155ca63b7884 10 p = 0.0f;
fbob 15:155ca63b7884 11 q = 0.0f;
fbob 15:155ca63b7884 12 r = 0.0f;
fbob 15:155ca63b7884 13 phi_accel = 0.0f;
fbob 15:155ca63b7884 14 theta_accel = 0.0f;
fbob 15:155ca63b7884 15 phi_gyro = 0.0f;
fbob 15:155ca63b7884 16 theta_gyro = 0.0f;
fbob 15:155ca63b7884 17 psi_gyro = 0.0f;
fbob 15:155ca63b7884 18 p_bias = 0.0f;
fbob 15:155ca63b7884 19 q_bias = 0.0f;
fbob 15:155ca63b7884 20 r_bias = 0.0f;
fbob 17:f682b4a5686d 21 led_3_blue = 0;
fbob 0:b1f2c9e88e32 22 }
fbob 0:b1f2c9e88e32 23
fbob 0:b1f2c9e88e32 24 // Initialize class
fbob 0:b1f2c9e88e32 25 void AttitudeEstimator::init()
fbob 0:b1f2c9e88e32 26 {
fbob 0:b1f2c9e88e32 27 // Initialize IMU sensor object
fbob 0:b1f2c9e88e32 28 imu.init();
fbob 0:b1f2c9e88e32 29 // Calibrate gyroscope
fbob 0:b1f2c9e88e32 30 calibrate_gyro();
fbob 0:b1f2c9e88e32 31 }
fbob 0:b1f2c9e88e32 32
fbob 0:b1f2c9e88e32 33 // Calibrates gyroscope by calculating angular velocity bias (rad/s)
fbob 0:b1f2c9e88e32 34 void AttitudeEstimator::calibrate_gyro()
fbob 0:b1f2c9e88e32 35 {
fbob 14:7439203a9e44 36 // Calculate angular velocities bias (rad/s) by averaging 600 samples during 2 seconds
fbob 14:7439203a9e44 37 for(int i = 0; i<600;i++)
fbob 0:b1f2c9e88e32 38 {
fbob 0:b1f2c9e88e32 39 imu.read();
fbob 14:7439203a9e44 40 p_bias += imu.gx/600.0f;
fbob 14:7439203a9e44 41 q_bias += imu.gy/600.0f;
fbob 14:7439203a9e44 42 r_bias += imu.gz/600.0f;
fbob 17:f682b4a5686d 43 if ((i%25)==0)
fbob 17:f682b4a5686d 44 {
fbob 17:f682b4a5686d 45 led_3_blue = !led_3_blue;
fbob 17:f682b4a5686d 46 }
fbob 14:7439203a9e44 47 wait(dt);
fbob 0:b1f2c9e88e32 48 }
fbob 1:24effec9e9aa 49 }
fbob 1:24effec9e9aa 50
fbob 1:24effec9e9aa 51 // Estimate euler angles (rad) and angular velocity (rad/s)
fbob 1:24effec9e9aa 52 void AttitudeEstimator::estimate()
fbob 1:24effec9e9aa 53 {
fbob 1:24effec9e9aa 54 // Read IMU sensor data
fbob 1:24effec9e9aa 55 imu.read();
fbob 1:24effec9e9aa 56 // Estimate Euler angles (rad) and angular velocities (rad/s) from accelerometer and gyroscope data
fbob 1:24effec9e9aa 57 estimate_accel();
fbob 1:24effec9e9aa 58 estimate_gyro();
fbob 1:24effec9e9aa 59 // Ponderate Euler angles (rad) estimation from accelerometer and gyroscope
fbob 14:7439203a9e44 60 phi = phi_accel*rho_att + phi_gyro*(1.0f-rho_att);
fbob 14:7439203a9e44 61 theta = theta_accel*rho_att + theta_gyro*(1.0f-rho_att);
fbob 1:24effec9e9aa 62 psi = psi_gyro;
fbob 1:24effec9e9aa 63 }
fbob 1:24effec9e9aa 64
fbob 1:24effec9e9aa 65 // Estimate Euler angles (rad) from accelerometer data
fbob 1:24effec9e9aa 66 void AttitudeEstimator::estimate_accel()
fbob 1:24effec9e9aa 67 {
fbob 1:24effec9e9aa 68 // Estimate Euler angles (rad)
fbob 1:24effec9e9aa 69 phi_accel = atan2(-imu.ay,-imu.az);
fbob 1:24effec9e9aa 70 theta_accel = atan2(imu.ax,-((imu.az>0)-(imu.az<0))*sqrt(pow(imu.ay,2)+pow(imu.az,2)));
fbob 1:24effec9e9aa 71 }
fbob 1:24effec9e9aa 72
fbob 1:24effec9e9aa 73 // Estimate Euler angles (rad) and angular velocities (rad/s) from gyroscope data
fbob 1:24effec9e9aa 74 void AttitudeEstimator::estimate_gyro()
fbob 1:24effec9e9aa 75 {
fbob 1:24effec9e9aa 76 // Estimate angular velocities (rad/s)
fbob 1:24effec9e9aa 77 p = imu.gx - p_bias;
fbob 1:24effec9e9aa 78 q = imu.gy - q_bias;
fbob 1:24effec9e9aa 79 r = imu.gz - r_bias;
fbob 1:24effec9e9aa 80 // Estimate Euler angles (rad)
fbob 13:1a871ebd35bb 81 phi_gyro = phi + (p+q*sin(phi)*tan(theta)+r*cos(phi)*tan(theta))*dt;
fbob 13:1a871ebd35bb 82 theta_gyro = theta + (q*cos(phi)-r*sin(phi))*dt;
fbob 13:1a871ebd35bb 83 psi_gyro = psi + (q*sin(phi)/cos(theta)+r*cos(phi)/cos(theta))*dt;
fbob 1:24effec9e9aa 84 // Adjust Euler angles (rad) to be in the interval of +/- pi
fbob 21:169cc2b1d2ff 85 if(phi_gyro > pi)
fbob 1:24effec9e9aa 86 {
fbob 21:169cc2b1d2ff 87 phi_gyro = phi_gyro - 2*pi;
fbob 1:24effec9e9aa 88 }
fbob 21:169cc2b1d2ff 89 else if(phi_gyro < -pi)
fbob 1:24effec9e9aa 90 {
fbob 21:169cc2b1d2ff 91 phi_gyro = phi_gyro + 2*pi;
fbob 1:24effec9e9aa 92 }
fbob 1:24effec9e9aa 93
fbob 21:169cc2b1d2ff 94 if(theta_gyro > pi)
fbob 1:24effec9e9aa 95 {
fbob 21:169cc2b1d2ff 96 theta_gyro = theta_gyro - 2*pi;
fbob 1:24effec9e9aa 97 }
fbob 21:169cc2b1d2ff 98 else if(theta_gyro < -pi)
fbob 1:24effec9e9aa 99 {
fbob 21:169cc2b1d2ff 100 theta_gyro = theta_gyro + 2*pi;
fbob 1:24effec9e9aa 101 }
fbob 21:169cc2b1d2ff 102 if(psi_gyro > pi)
fbob 1:24effec9e9aa 103 {
fbob 21:169cc2b1d2ff 104 psi_gyro = psi_gyro - 2*pi;
fbob 1:24effec9e9aa 105 }
fbob 21:169cc2b1d2ff 106 else if(psi_gyro < -pi)
fbob 1:24effec9e9aa 107 {
fbob 21:169cc2b1d2ff 108 psi_gyro = psi_gyro + 2*pi;
fbob 1:24effec9e9aa 109 }
fbob 0:b1f2c9e88e32 110 }