Diff: main.cpp

Revision:

1:268fc98970bb

Parent:

0:aa129e8d6912

Child:

2:986e8a434da3

--- a/main.cpp	Thu Sep 22 12:57:40 2016 +0000
+++ b/main.cpp	Thu Sep 22 14:40:17 2016 +0000
@@ -3,12 +3,12 @@
 
 /* defines the axis for acc */
 #define ACC_NOOF_AXIS       3
-#define GYR_NOOF_AXIS       3
+#define GYR_NOOF_AXIS       2
 
 /* bmi160 slave address */
 #define BMI160_ADDR         ((0x68)<<1)
 
-#define M_PI           3.1415927
+#define RAD_DEG           57.29577951
  
 Serial pc(USBTX, USBRX); // tx, rx
 
@@ -17,10 +17,14 @@
 
 /* buffer to store acc samples */
 int16_t acc_sample_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
-int16_t gyr_sample_buffer[GYR_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
+int16_t gyr_sample_buffer[GYR_NOOF_AXIS] = {0x5555, 0x5555};
 
 double acc_result_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
-double gyr_result_buffer[GYR_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
+double gyr_result_buffer[GYR_NOOF_AXIS] = {0x5555, 0x5555};
+
+double accel_ang_x, accel_ang_y;
+double tiltx, tilty;
+double tiltx_prev, tilty_prev;
 
 char i2c_reg_buffer[2] = {0};
  
@@ -79,26 +83,31 @@
         i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true);
         i2c.read(BMI160_ADDR, (char *)&gyr_sample_buffer, sizeof(gyr_sample_buffer), false);
         
-        /*Ajusta dados brutos Acelerometro e transforma m/s^2*/
-        acc_result_buffer[0] = (acc_sample_buffer[0]/16384.0)*9.80665;
-        acc_result_buffer[1] = (acc_sample_buffer[1]/16384.0)*9.80665;
-        acc_result_buffer[2] = (acc_sample_buffer[2]/16384.0)*9.80665;
+        /*Ajusta dados brutos Acelerometro em unidades de g */
+        acc_result_buffer[0] = (acc_sample_buffer[0]/16384.0);
+        acc_result_buffer[1] = (acc_sample_buffer[1]/16384.0);
+        acc_result_buffer[2] = (acc_sample_buffer[2]/16384.0);
         
-        /*Ajusta dados Brutos do Giroscopio e transforma rad/s*/
-        gyr_result_buffer[0] = (gyr_sample_buffer[0]/16.4)*0.017453;
-        gyr_result_buffer[1] = (gyr_sample_buffer[1]/16.4)*0.017453;
-        gyr_result_buffer[2] = (gyr_sample_buffer[2]/16.4)*0.017453;
-                  
-                                 
+        /*Ajusta dados Brutos do Giroscopio em unidades de deg/s */
+        gyr_result_buffer[0] = (gyr_sample_buffer[0]/16.4);
+        gyr_result_buffer[1] = (gyr_sample_buffer[1]/16.4);
+                
+        /*Calcula os Angulos de Inclinacao com valor do Acelerometro*/
+        accel_ang_x=atan(acc_result_buffer[0]/sqrt(pow(acc_result_buffer[1],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
+        accel_ang_y=atan(acc_result_buffer[1]/sqrt(pow(acc_result_buffer[0],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
+        
+        /*Calcula os Angulos de Rotacao com valor do Giroscopio e aplica filtro complementar realizando a fusao*/
+        tiltx = (0.98*(tiltx_prev+(gyr_result_buffer[0]*0.01)))+(0.02*(accel_ang_x));
+        tilty = (0.98*(tilty_prev+(gyr_result_buffer[1]*0.01)))+(0.02*(accel_ang_y));
+        
+        tiltx_prev = tiltx;
+        tilty_prev = tilty;                                 
         
         /*Imprime os dados ACC pre-formatados*/
-        pc.printf("AccXraw %f ", (acc_result_buffer[0]));
-        pc.printf("AccYraw %f ", (acc_result_buffer[1]));
-        pc.printf("AccZraw %f ", (acc_result_buffer[2]));
-        /*Imprime os dados GYR pre-formatados*/
-        pc.printf("GYRXraw %f ", (gyr_result_buffer[0]));
-        pc.printf("GYRYraw %f ", (gyr_result_buffer[1]));
-        pc.printf("GYRZraw %f\n\r", (gyr_result_buffer[2]));
-        wait_ms(100);
+        pc.printf("%.3f,%.3f\n\r",tiltx, tilty);
+
+
+         
+        wait_ms(10);
     }
 }