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@0:b1f2c9e88e32, 2018-08-31 (annotated)

Committer:
fbob
Date:
Fri Aug 31 18:41:31 2018 +0000
Revision:
0:b1f2c9e88e32
Child:
1:24effec9e9aa
Include attitude controller, attitude estimator and mixer

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 // Estimate euler angles (rad) and angular velocity (rad/s)
fbob 0:b1f2c9e88e32 19 void AttitudeEstimator::estimate()
fbob 0:b1f2c9e88e32 20 {
fbob 0:b1f2c9e88e32 21 // Read IMU sensor data
fbob 0:b1f2c9e88e32 22 imu.read();
fbob 0:b1f2c9e88e32 23 // Estimate Euler angles (rad) and angular velocities (rad/s) from accelerometer and gyroscope data
fbob 0:b1f2c9e88e32 24 estimate_accel();
fbob 0:b1f2c9e88e32 25 estimate_gyro();
fbob 0:b1f2c9e88e32 26 //
fbob 0:b1f2c9e88e32 27 phi = phi_accel*rho + phi_gyro*(1.0f-rho);
fbob 0:b1f2c9e88e32 28 theta = theta_accel*rho + theta_gyro*(1.0f-rho);
fbob 0:b1f2c9e88e32 29 }
fbob 0:b1f2c9e88e32 30
fbob 0:b1f2c9e88e32 31 // Estimate Euler angles (rad) from accelerometer data
fbob 0:b1f2c9e88e32 32 void AttitudeEstimator::estimate_accel()
fbob 0:b1f2c9e88e32 33 {
fbob 0:b1f2c9e88e32 34 // Estimate Euler angles (rad)
fbob 0:b1f2c9e88e32 35 phi_accel = atan2(-imu.ay,-imu.az);
fbob 0:b1f2c9e88e32 36 theta_accel = atan2(imu.ax,-imu.az);
fbob 0:b1f2c9e88e32 37 }
fbob 0:b1f2c9e88e32 38
fbob 0:b1f2c9e88e32 39 // Estimate Euler angles (rad) and angular velocities (rad/s) from gyroscope data
fbob 0:b1f2c9e88e32 40 void AttitudeEstimator::estimate_gyro()
fbob 0:b1f2c9e88e32 41 {
fbob 0:b1f2c9e88e32 42 // Estimate angular velocities (rad/s)
fbob 0:b1f2c9e88e32 43 p = imu.gx - p_bias;
fbob 0:b1f2c9e88e32 44 q = imu.gy - q_bias;
fbob 0:b1f2c9e88e32 45 r = imu.gz - r_bias;
fbob 0:b1f2c9e88e32 46 // Estimate Euler angles (rad)
fbob 0:b1f2c9e88e32 47 phi_gyro = phi + p*dt;
fbob 0:b1f2c9e88e32 48 theta_gyro = theta + q*dt;
fbob 0:b1f2c9e88e32 49 psi = psi + r*dt;
fbob 0:b1f2c9e88e32 50 }
fbob 0:b1f2c9e88e32 51
fbob 0:b1f2c9e88e32 52 // Calibrates gyroscope by calculating angular velocity bias (rad/s)
fbob 0:b1f2c9e88e32 53 void AttitudeEstimator::calibrate_gyro()
fbob 0:b1f2c9e88e32 54 {
fbob 0:b1f2c9e88e32 55 // Reser angular velocites bias (rad/s)
fbob 0:b1f2c9e88e32 56 p_bias = 0.0f;
fbob 0:b1f2c9e88e32 57 q_bias = 0.0f;
fbob 0:b1f2c9e88e32 58 r_bias = 0.0f;
fbob 0:b1f2c9e88e32 59 // Calculate angular velocities bias (rad/s) by averaging 200 samples in 1 second
fbob 0:b1f2c9e88e32 60 for(int i = 0; i<200;i++)
fbob 0:b1f2c9e88e32 61 {
fbob 0:b1f2c9e88e32 62 imu.read();
fbob 0:b1f2c9e88e32 63 p_bias += imu.gx/200.0f;
fbob 0:b1f2c9e88e32 64 q_bias += imu.gy/200.0f;
fbob 0:b1f2c9e88e32 65 r_bias += imu.gz/200.0f;
fbob 0:b1f2c9e88e32 66 wait(0.005);
fbob 0:b1f2c9e88e32 67 }
fbob 0:b1f2c9e88e32 68 }