Brunno Oliveira / Pulga_BLE_GPS

GPS to Pulga

Dependencies:   Si1133 BME280

source/BMX160.txt@24:595155aa83c3, 2020-01-03 (annotated)

Committer:
pancotinho
Date:
Fri Jan 03 09:28:20 2020 +0000
Revision:
24:595155aa83c3
Parent:
BMX160.txt@23:7f1c9c1a4c57 
Working sensors, GPS, BLE, Lorawan (w/ Buffering)

Who changed what in which revision?

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