Diff: bmx160.txt

Revision:

4:05d5aa4d3f2d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bmx160.txt	Wed Nov 13 16:42:06 2019 +0000
@@ -0,0 +1,138 @@
+
+
+/* defines the axis for acc */
+#define ACC_NOOF_AXIS       3
+#define GYR_NOOF_AXIS       2
+ 
+/* bmi160 slave address */
+//DigitalOut BMX_ADDR(P1_10);
+#define BMI160_ADDR         ((0x69)<<1)
+ 
+#define RAD_DEG           57.29577951
+ 
+//Serial pc(USBTX, USBRX); // tx, rx
+ 
+I2C bmx(p13, p15);
+
+int16_t acc_sample_buffer[ACC_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};
+ 
+double accel_ang_x, accel_ang_y;
+double tiltx, tilty;
+double tiltx_prev, tilty_prev;
+ 
+char i2c_reg_buffer[2] = {0};
+
+void bmx_config(){
+    
+    bmx.frequency(20000);
+    
+    /*Reset BMX160*/
+    i2c_reg_buffer[0] = 0x7E;
+    i2c_reg_buffer[1] = 0xB6;    
+    bmx.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    wait_ms(200);
+    printf("BMX160 Resetado\n\r");
+    
+    /*Habilita o Acelerometro*/
+    i2c_reg_buffer[0] = 0x7E;
+    i2c_reg_buffer[1] = 0x11; //PMU Normal   
+    bmx.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    printf("Acc Habilitado\n\r");
+    
+    /*Habilita o Giroscopio*/
+    i2c_reg_buffer[0] = 0x7E;
+    i2c_reg_buffer[1] = 0x15;  //PMU Normal 
+    bmx.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    printf("Gyr Habilitado\n\r");
+    
+    /*Config o Data Rate ACC em 1600Hz*/
+    i2c_reg_buffer[0] = 0x40;
+    i2c_reg_buffer[1] = 0x2C;    
+    bmx.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    printf("Data Rate ACC Selecionado a 1600Hz\n\r");
+    
+    /*Config o Data Rate GYR em 1600Hz*/
+    i2c_reg_buffer[0] = 0x42;
+    i2c_reg_buffer[1] = 0x2C;    
+    bmx.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    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;    
+    bmx.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    printf("Range GYR Selecionado a 250deg/s\n\r");
+    
+}
+void bmx_read(){
+
+        i2c_reg_buffer[0] = 0x12;
+        bmx.write(BMI160_ADDR, i2c_reg_buffer, 1, true);
+        bmx.read(BMI160_ADDR, (char *)&acc_sample_buffer, sizeof(acc_sample_buffer), false);
+        
+        /*Le os Registradores do Giroscopio*/
+        i2c_reg_buffer[0] = 0x0C;
+        bmx.write(BMI160_ADDR, i2c_reg_buffer, 1, true);
+        bmx.read(BMI160_ADDR, (char *)&gyr_sample_buffer, sizeof(gyr_sample_buffer), false);
+        
+        /*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 em unidades de deg/s */
+        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.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;                                 
+        
+        /*Imprime os dados ACC pre-formatados*/
+        printf("giro = %.3f,%.3f\n\r",tiltx, tilty);
+        
+}
+
+void bmx_sleep(){
+    
+    i2c_reg_buffer[0] = 0x7E;
+    i2c_reg_buffer[1] = 0x19;    
+    bmx.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    
+    wait_ms (1);
+    
+    i2c_reg_buffer[0] = 0x4C;
+    i2c_reg_buffer[1] = 0x80; 
+    bmx.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    
+    i2c_reg_buffer[0] = 0x4F;
+    i2c_reg_buffer[1] = 0x00;
+    bmx.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    
+    i2c_reg_buffer[0] = 0x4E;
+    i2c_reg_buffer[1] = 0x4B;
+    bmx.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    
+    i2c_reg_buffer[0] = 0x7;
+    i2c_reg_buffer[1] = 0x18;
+    bmx.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
+    
+    
+    }
+    
+void bmx_wake(){
+    
+    bmx_config();
+        
+}
\ No newline at end of file