From a690abeec8299e3902b861098d2cb7f7831517fa Mon Sep 17 00:00:00 2001 From: fred13kim Date: Tue, 17 Dec 2024 00:32:41 -0500 Subject: [PATCH] major refactor and implement hal --- components/fk/src/main.c | 15 ++- lib/drivers/SConscript.py | 2 + lib/drivers/mpu6050/SConscript.py | 2 +- lib/drivers/mpu6050/i2c_hal.h | 24 ++++ lib/drivers/mpu6050/i2c_hal_esp32.c | 70 +++++++++++ lib/drivers/mpu6050/i2c_hal_esp32.h | 30 +++++ lib/drivers/mpu6050/mpu6050.c | 185 ++++++++++++---------------- lib/drivers/mpu6050/mpu6050.h | 44 ++++++- 8 files changed, 257 insertions(+), 115 deletions(-) create mode 100644 lib/drivers/mpu6050/i2c_hal.h create mode 100644 lib/drivers/mpu6050/i2c_hal_esp32.c create mode 100644 lib/drivers/mpu6050/i2c_hal_esp32.h diff --git a/components/fk/src/main.c b/components/fk/src/main.c index 40e41261d..2c45d7d98 100644 --- a/components/fk/src/main.c +++ b/components/fk/src/main.c @@ -4,18 +4,27 @@ int app_main(void) { - mpu6050_init(); + mpu6050_handle_t mpu6050_handle; + mpu6050_config_t mpu6050_config = { + .fs = FS_SEL_500, + .afs = AFS_SEL_4G, + .scl = (gpio_num_t) (40), + .sda = (gpio_num_t) (37), + }; - i2c_mpu6050_raw_gyro_t raw_gyro; + mpu6050_init(&mpu6050_handle, &mpu6050_config); + + mpu6050_raw_gyro_t raw_gyro; float x, y, z; for (;;) { - mpu6050_get_raw_gyro(dev_handle, &raw_gyro); + mpu6050_get_raw_gyro(&mpu6050_handle, &raw_gyro); x = raw_gyro.gyro_raw_xout / 65.5; y = raw_gyro.gyro_raw_yout / 65.5; z = raw_gyro.gyro_raw_zout / 65.5; printf("%f %f %f\n", x, y, z); vTaskDelay(pdMS_TO_TICKS(10)); } + return EXIT_SUCCESS; } diff --git a/lib/drivers/SConscript.py b/lib/drivers/SConscript.py index a35f968af..4be35a16b 100644 --- a/lib/drivers/SConscript.py +++ b/lib/drivers/SConscript.py @@ -7,6 +7,7 @@ env.Dir(f'{dir}') for dir in [ 'mpu6050', + 'hal', ] ] ) @@ -16,6 +17,7 @@ f'{driver}/SConscript.py' for driver in [ 'mpu6050', + 'hal', ] ] ) diff --git a/lib/drivers/mpu6050/SConscript.py b/lib/drivers/mpu6050/SConscript.py index 9d916a90a..0cc42ae70 100644 --- a/lib/drivers/mpu6050/SConscript.py +++ b/lib/drivers/mpu6050/SConscript.py @@ -2,6 +2,6 @@ Import('env') -source = [env.File('mpu6050.c')] +source = [env.File('mpu6050.c'), env.File('i2c_hal_esp32.c')] Return('source') diff --git a/lib/drivers/mpu6050/i2c_hal.h b/lib/drivers/mpu6050/i2c_hal.h new file mode 100644 index 000000000..99c5c1b85 --- /dev/null +++ b/lib/drivers/mpu6050/i2c_hal.h @@ -0,0 +1,24 @@ +#ifndef I2C_HAL_H +#define I2C_HAL_H + +#include + +typedef struct i2c_hal_handle_s i2c_hal_handle_t; +typedef struct i2c_hal_config_s i2c_hal_config_t; + +typedef struct { + void (*i2c_hal_init)(i2c_hal_handle_t *i2c_hal_handle, + i2c_hal_config_t *i2c_hal_config); + void (*i2c_hal_read)(i2c_hal_handle_t *i2c_hal_handle, + const uint8_t addr, + uint8_t *data, + uint32_t num_bytes); + void (*i2c_hal_write)(i2c_hal_handle_t *i2c_hal_handle, + const uint8_t addr, + uint8_t *data, + uint32_t num_bytes); +} i2c_hal_t; + +extern i2c_hal_t i2c_hal; + +#endif // I2C_HAL_H diff --git a/lib/drivers/mpu6050/i2c_hal_esp32.c b/lib/drivers/mpu6050/i2c_hal_esp32.c new file mode 100644 index 000000000..f59948efa --- /dev/null +++ b/lib/drivers/mpu6050/i2c_hal_esp32.c @@ -0,0 +1,70 @@ +#include "i2c_hal_esp32.h" + +#include "i2c_hal.h" + +#include + +#define I2C_MASTER_PORT I2C_NUM_0 + +#define I2C_TIMEOUT_MS 500 + +i2c_hal_t i2c_hal = { + .i2c_hal_init = i2c_hal_init, + .i2c_hal_read = i2c_hal_read, + .i2c_hal_write = i2c_hal_write, +}; + +void i2c_hal_init( + i2c_hal_handle_t *i2c_hal_handle, i2c_hal_config_t *i2c_hal_config) +{ + i2c_master_bus_config_t i2c_master_conf = { + .clk_source = I2C_CLK_SRC_DEFAULT, + .i2c_port = I2C_MASTER_PORT, + .scl_io_num = i2c_hal_config->scl, + .sda_io_num = i2c_hal_config->sda, + .glitch_ignore_cnt = 7, + }; + + i2c_master_bus_handle_t bus_handle; + + ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_master_conf, &bus_handle)); + + i2c_device_config_t i2c_dev_conf = { + .dev_addr_length = I2C_ADDR_BIT_LEN_7, + + // should probably make a check that device_address & + // scl_speed_hz is configured properly before we continue + .device_address = i2c_hal_config->device_address, + .scl_speed_hz = i2c_hal_config->scl_speed_hz, + }; + + ESP_ERROR_CHECK(i2c_master_bus_add_device( + bus_handle, &i2c_dev_conf, &(i2c_hal_handle->dev_handle))); +} + +void i2c_hal_read(i2c_hal_handle_t *i2c_hal_handle, + const uint8_t addr, + uint8_t *data, + uint32_t num_bytes) +{ + i2c_master_transmit_receive(i2c_hal_handle->dev_handle, + &addr, + 1, + data, + num_bytes, + I2C_TIMEOUT_MS); +} + +void i2c_hal_write(i2c_hal_handle_t *i2c_hal_handle, + const uint8_t addr, + uint8_t *data, + uint32_t num_bytes) +{ + uint8_t tx[num_bytes + 1]; + tx[0] = addr; + + memcpy(tx + 1, data, num_bytes); + + i2c_master_transmit( + i2c_hal_handle->dev_handle, tx, sizeof(tx), I2C_TIMEOUT_MS); +} diff --git a/lib/drivers/mpu6050/i2c_hal_esp32.h b/lib/drivers/mpu6050/i2c_hal_esp32.h new file mode 100644 index 000000000..8d7084030 --- /dev/null +++ b/lib/drivers/mpu6050/i2c_hal_esp32.h @@ -0,0 +1,30 @@ +#ifndef I2C_HAL_ESP32 +#define I2C_HAL_ESP32 + +#include "i2c_hal.h" +#include +#include + +struct i2c_hal_handle_s { + i2c_master_dev_handle_t dev_handle; +}; + +struct i2c_hal_config_s { + uint8_t device_address; + uint32_t scl_speed_hz; + uint8_t scl; + uint8_t sda; +}; + +void i2c_hal_init( + i2c_hal_handle_t *i2c_hal_handle, i2c_hal_config_t *i2c_hal_config); +void i2c_hal_read(i2c_hal_handle_t *i2c_hal_handle, + const uint8_t addr, + uint8_t *data, + uint32_t num_bytes); +void i2c_hal_write(i2c_hal_handle_t *i2c_hal_handle, + const uint8_t addr, + uint8_t *data, + uint32_t num_bytes); + +#endif // I2C_HAL_ESP32 diff --git a/lib/drivers/mpu6050/mpu6050.c b/lib/drivers/mpu6050/mpu6050.c index 2ac5f4a76..00bde533d 100644 --- a/lib/drivers/mpu6050/mpu6050.c +++ b/lib/drivers/mpu6050/mpu6050.c @@ -1,92 +1,69 @@ #include "mpu6050.h" +#include "i2c_hal_esp32.h" #include -#include +#define MPU6050_DEV_ADDR 0x68 // AD0 +#define MPU6050_DEV_FREQ_HZ 400000 -#define I2C_MASTER_PORT I2C_NUM_0 -#define I2C_MASTER_SCL_IO (gpio_num_t)(40) -#define I2C_MASTER_SDA_IO (gpio_num_t)(37) +#define MPU6050_WHO_AM_I 0x75 +#define MPU6050_DEV_ID 0x68 +#define MPU6050_GYRO_CONFIG 0x1B +#define MPU6050_PWR_MGTM_1 0x6B +#define MPU6050_ACCEL_XOUT 0x3B +#define MPU6050_GYRO_XOUT 0x43 -#define I2C_MPU6050_DEV_ADDR 0x68 // AD0 -#define I2C_MPU6050_DEV_FREQ_HZ 400000 - -#define I2C_MPU6050_WHO_AM_I 0x75 -#define I2C_MPU6050_GYRO_CONFIG 0x1B -#define I2C_MPU6050_PWR_MGTM_1 0x6B -#define I2C_MPU6050_ACCEL_XOUT 0x3B -#define I2C_MPU6050_GYRO_XOUT 0x43 - -#define I2C_MPU6050_DEV_ID 0x68 - -#define I2C_TIMEOUT_MS 500 - -typedef enum { - FS_SEL_250, - FS_SEL_500, - FS_SEL_1000, - FS_SEL_2000 -} i2c_mpu6050_fs_sel_t; - -typedef enum { - AFS_SEL_2G, - AFS_SEL_4G, - AFS_SEL_8G, - AFS_SEL_16G -} i2c_mpu6050_afs_sel_t; - +/* Static Prototypes */ static const char *TAG = "MPU6050.c"; -static void i2c_mpu6050_read(i2c_master_dev_handle_t i2c_dev, - uint8_t addr, - uint8_t *data, - size_t num_bytes) +static void mpu6050_configure( + mpu6050_handle_t *mpu6050_handle, mpu6050_config_t *mpu6050_config); +static void mpu6050_power_up(mpu6050_handle_t *mpu6050_handle); + +/* + * Performs startup initialization: + * Checks device id + * Configures sensitivity + * Powers up + */ +void mpu6050_init( + mpu6050_handle_t *mpu6050_handle, mpu6050_config_t *mpu6050_config) { - i2c_master_transmit_receive( - i2c_dev, &addr, 1, data, num_bytes, I2C_TIMEOUT_MS); -} - -static void i2c_mpu6050_write(i2c_master_dev_handle_t i2c_dev, - uint8_t addr, - uint8_t *data, - size_t num_bytes) -{ - uint8_t tx[num_bytes + 1]; - tx[0] = addr; - - memcpy(tx + 1, data, num_bytes); - - i2c_master_transmit(i2c_dev, tx, sizeof(tx), I2C_TIMEOUT_MS); -} + mpu6050_config->i2c_hal_config.device_address = MPU6050_DEV_ADDR; + mpu6050_config->i2c_hal_config.scl_speed_hz = MPU6050_DEV_FREQ_HZ; + mpu6050_config->i2c_hal_config.scl = mpu6050_config->scl; + mpu6050_config->i2c_hal_config.sda = mpu6050_config->sda; -void mpu6050_config(i2c_master_dev_handle_t i2c_dev, - i2c_mpu6050_fs_sel_t fs_sel, - i2c_mpu6050_afs_sel_t afs_sel) -{ - uint8_t data[2] = {fs_sel << 3, afs_sel << 3}; + i2c_hal_init(&(mpu6050_handle->i2c_hal_handle), + &(mpu6050_config->i2c_hal_config)); - i2c_mpu6050_write( - i2c_dev, I2C_MPU6050_GYRO_CONFIG, data, sizeof(data)); -} + uint8_t dev_id; + i2c_hal_read(&(mpu6050_handle->i2c_hal_handle), + MPU6050_WHO_AM_I, + &dev_id, + 1); -void mpu6050_power_up(i2c_master_dev_handle_t i2c_dev) -{ - uint8_t data; - i2c_mpu6050_read(i2c_dev, I2C_MPU6050_PWR_MGTM_1, &data, 1); + if (dev_id != (uint8_t) MPU6050_DEV_ID) { + ESP_LOGE(TAG, "Failure @ dev id: %x", dev_id); + return; + } + ESP_LOGI(TAG, "MPU6050 RECOGNIZED!"); - // Turn off sleep mode - data &= (~0x40); + // configure sensitivity + mpu6050_configure(mpu6050_handle, mpu6050_config); + ESP_LOGI(TAG, "GRYO & ACCEL CONFIG FINISHED!"); - i2c_mpu6050_write(i2c_dev, I2C_MPU6050_PWR_MGTM_1, &data, 1); + mpu6050_power_up(mpu6050_handle); + ESP_LOGI(TAG, "AWAKE!"); } void mpu6050_get_raw_gyro( - i2c_master_dev_handle_t i2c_dev, i2c_mpu6050_raw_gyro_t *gyro_raw_val) + mpu6050_handle_t *mpu6050_handle, mpu6050_raw_gyro_t *gyro_raw_val) { uint8_t data[6]; - i2c_mpu6050_read(i2c_dev, - I2C_MPU6050_GYRO_XOUT, + i2c_hal_read(&(mpu6050_handle->i2c_hal_handle), + MPU6050_GYRO_XOUT, (uint8_t *) &data, sizeof(data)); @@ -95,48 +72,44 @@ void mpu6050_get_raw_gyro( gyro_raw_val->gyro_raw_zout = (int16_t) (data[4] << 8 | data[5]); } -void mpu6050_init(i2c_master_dev_handle_t dev_handle) +void mpu6050_get_raw_accel( + mpu6050_handle_t *mpu6050_handle, mpu6050_raw_accel_t *accel_raw_val) { - i2c_master_bus_config_t i2c_master_conf = { - .clk_source = I2C_CLK_SRC_DEFAULT, - .i2c_port = I2C_NUM_0, - .scl_io_num = I2C_MASTER_SCL_IO, - .sda_io_num = I2C_MASTER_SDA_IO, - .glitch_ignore_cnt = 7, - }; - - i2c_master_bus_handle_t bus_handle; - - ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_master_conf, &bus_handle)); - - i2c_device_config_t i2c_dev_conf = { - .dev_addr_length = I2C_ADDR_BIT_LEN_7, - .device_address = I2C_MPU6050_DEV_ADDR, - .scl_speed_hz = I2C_MPU6050_DEV_FREQ_HZ, - }; - - - ESP_ERROR_CHECK(i2c_master_bus_add_device( - bus_handle, &i2c_dev_conf, &dev_handle)); + uint8_t data[6]; + i2c_hal_read(&(mpu6050_handle->i2c_hal_handle), + MPU6050_ACCEL_XOUT, + (uint8_t *) &data, + sizeof(data)); - // Check Dev ID - uint8_t dev_id; - i2c_mpu6050_read(dev_handle, I2C_MPU6050_WHO_AM_I, &dev_id, 1); + accel_raw_val->accel_raw_xout = (int16_t) (data[0] << 8 | data[1]); + accel_raw_val->accel_raw_yout = (int16_t) (data[2] << 8 | data[3]); + accel_raw_val->accel_raw_zout = (int16_t) (data[4] << 8 | data[5]); +} - if (dev_id != (uint8_t) I2C_MPU6050_DEV_ID) { - ESP_LOGE(TAG, "Failure @ dev id: %x", dev_id); - return; - } - ESP_LOGI(TAG, "MPU6050 RECOGNIZED!"); +static void mpu6050_configure( + mpu6050_handle_t *mpu6050_handle, mpu6050_config_t *mpu6050_config) +{ + uint8_t data[2] = {mpu6050_config->fs << 3, mpu6050_config->afs << 3}; - // Config GYRO & ACCEL - mpu6050_config(dev_handle, FS_SEL_500, AFS_SEL_4G); - uint8_t tmp[2]; - i2c_mpu6050_read(dev_handle, I2C_MPU6050_GYRO_CONFIG, tmp, 2); + i2c_hal_write(&(mpu6050_handle->i2c_hal_handle), + MPU6050_GYRO_CONFIG, + data, + sizeof(data)); +} - ESP_LOGI(TAG, "GRYO & ACCEL CONFIG FINISHED!"); +static void mpu6050_power_up(mpu6050_handle_t *mpu6050_handle) +{ + uint8_t data; + i2c_hal_read(&(mpu6050_handle->i2c_hal_handle), + MPU6050_PWR_MGTM_1, + &data, + 1); - mpu6050_power_up(dev_handle); + // Turn off sleep mode + data &= (~0x40); - ESP_LOGI(TAG, "AWAKE!"); + i2c_hal_write(&(mpu6050_handle->i2c_hal_handle), + MPU6050_PWR_MGTM_1, + &data, + 1); } diff --git a/lib/drivers/mpu6050/mpu6050.h b/lib/drivers/mpu6050/mpu6050.h index 5557a97ce..9885ef2ab 100644 --- a/lib/drivers/mpu6050/mpu6050.h +++ b/lib/drivers/mpu6050/mpu6050.h @@ -1,17 +1,51 @@ #ifndef MPU6050_H #define MPU6050_H -#include -#include +#include "i2c_hal_esp32.h" + +typedef enum { + FS_SEL_250, + FS_SEL_500, + FS_SEL_1000, + FS_SEL_2000 +} mpu6050_fs_sel_t; + +typedef enum { + AFS_SEL_2G, + AFS_SEL_4G, + AFS_SEL_8G, + AFS_SEL_16G +} mpu6050_afs_sel_t; typedef struct { int16_t gyro_raw_xout; int16_t gyro_raw_yout; int16_t gyro_raw_zout; -} i2c_mpu6050_raw_gyro_t; +} mpu6050_raw_gyro_t; + +typedef struct { + int16_t accel_raw_xout; + int16_t accel_raw_yout; + int16_t accel_raw_zout; +} mpu6050_raw_accel_t; + +typedef struct mpu6050_handle_s { + i2c_hal_handle_t i2c_hal_handle; +} mpu6050_handle_t; -void mpu6050_init(i2c_master_dev_handle_t); +typedef struct mpu6050_config_s { + i2c_hal_config_t i2c_hal_config; + mpu6050_fs_sel_t fs; + mpu6050_afs_sel_t afs; + gpio_num_t scl; + gpio_num_t sda; +} mpu6050_config_t; +void mpu6050_init( + mpu6050_handle_t *mpu6050_handle, mpu6050_config_t *mpu6050_config); +void mpu6050_get_raw_accel( + mpu6050_handle_t *mpu6050_handle, mpu6050_raw_accel_t *accel_raw_val); void mpu6050_get_raw_gyro( - i2c_master_dev_handle_t i2c_dev, i2c_mpu6050_raw_gyro_t *gyro_raw_val); + mpu6050_handle_t *mpu6050_handle, mpu6050_raw_gyro_t *gyro_raw_val); + #endif // MPU6050_H