Brunno Oliveira / BMX160_Coragem_Test

Teste de BMX160 para Coragem

main.cpp@0:4ed4ca850714, 2019-07-11 (annotated)

Committer:
brunnobbco
Date:
Thu Jul 11 18:10:11 2019 +0000
Revision:
0:4ed4ca850714
Teste Coragem BMX160

Who changed what in which revision?

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