AttitudeEstimator.h

00001 #ifndef AttitudeEstimator_h
00002 #define AttitudeEstimator_h
00003 
00004 #include "mbed.h"
00005 #include "MPU9250.h"
00006 #include "Library.h"
00007 
00008 /*
00009 // Estimator constants
00010  float const pi = 3.14159265f;
00011  float const dt = 0.005f;
00012  float const rho = 0.05f;
00013 */
00014 
00015 
00016  // Attitude estimator class
00017  class AttitudeEstimator
00018  {
00019      public :
00020      // Class constructor
00021      AttitudeEstimator () ;
00022      // Initialize class
00023      void init () ;
00024      // Estimate Euler angles (rad ) and angular velocities ( rad /s)
00025      void estimate () ;
00026      // Euler angles ( rad)
00027      float phi , theta , psi ;
00028      // Angular velocities ( rad /s)
00029      float p, q, r;
00030      
00031      private :
00032      // IMU sensor object
00033      MPU9250 imu;
00034      // Calibrates gyroscope by calculating angular velocity bias (rad/s)
00035      void calibrate_gyro () ;
00036      // Estimate Euler angles (rad ) from accelerometer data
00037      void estimate_accel () ;
00038      // Estimate Euler angles (rad ) and angular velocities ( rad /s) from gyroscope data
00039      void estimate_gyro () ;
00040      // Euler angles ( rad) from accelerometer data
00041      float phi_accel , theta_accel ;
00042      // Euler angles ( rad) from gyroscope data
00043      float phi_gyro , theta_gyro , psi_gyro ;
00044      // Angular velocities bias ( rad /s)
00045      float p_bias , q_bias , r_bias ;
00046 
00047  };
00048 
00049 
00050 #endif
00051 
00052 
00053 
00054 
00055