Habilita acelerómetro y giroscopio en el driver

cpbeto · Feb 14, 2022 · d14c7b0 · d14c7b0
1 parent f19aa02
commit d14c7b0
Show file tree

Hide file tree

Showing 3 changed files with 75 additions and 35 deletions.
diff --git a/src/bmx160.c b/src/bmx160.c
@@ -15,12 +15,20 @@
 #define BMX160_REG_CMD 0x7e
 
 #define BMX160_CMD_SOFT_RESET 0xb6
+#define BMX160_CMD_ACC_NORMAL 0x11
+#define BMX160_CMD_GYR_NORMAL 0x15
 #define BMX160_CMD_MAG_NORMAL 0x19
 #define BMX160_CMD_MAG_LOW_POWER 0x1A
 
-#define BMX160_MAG_RES_X (0.0351f) // +- 1150 uT / 16 bits
-#define BMX160_MAG_RES_Y (0.0351f) // +- 1150 uT / 16 bits
-#define BMX160_MAG_RES_Z (0.0763f) // +- 2500 uT / 16 bits
+#define BMX160_MAG_RES_X 0.0351 // +- 1150 uT / 16 bits
+#define BMX160_MAG_RES_Y 0.0351 // +- 1150 uT / 16 bits
+#define BMX160_MAG_RES_Z 0.0763 // +- 2500 uT / 16 bits
+
+#define BMX160_ACC_RES 0.000061035 // +- 2g / 16 bits
+
+#define BMX160_GYR_RES 0.061035 // +- 2000dps / 16 bits
+
+#define BMX160_DATA_SIZE 20 // Tamaño del buffer de datos
 
 /**
  * @brief Escribe un registro n (1 byte) en el sensor
@@ -60,6 +68,14 @@ void bmx160_init() {
     bmx160_write_cmd(BMX160_CMD_SOFT_RESET);
     vTaskDelay(100 / portTICK_PERIOD_MS);
 
+    // Habilitar acelerómetro
+    bmx160_write_cmd(BMX160_CMD_ACC_NORMAL);
+    vTaskDelay(10 / portTICK_PERIOD_MS);
+
+    // Habilitar giroscopio
+    bmx160_write_cmd(BMX160_CMD_GYR_NORMAL);
+    vTaskDelay(10 / portTICK_PERIOD_MS);
+
     // Habilitar magnetómetro
     bmx160_write_cmd(BMX160_CMD_MAG_NORMAL);
     vTaskDelay(10 / portTICK_PERIOD_MS);
@@ -90,11 +106,11 @@ void bmx160_init() {
 }
 
 /**
- * @brief Lee los datos brutos del sensor
+ * @brief Lee los datos en crudo del sensor
  * 
- * @param data Buffer de lectura
+ * @param buffer Buffer donde guardar los datos (tamaño mínimo BMX160_DATA_SIZE)
  */
-void bmx160_read_data(uint8_t *data) {
+void bmx160_read_raw(uint8_t *buffer) {
     i2c_cmd_handle_t cmd_handle = i2c_cmd_link_create();
 
     // Registro a leer
@@ -105,8 +121,8 @@ void bmx160_read_data(uint8_t *data) {
     // Realizar lectura
     i2c_master_start(cmd_handle);
     i2c_master_write_byte(cmd_handle, BMX160_SLAVE_ADDR, true);
-    i2c_master_read(cmd_handle, data, 5, I2C_MASTER_ACK);
-    i2c_master_read(cmd_handle, data + 5, 1, I2C_MASTER_NACK);
+    i2c_master_read(cmd_handle, buffer, BMX160_DATA_SIZE - 1, I2C_MASTER_ACK);
+    i2c_master_read(cmd_handle, buffer + BMX160_DATA_SIZE - 1, 1, I2C_MASTER_NACK);
     i2c_master_stop(cmd_handle);
 
     i2c_master_cmd_begin(i2c_master_port, cmd_handle, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
@@ -119,24 +135,46 @@ void bmx160_read_data(uint8_t *data) {
  * 
  * @return sensor_t Datos leidos
  */
-sensor_t bmx160_read() {
-    // Leer datos del sensor
-    uint8_t data[6];
-    bmx160_read_data(data);
+bmx160_data_t bmx160_read() {
+    // Leer datos crudos del sensor
+    uint8_t buffer[BMX160_DATA_SIZE];
+    bmx160_read_raw(buffer);
 
     // Convertir datos a estructura
-    sensor_t sensor;
+    bmx160_data_t data;
 
     int16_t x, y, z;
 
     // Magnetómetro
-    x = (int16_t) (((uint16_t)data[1] << 8) | data[0]);
-    y = (int16_t) (((uint16_t)data[3] << 8) | data[2]);
-    z = (int16_t) (((uint16_t)data[5] << 8) | data[4]);
+    x = (int16_t) (((uint16_t)buffer[1] << 8) | buffer[0]);
+    y = (int16_t) (((uint16_t)buffer[3] << 8) | buffer[2]);
+    z = (int16_t) (((uint16_t)buffer[5] << 8) | buffer[4]);
+
+    data.mag.x = x * BMX160_MAG_RES_X;
+    data.mag.y = y * BMX160_MAG_RES_Y;
+    data.mag.z = z * BMX160_MAG_RES_Z;
+
+    // Sensor Hall
+    // @todo
+    data.hall = 0.0;
+
+    // Magnetómetro
+    x = (int16_t) (((uint16_t)buffer[9] << 8) | buffer[8]);
+    y = (int16_t) (((uint16_t)buffer[11] << 8) | buffer[10]);
+    z = (int16_t) (((uint16_t)buffer[13] << 8) | buffer[12]);
+
+    data.mag.x = x * BMX160_ACC_RES;
+    data.mag.y = y * BMX160_ACC_RES;
+    data.mag.z = z * BMX160_ACC_RES;
+
+    // Magnetómetro
+    x = (int16_t) (((uint16_t)buffer[15] << 8) | buffer[14]);
+    y = (int16_t) (((uint16_t)buffer[17] << 8) | buffer[16]);
+    z = (int16_t) (((uint16_t)buffer[19] << 8) | buffer[18]);
 
-    sensor.mag.x = x * BMX160_MAG_RES_X;
-    sensor.mag.y = y * BMX160_MAG_RES_Y;
-    sensor.mag.z = z * BMX160_MAG_RES_Z;
+    data.mag.x = x * BMX160_GYR_RES;
+    data.mag.y = y * BMX160_GYR_RES;
+    data.mag.z = z * BMX160_GYR_RES;
 
-    return sensor;
+    return data;
 }
diff --git a/src/bmx160.h b/src/bmx160.h
@@ -8,17 +8,19 @@
  * 
  */
 
-typedef struct {
-    float x;
-    float y;
-    float z;
-} coord_t;
+#ifndef BMX160_H
+#define BMX160_H
+
+#include "vec3.h"
 
 typedef struct {
-    coord_t acc;
-    coord_t gyr;
-    coord_t mag;
-} sensor_t;
+    vec3_t acc;
+    vec3_t gyr;
+    vec3_t mag;
+    double hall;
+} bmx160_data_t;
 
 void bmx160_init();
-sensor_t bmx160_read();
+bmx160_data_t bmx160_read();
+
+#endif
diff --git a/src/main.c b/src/main.c
@@ -60,17 +60,17 @@ void bmx160_task() {
 
     // Leer e imprimir en pantalla
     while(true) {
-        sensor_t sensor = bmx160_read();
+        bmx160_data_t sensor_data = bmx160_read();
 
-        ESP_LOGI(TAG, "Magnetometro en uT. x = %f y = %f z = %f", sensor.mag.x, sensor.mag.y, sensor.mag.z);
+        ESP_LOGI(TAG, "Magnetometro en uT. x = %f y = %f z = %f", sensor_data.mag.x, sensor_data.mag.y, sensor_data.mag.z);
 
         // Convertir vector de campo magnético a orientación
         orientacion_t orientacion;
 
-        orientacion.alpha = atan2f(sensor.mag.x, sensor.mag.y) * 180 / M_PI;
+        orientacion.alpha = atan2f(sensor_data.mag.x, sensor_data.mag.y) * 180 / M_PI;
 
-        float h = sqrtf(powf(sensor.mag.x, 2.0f) + powf(sensor.mag.y, 2.0f));
-        orientacion.gamma = atan2f(h, sensor.mag.z) * 180 / M_PI;
+        float h = sqrtf(powf(sensor_data.mag.x, 2.0f) + powf(sensor_data.mag.y, 2.0f));
+        orientacion.gamma = atan2f(h, sensor_data.mag.z) * 180 / M_PI;
 
         ESP_LOGI(TAG, "Orientacion = %f | Inclinacion = %f", orientacion.alpha, orientacion.gamma);