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.h@1:24effec9e9aa, 2018-09-12 (annotated)

Committer:
fbob
Date:
Wed Sep 12 00:18:15 2018 +0000
Revision:
1:24effec9e9aa
Parent:
0:b1f2c9e88e32
Child:
5:b9947e3d20cf
Updated AttitudeEstimator

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fbob 0:b1f2c9e88e32 1 #ifndef AttitudeEstimator_h
fbob 0:b1f2c9e88e32 2 #define AttitudeEstimator_h
fbob 0:b1f2c9e88e32 3
fbob 0:b1f2c9e88e32 4 #include "mbed.h"
fbob 0:b1f2c9e88e32 5 #include "MPU9250.h"
fbob 0:b1f2c9e88e32 6
fbob 0:b1f2c9e88e32 7 // Estimator constants
fbob 1:24effec9e9aa 8 float const pi = 3.14159265f;
fbob 0:b1f2c9e88e32 9 float const dt = 0.005f;
fbob 1:24effec9e9aa 10 float const rho = 0.02f;
fbob 0:b1f2c9e88e32 11
fbob 0:b1f2c9e88e32 12 // Attitude estimator class
fbob 0:b1f2c9e88e32 13 class AttitudeEstimator
fbob 0:b1f2c9e88e32 14 {
fbob 0:b1f2c9e88e32 15 public:
fbob 0:b1f2c9e88e32 16 // Class constructor
fbob 0:b1f2c9e88e32 17 AttitudeEstimator();
fbob 0:b1f2c9e88e32 18 // Initialize class
fbob 0:b1f2c9e88e32 19 void init();
fbob 0:b1f2c9e88e32 20 // Estimate Euler angles (rad) and angular velocities (rad/s)
fbob 0:b1f2c9e88e32 21 void estimate();
fbob 0:b1f2c9e88e32 22 // Euler angles (rad)
fbob 0:b1f2c9e88e32 23 float phi, theta, psi;
fbob 0:b1f2c9e88e32 24 // Angular velocities (rad/s)
fbob 0:b1f2c9e88e32 25 float p, q, r;
fbob 0:b1f2c9e88e32 26 private:
fbob 0:b1f2c9e88e32 27 // IMU sensor object
fbob 0:b1f2c9e88e32 28 MPU9250 imu;
fbob 0:b1f2c9e88e32 29 // Calibrates gyroscope by calculating angular velocity bias (rad/s)
fbob 0:b1f2c9e88e32 30 void calibrate_gyro();
fbob 0:b1f2c9e88e32 31 // Estimate Euler angles (rad) from accelerometer data
fbob 0:b1f2c9e88e32 32 void estimate_accel();
fbob 0:b1f2c9e88e32 33 // Estimate Euler angles (rad) and angular velocities (rad/s) from gyroscope data
fbob 0:b1f2c9e88e32 34 void estimate_gyro();
fbob 0:b1f2c9e88e32 35 // Euler angles (rad) from accelerometer data
fbob 0:b1f2c9e88e32 36 float phi_accel, theta_accel;
fbob 0:b1f2c9e88e32 37 // Euler angles (rad) from gyroscope data
fbob 1:24effec9e9aa 38 float phi_gyro, theta_gyro, psi_gyro;
fbob 0:b1f2c9e88e32 39 // Angular velocities bias (rad/s)
fbob 0:b1f2c9e88e32 40 float p_bias, q_bias, r_bias;
fbob 0:b1f2c9e88e32 41
fbob 0:b1f2c9e88e32 42 };
fbob 0:b1f2c9e88e32 43
fbob 0:b1f2c9e88e32 44 #endif