Program test for Coragem
Dependencies: SX1272 SPI_MX25R
bmx160.txt
- Committer:
- marcoantonioara
- Date:
- 2019-11-13
- Revision:
- 4:05d5aa4d3f2d
File content as of revision 4:05d5aa4d3f2d:
/* 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(); }