From d14c7b01ca3fdc88766f986bdd3ccb41a075e5e7 Mon Sep 17 00:00:00 2001 From: cpbeto <41555339+cpbeto@users.noreply.github.com> Date: Mon, 14 Feb 2022 14:05:13 -0300 Subject: [PATCH] =?UTF-8?q?Habilita=20aceler=C3=B3metro=20y=20giroscopio?= =?UTF-8?q?=20en=20el=20driver?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/bmx160.c | 78 ++++++++++++++++++++++++++++++++++++++-------------- src/bmx160.h | 22 ++++++++------- src/main.c | 10 +++---- 3 files changed, 75 insertions(+), 35 deletions(-) diff --git a/src/bmx160.c b/src/bmx160.c index 3cf514c..b537be0 100644 --- 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; } \ No newline at end of file diff --git a/src/bmx160.h b/src/bmx160.h index a45c731..50a5cfc 100644 --- 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(); \ No newline at end of file +bmx160_data_t bmx160_read(); + +#endif \ No newline at end of file diff --git a/src/main.c b/src/main.c index ee35dbd..f8c969e 100644 --- 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);