marco souza / Coragem_all_sensors

Program test for Coragem

Dependencies:   SX1272 SPI_MX25R

bmx160.txt@4:05d5aa4d3f2d, 2019-11-13 (annotated)

Committer:
marcoantonioara
Date:
Wed Nov 13 16:42:06 2019 +0000
Revision:
4:05d5aa4d3f2d
Test version firmware Coragem using all sensors

Who changed what in which revision?

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