Diff: AttitudeEstimator/AttitudeEstimator.h

Revision:

0:b1f2c9e88e32

Child:

1:24effec9e9aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.h	Fri Aug 31 18:41:31 2018 +0000
@@ -0,0 +1,43 @@
+#ifndef AttitudeEstimator_h
+#define AttitudeEstimator_h
+
+#include "mbed.h"
+#include "MPU9250.h"
+
+// Estimator constants 
+float const rho = 0.05f;
+float const dt = 0.005f;
+
+// 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;
+    // Angular velocities bias (rad/s)
+    float p_bias, q_bias, r_bias;
+    
+};
+
+#endif