AttitudeEstimator.h

00001 #ifndef AttitudeEstimator_h
00002 #define AttitudeEstimator_h
00003 
00004 #include "mbed.h"
00005 #include "Parameters.h"
00006 #include "MPU9250.h"
00007 
00008 // Attitude estimator class
00009 class AttitudeEstimator
00010 {
00011     public:
00012         // Class constructor
00013         AttitudeEstimator();
00014         // Initialize class
00015         void init();
00016         // Estimate Euler angles (rad) and angular velocities (rad/s)
00017         void estimate();
00018         // Euler angles (rad)
00019         float phi, theta, psi;
00020         // Angular velocities (rad/s)
00021         float p, q, r;
00022     private:
00023         // IMU sensor object
00024         MPU9250 imu;
00025         // Calibrates gyroscope by calculating angular velocity bias (rad/s)
00026         void calibrate_gyro();
00027         // Estimate Euler angles (rad) from accelerometer data
00028         void estimate_accel();
00029         // Estimate Euler angles (rad) and angular velocities (rad/s) from gyroscope data
00030         void estimate_gyro();
00031         // Euler angles (rad) from accelerometer data
00032         float phi_accel, theta_accel;
00033         // Euler angles (rad) from gyroscope data
00034         float phi_gyro, theta_gyro, psi_gyro;
00035         // Angular velocities bias (rad/s)
00036         float p_bias, q_bias, r_bias;
00037 };
00038 
00039 #endif