Diff: main.cpp

Revision:

2:986e8a434da3

Parent:

1:268fc98970bb

Child:

4:c79a3c86ab36

diff -r 268fc98970bb -r 986e8a434da3 main.cpp
--- a/main.cpp	Thu Sep 22 14:40:17 2016 +0000
+++ b/main.cpp	Thu Sep 22 15:44:43 2016 +0000
@@ -55,17 +55,23 @@
     i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
     pc.printf("Gyr Habilitado\n\r");
     
-    /*Config o Data Rate ACC em 100Hz*/
+    /*Config o Data Rate ACC em 1600Hz*/
     i2c_reg_buffer[0] = 0x40;
-    i2c_reg_buffer[1] = 0x28;    
+    i2c_reg_buffer[1] = 0x2C;    
     i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
-    pc.printf("Data Rate ACC Selecionado a 100Hz\n\r");
+    pc.printf("Data Rate ACC Selecionado a 1600Hz\n\r");
     
-    /*Config o Data Rate GYR em 100Hz*/
+    /*Config o Data Rate GYR em 1600Hz*/
     i2c_reg_buffer[0] = 0x42;
-    i2c_reg_buffer[1] = 0x28;    
+    i2c_reg_buffer[1] = 0x2C;    
     i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
-    pc.printf("Data Rate GYR Selecionado a 100Hz\n\r");
+    pc.printf("Data Rate GYR Selecionado a 1600Hz\n\r");
+    
+    /*Config o Range GYR em 250º/s*/
+    i2c_reg_buffer[0] = 0x43;
+    i2c_reg_buffer[1] = 0x03;    
+    i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    pc.printf("Range GYR Selecionado a 250deg/s\n\r");
     
     wait_ms(2000);
     pc.printf("BMI160 Configurado\n\r");
@@ -89,16 +95,16 @@
         acc_result_buffer[2] = (acc_sample_buffer[2]/16384.0);
         
         /*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);
+        gyr_result_buffer[0] = (gyr_sample_buffer[0]/131.2);
+        gyr_result_buffer[1] = (gyr_sample_buffer[1]/131.2);
                 
         /*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 = (0.98*(tiltx_prev+(gyr_result_buffer[0]*0.001)))+(0.02*(accel_ang_x));
+        tilty = (0.98*(tilty_prev+(gyr_result_buffer[1]*0.001)))+(0.02*(accel_ang_y));
         
         tiltx_prev = tiltx;
         tilty_prev = tilty;                                 
@@ -108,6 +114,6 @@
 
 
          
-        wait_ms(10);
+        wait_ms(1);
     }
 }