Program test for Coragem
Dependencies: SX1272 SPI_MX25R
Diff: bmx160.txt
- Revision:
- 4:05d5aa4d3f2d
diff -r cbe3f441353e -r 05d5aa4d3f2d bmx160.txt --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/bmx160.txt Wed Nov 13 16:42:06 2019 +0000 @@ -0,0 +1,138 @@ + + +/* 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(); + +} \ No newline at end of file