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@15:155ca63b7884, 2018-10-03 (annotated)

Committer:
fbob
Date:
Wed Oct 03 22:12:38 2018 +0000
Revision:
15:155ca63b7884
Parent:
14:7439203a9e44
Child:
17:f682b4a5686d
10 fucking seconds flight!

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