Diff: AttitudeEstimator/AttitudeEstimator.h

Revision:

1:579511e9f0b8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.h	Wed Oct 10 11:19:54 2018 +0000
@@ -0,0 +1,55 @@
+#ifndef AttitudeEstimator_h
+#define AttitudeEstimator_h
+
+#include "mbed.h"
+#include "MPU9250.h"
+#include "Library.h"
+
+/*
+// Estimator constants
+ float const pi = 3.14159265f;
+ float const dt = 0.005f;
+ float const rho = 0.05f;
+*/
+
+
+ // Attitude estimator class
+ class AttitudeEstimator
+ {
+     public :
+     // Class constructor
+     AttitudeEstimator () ;
+     // Initialize class
+     void init () ;
+     // Estimate Euler angles (rad ) and angular velocities ( rad /s)
+     void estimate () ;
+     // Euler angles ( rad)
+     float phi , theta , psi ;
+     // Angular velocities ( rad /s)
+     float p, q, r;
+     
+     private :
+     // IMU sensor object
+     MPU9250 imu;
+     // Calibrates gyroscope by calculating angular velocity bias (rad/s)
+     void calibrate_gyro () ;
+     // Estimate Euler angles (rad ) from accelerometer data
+     void estimate_accel () ;
+     // Estimate Euler angles (rad ) and angular velocities ( rad /s) from gyroscope data
+     void estimate_gyro () ;
+     // Euler angles ( rad) from accelerometer data
+     float phi_accel , theta_accel ;
+     // Euler angles ( rad) from gyroscope data
+     float phi_gyro , theta_gyro , psi_gyro ;
+     // Angular velocities bias ( rad /s)
+     float p_bias , q_bias , r_bias ;
+
+ };
+
+
+#endif
+
+
+
+
+