GeoDynamics Hibrid Seismograph / Mbed 2 deprecated BMI160Test

Firmware Test of Tilt Sense using BMI160

Dependencies:   mbed TI_ADS1220 ESP8266

Fork of GeoDynamics by GeoDynamics Hibrid Seismograph

main.cpp@1:268fc98970bb, 2016-09-22 (annotated)

Committer:
firewalk
Date:
Thu Sep 22 14:40:17 2016 +0000
Revision:
1:268fc98970bb
Parent:
0:aa129e8d6912
Child:
2:986e8a434da3
Tilt sense without offset correction and dt of gyr static defined

Who changed what in which revision?

UserRevisionLine numberNew contents of line
firewalk 0:aa129e8d6912 1 #include "mbed.h"
firewalk 0:aa129e8d6912 2
firewalk 0:aa129e8d6912 3
firewalk 0:aa129e8d6912 4 /* defines the axis for acc */
firewalk 0:aa129e8d6912 5 #define ACC_NOOF_AXIS 3
firewalk 1:268fc98970bb 6 #define GYR_NOOF_AXIS 2
firewalk 0:aa129e8d6912 7
firewalk 0:aa129e8d6912 8 /* bmi160 slave address */
firewalk 0:aa129e8d6912 9 #define BMI160_ADDR ((0x68)<<1)
firewalk 0:aa129e8d6912 10
firewalk 1:268fc98970bb 11 #define RAD_DEG 57.29577951
firewalk 0:aa129e8d6912 12
firewalk 0:aa129e8d6912 13 Serial pc(USBTX, USBRX); // tx, rx
firewalk 0:aa129e8d6912 14
firewalk 0:aa129e8d6912 15 I2C i2c(P2_3, P2_4);
firewalk 0:aa129e8d6912 16
firewalk 0:aa129e8d6912 17
firewalk 0:aa129e8d6912 18 /* buffer to store acc samples */
firewalk 0:aa129e8d6912 19 int16_t acc_sample_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
firewalk 1:268fc98970bb 20 int16_t gyr_sample_buffer[GYR_NOOF_AXIS] = {0x5555, 0x5555};
firewalk 0:aa129e8d6912 21
firewalk 0:aa129e8d6912 22 double acc_result_buffer[ACC_NOOF_AXIS] = {0x5555, 0x5555, 0x5555};
firewalk 1:268fc98970bb 23 double gyr_result_buffer[GYR_NOOF_AXIS] = {0x5555, 0x5555};
firewalk 1:268fc98970bb 24
firewalk 1:268fc98970bb 25 double accel_ang_x, accel_ang_y;
firewalk 1:268fc98970bb 26 double tiltx, tilty;
firewalk 1:268fc98970bb 27 double tiltx_prev, tilty_prev;
firewalk 0:aa129e8d6912 28
firewalk 0:aa129e8d6912 29 char i2c_reg_buffer[2] = {0};
firewalk 0:aa129e8d6912 30
firewalk 0:aa129e8d6912 31 int main() {
firewalk 0:aa129e8d6912 32 pc.printf("Teste BMI160\n\r");
firewalk 0:aa129e8d6912 33 pc.printf("Configurando BMI160...\n\r");
firewalk 0:aa129e8d6912 34 wait_ms(250);
firewalk 0:aa129e8d6912 35
firewalk 0:aa129e8d6912 36 /*Config Freq. I2C Bus*/
firewalk 0:aa129e8d6912 37 i2c.frequency(20000);
firewalk 0:aa129e8d6912 38
firewalk 0:aa129e8d6912 39 /*Reset BMI160*/
firewalk 0:aa129e8d6912 40 i2c_reg_buffer[0] = 0x7E;
firewalk 0:aa129e8d6912 41 i2c_reg_buffer[1] = 0xB6;
firewalk 0:aa129e8d6912 42 i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
firewalk 0:aa129e8d6912 43 wait_ms(200);
firewalk 0:aa129e8d6912 44 pc.printf("BMI160 Resetado\n\r");
firewalk 0:aa129e8d6912 45
firewalk 0:aa129e8d6912 46 /*Habilita o Acelerometro*/
firewalk 0:aa129e8d6912 47 i2c_reg_buffer[0] = 0x7E;
firewalk 0:aa129e8d6912 48 i2c_reg_buffer[1] = 0x11; //PMU Normal
firewalk 0:aa129e8d6912 49 i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
firewalk 0:aa129e8d6912 50 pc.printf("Acc Habilitado\n\r");
firewalk 0:aa129e8d6912 51
firewalk 0:aa129e8d6912 52 /*Habilita o Giroscopio*/
firewalk 0:aa129e8d6912 53 i2c_reg_buffer[0] = 0x7E;
firewalk 0:aa129e8d6912 54 i2c_reg_buffer[1] = 0x15; //PMU Normal
firewalk 0:aa129e8d6912 55 i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
firewalk 0:aa129e8d6912 56 pc.printf("Gyr Habilitado\n\r");
firewalk 0:aa129e8d6912 57
firewalk 0:aa129e8d6912 58 /*Config o Data Rate ACC em 100Hz*/
firewalk 0:aa129e8d6912 59 i2c_reg_buffer[0] = 0x40;
firewalk 0:aa129e8d6912 60 i2c_reg_buffer[1] = 0x28;
firewalk 0:aa129e8d6912 61 i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
firewalk 0:aa129e8d6912 62 pc.printf("Data Rate ACC Selecionado a 100Hz\n\r");
firewalk 0:aa129e8d6912 63
firewalk 0:aa129e8d6912 64 /*Config o Data Rate GYR em 100Hz*/
firewalk 0:aa129e8d6912 65 i2c_reg_buffer[0] = 0x42;
firewalk 0:aa129e8d6912 66 i2c_reg_buffer[1] = 0x28;
firewalk 0:aa129e8d6912 67 i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false);
firewalk 0:aa129e8d6912 68 pc.printf("Data Rate GYR Selecionado a 100Hz\n\r");
firewalk 0:aa129e8d6912 69
firewalk 0:aa129e8d6912 70 wait_ms(2000);
firewalk 0:aa129e8d6912 71 pc.printf("BMI160 Configurado\n\r");
firewalk 0:aa129e8d6912 72
firewalk 0:aa129e8d6912 73
firewalk 0:aa129e8d6912 74 while(1) {
firewalk 0:aa129e8d6912 75
firewalk 0:aa129e8d6912 76 /*Le os Registradores do Acelerometro*/
firewalk 0:aa129e8d6912 77 i2c_reg_buffer[0] = 0x12;
firewalk 0:aa129e8d6912 78 i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true);
firewalk 0:aa129e8d6912 79 i2c.read(BMI160_ADDR, (char *)&acc_sample_buffer, sizeof(acc_sample_buffer), false);
firewalk 0:aa129e8d6912 80
firewalk 0:aa129e8d6912 81 /*Le os Registradores do Giroscopio*/
firewalk 0:aa129e8d6912 82 i2c_reg_buffer[0] = 0x0C;
firewalk 0:aa129e8d6912 83 i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true);
firewalk 0:aa129e8d6912 84 i2c.read(BMI160_ADDR, (char *)&gyr_sample_buffer, sizeof(gyr_sample_buffer), false);
firewalk 0:aa129e8d6912 85
firewalk 1:268fc98970bb 86 /*Ajusta dados brutos Acelerometro em unidades de g */
firewalk 1:268fc98970bb 87 acc_result_buffer[0] = (acc_sample_buffer[0]/16384.0);
firewalk 1:268fc98970bb 88 acc_result_buffer[1] = (acc_sample_buffer[1]/16384.0);
firewalk 1:268fc98970bb 89 acc_result_buffer[2] = (acc_sample_buffer[2]/16384.0);
firewalk 0:aa129e8d6912 90
firewalk 1:268fc98970bb 91 /*Ajusta dados Brutos do Giroscopio em unidades de deg/s */
firewalk 1:268fc98970bb 92 gyr_result_buffer[0] = (gyr_sample_buffer[0]/16.4);
firewalk 1:268fc98970bb 93 gyr_result_buffer[1] = (gyr_sample_buffer[1]/16.4);
firewalk 1:268fc98970bb 94
firewalk 1:268fc98970bb 95 /*Calcula os Angulos de Inclinacao com valor do Acelerometro*/
firewalk 1:268fc98970bb 96 accel_ang_x=atan(acc_result_buffer[0]/sqrt(pow(acc_result_buffer[1],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
firewalk 1:268fc98970bb 97 accel_ang_y=atan(acc_result_buffer[1]/sqrt(pow(acc_result_buffer[0],2) + pow(acc_result_buffer[2],2)))*RAD_DEG;
firewalk 1:268fc98970bb 98
firewalk 1:268fc98970bb 99 /*Calcula os Angulos de Rotacao com valor do Giroscopio e aplica filtro complementar realizando a fusao*/
firewalk 1:268fc98970bb 100 tiltx = (0.98*(tiltx_prev+(gyr_result_buffer[0]*0.01)))+(0.02*(accel_ang_x));
firewalk 1:268fc98970bb 101 tilty = (0.98*(tilty_prev+(gyr_result_buffer[1]*0.01)))+(0.02*(accel_ang_y));
firewalk 1:268fc98970bb 102
firewalk 1:268fc98970bb 103 tiltx_prev = tiltx;
firewalk 1:268fc98970bb 104 tilty_prev = tilty;
firewalk 0:aa129e8d6912 105
firewalk 0:aa129e8d6912 106 /*Imprime os dados ACC pre-formatados*/
firewalk 1:268fc98970bb 107 pc.printf("%.3f,%.3f\n\r",tiltx, tilty);
firewalk 1:268fc98970bb 108
firewalk 1:268fc98970bb 109
firewalk 1:268fc98970bb 110
firewalk 1:268fc98970bb 111 wait_ms(10);
firewalk 0:aa129e8d6912 112 }
firewalk 0:aa129e8d6912 113 }