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?
User | Revision | Line number | New 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 | } |