Teste de BMX160 para Coragem
main.cpp
- Committer:
- brunnobbco
- Date:
- 2019-07-11
- Revision:
- 0:4ed4ca850714
File content as of revision 0:4ed4ca850714:
#include "mbed.h" /* defines the axis for acc */ #define ACC_NOOF_AXIS 3 #define GYR_NOOF_AXIS 2 /* bmi160 slave address */ #define BMI160_ADDR ((0x69)<<1) #define RAD_DEG 57.29577951 Serial pc(USBTX, USBRX); // tx, rx I2C i2c(p13, p15); /* buffer to store acc samples */ 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}; int main() { pc.printf("Teste BMI160\n\r"); pc.printf("Configurando BMI160...\n\r"); wait_ms(250); /*Config Freq. I2C Bus*/ i2c.frequency(20000); /*Reset BMI160*/ i2c_reg_buffer[0] = 0x7E; i2c_reg_buffer[1] = 0xB6; i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false); wait_ms(200); pc.printf("BMI160 Resetado\n\r"); /*Habilita o Acelerometro*/ i2c_reg_buffer[0] = 0x7E; i2c_reg_buffer[1] = 0x11; //PMU Normal i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false); pc.printf("Acc Habilitado\n\r"); /*Habilita o Giroscopio*/ i2c_reg_buffer[0] = 0x7E; i2c_reg_buffer[1] = 0x15; //PMU Normal i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false); pc.printf("Gyr Habilitado\n\r"); /*Config o Data Rate ACC em 1600Hz*/ i2c_reg_buffer[0] = 0x40; i2c_reg_buffer[1] = 0x2C; i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false); pc.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; i2c.write(BMI160_ADDR, i2c_reg_buffer, sizeof(i2c_reg_buffer), false); 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"); while(1) { /*Le os Registradores do Acelerometro*/ i2c_reg_buffer[0] = 0x12; i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true); i2c.read(BMI160_ADDR, (char *)&acc_sample_buffer, sizeof(acc_sample_buffer), false); /*Le os Registradores do Giroscopio*/ i2c_reg_buffer[0] = 0x0C; i2c.write(BMI160_ADDR, i2c_reg_buffer, 1, true); i2c.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*/ pc.printf("%.3f,%.3f\n\r",tiltx, tilty); wait_ms(1); } }