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@14:7439203a9e44, 2018-10-03 (annotated)

Committer:
fbob
Date:
Wed Oct 03 19:39:48 2018 +0000
Revision:
14:7439203a9e44
Parent:
13:1a871ebd35bb
Child:
15:155ca63b7884
a

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