Diff: AttitudeEstimator/AttitudeEstimator.cpp

Revision:

1:579511e9f0b8

Child:

8:1ad52489f6f3

diff -r 3871dc7bedf7 -r 579511e9f0b8 AttitudeEstimator/AttitudeEstimator.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.cpp	Wed Oct 10 11:19:54 2018 +0000
@@ -0,0 +1,101 @@
+#include "mbed.h"
+#include "AttitudeEstimator.h"
+#include "Library.h"
+
+
+// Class constructor
+AttitudeEstimator :: AttitudeEstimator () : imu(PC_9 , PA_8 )
+{
+    
+}
+// Initialize class
+void AttitudeEstimator::init()
+{
+    imu.init();
+    calibrate_gyro();
+    phi = 0;
+    theta = 0;
+    psi = 0;
+}
+// Calibrates gyroscope by calculating angular velocity bias (rad/s)
+void AttitudeEstimator::calibrate_gyro()
+{
+    int taxa = 200; //taxa em Hz de leitura
+    int count = 0;
+    p_bias = 0.0f;
+    q_bias = 0.0f;
+    r_bias = 0.0f;
+
+    for(count = 1; count <= taxa; ++count) 
+    {
+        imu.read();
+        p_bias += imu.gx/200.0f;
+        q_bias += imu.gy/200.0f;
+        r_bias += imu.gz/200.0f;
+        wait(0.005);
+    }
+}
+
+// Estimate Euler angles (rad ) from accelerometer data
+void AttitudeEstimator::estimate_accel ()
+{
+    
+    phi_accel = atan2(-imu.ay,-imu.az);    
+    if (imu.az!=0) 
+    {
+        theta_accel = atan2( imu.ax,( -(imu.az/abs(imu.az))* sqrt(pow(imu.ay,2) + pow(imu.az,2))  ) );
+    }
+    else 
+    {
+        theta_accel = atan2( imu.ax,(- sqrt(pow(imu.ay,2) + pow (imu.az,2)) ) );
+    }
+    
+    //theta_accel = atan2(imu.gx,((imu.gz/(abs(imu.gz)+0.00001)*(pow(imu.gy, 2)+pow(imu.gz, 2)))
+
+}
+
+// Estimate Euler angles (rad ) and angular velocities ( rad /s) from gyroscope data
+void AttitudeEstimator::estimate_gyro ()
+{
+    p = imu.gx - p_bias;
+    q = imu.gy - q_bias;
+    r = imu.gz - r_bias;
+
+    phi_gyro = phi + (p + (sin(phi)*tan(theta))*q + (cos(phi)*tan(theta))*r)*dt;
+    if (phi_gyro < -pi) {
+        phi_gyro += 2*pi;
+    }
+    if (phi_gyro > pi) {
+        phi_gyro -= 2*pi;
+    }   
+    
+    theta_gyro = theta + (q*(cos(phi)) - (sin(phi))*r)*dt;
+    if (theta_gyro < -pi) {
+        theta_gyro += 2*pi;
+    }
+    if (theta_gyro > pi) {
+        theta_gyro -= 2*pi ;
+    }
+    
+    psi_gyro   = psi + (sin(phi)*(1/cos(theta))*q + cos(phi)*(1/cos(theta))*r)*dt;
+    if (psi_gyro < -pi) {
+        psi_gyro += 2*pi;
+    }
+    if (psi_gyro > pi) {
+        psi_gyro -= 2*pi;
+    }    
+}                        
+
+// Estimate euler angles (rad ) and angular velocity (rad/s)
+void AttitudeEstimator::estimate ()
+{
+    imu.read();
+    estimate_accel();
+    estimate_gyro();
+    phi = rho*phi_accel + (1-rho)*phi_gyro;
+    theta = rho*theta_accel + (1-rho)*theta_gyro;
+    psi = psi_gyro;
+}
+
+
+