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?
User | Revision | Line number | New 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 | } |