From fad789c425e3c5151643f2987ec28d905896dfea Mon Sep 17 00:00:00 2001 From: Myers-Ty Date: Sun, 17 Nov 2024 20:25:53 -0500 Subject: [PATCH] Integrated new STM32 Driver --- Core/Inc/msb.h | 4 ++-- Core/Src/monitor.c | 34 +++++++++++++++++----------------- Core/Src/msb.c | 41 ++++++++++++++++++++--------------------- Makefile | 1 + 4 files changed, 40 insertions(+), 40 deletions(-) diff --git a/Core/Inc/msb.h b/Core/Inc/msb.h index d58773d..a3ba1af 100644 --- a/Core/Inc/msb.h +++ b/Core/Inc/msb.h @@ -24,9 +24,9 @@ int8_t central_temp_measure(uint16_t *temp, uint16_t *humidity); #endif #ifdef SENSOR_IMU -int8_t accel_read(uint16_t accel[3]); +int8_t accel_read(LSM6DSO_Axes_t* accel); -int8_t gyro_read(uint16_t gyro[3]); +int8_t gyro_read(LSM6DSO_Axes_t* gyro); #endif #ifdef SENSOR_TOF diff --git a/Core/Src/monitor.c b/Core/Src/monitor.c index 1098260..f5776f5 100644 --- a/Core/Src/monitor.c +++ b/Core/Src/monitor.c @@ -88,7 +88,7 @@ void vTempMonitor(void *pv_params) osThreadId_t imu_monitor_handle; const osThreadAttr_t imu_monitor_attributes = { .name = "IMUMonitor", - .stack_size = 64 * 8, + .stack_size = 128 * 8, .priority = (osPriority_t)osPriorityHigh, }; @@ -105,38 +105,38 @@ void vIMUMonitor(void *pv_params) .data = { 0 } }; struct __attribute__((__packed__)) { - uint16_t accel_x; - uint16_t accel_y; - uint16_t accel_z; + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; } accel_data; struct __attribute__((__packed__)) { - uint16_t gyro_x; - uint16_t gyro_y; - uint16_t gyro_z; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; } gyro_data; - uint16_t accel_data_temp[3] = { 0 }; - uint16_t gyro_data_temp[3] = { 0 }; + LSM6DSO_Axes_t accel_data_temp = { 0 }; + LSM6DSO_Axes_t gyro_data_temp = (LSM6DSO_Axes_t) { 0 }; for (;;) { /* Take measurement */ - if (accel_read(accel_data_temp)) { + if (accel_read(&accel_data_temp)) { printf("Failed to get IMU acceleration\r\n"); } - if (gyro_read(gyro_data_temp)) { + if (gyro_read(&gyro_data_temp)) { printf("Failed to get IMU gyroscope\r\n"); } /* Run values through LPF of sample size */ - accel_data.accel_x = (accel_data.accel_x + accel_data_temp[0]); - accel_data.accel_y = (accel_data.accel_y + accel_data_temp[1]); - accel_data.accel_z = (accel_data.accel_z + accel_data_temp[2]); - gyro_data.gyro_x = (gyro_data.gyro_x + gyro_data_temp[0]); - gyro_data.gyro_y = (gyro_data.gyro_y + gyro_data_temp[1]); - gyro_data.gyro_z = (gyro_data.gyro_z + gyro_data_temp[2]); + accel_data.accel_x = accel_data_temp.x; + accel_data.accel_y = accel_data_temp.y; + accel_data.accel_z = accel_data_temp.z; + gyro_data.gyro_x = gyro_data_temp.x; + gyro_data.gyro_y = gyro_data_temp.y; + gyro_data.gyro_z = gyro_data_temp.z; #ifdef LOG_VERBOSE printf("IMU Accel x: %d y: %d z: %d \r\n", accel_data.accel_x, diff --git a/Core/Src/msb.c b/Core/Src/msb.c index 9c7be3f..226795f 100644 --- a/Core/Src/msb.c +++ b/Core/Src/msb.c @@ -1,6 +1,7 @@ #include "msb.h" #include "main.h" #include "lsm6dso.h" +#include "lsm6dso_reg.h" #include #include #include @@ -15,27 +16,25 @@ extern device_loc_t device_loc; osMutexId_t i2c_mutex; // reads imu reg -static inline int imu_read_reg(uint8_t *data, uint8_t reg, uint8_t length) + +int32_t lsm6dso_read_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data, uint16_t len) { - return HAL_I2C_Mem_Read(&hi2c3, LSM6DSO_I2C_ADDRESS, reg, - I2C_MEMADD_SIZE_8BIT, data, length, + return HAL_I2C_Mem_Read(&hi2c3, LSM6DSO_I2C_ADD_L, reg, + I2C_MEMADD_SIZE_8BIT, data, len, HAL_MAX_DELAY); } - -// read imu write -static inline int imu_write_reg(uint8_t *data, uint8_t reg, uint8_t length) +int32_t lsm6dso_write_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data, uint16_t len) { - return HAL_I2C_Mem_Write(&hi2c3, LSM6DSO_I2C_ADDRESS, reg, - I2C_MEMADD_SIZE_8BIT, data, length, - HAL_MAX_DELAY); + return HAL_I2C_Mem_Write(&hi2c3, LSM6DSO_I2C_ADD_L, reg, + I2C_MEMADD_SIZE_8BIT, data, len, + HAL_MAX_DELAY); } - #ifdef SENSOR_TEMP sht30_t temp_sensor; #endif #ifdef SENSOR_IMU -lsm6dso_t imu; +LSM6DSO_Object_t imu; #endif #ifdef SENSOR_TOF @@ -58,8 +57,7 @@ int8_t msb_init() #ifdef SENSOR_IMU /* Initialize the IMU */ - assert(!lsm6dso_init(&imu, imu_read_reg, - imu_write_reg)); /* This is always connected */ + assert(!LSM6DSO_Init(&imu)); /* This is always connected */ #endif #ifdef SENSOR_TOF @@ -85,6 +83,9 @@ int8_t msb_init() i2c_mutex = osMutexNew(&msb_i2c_mutex_attr); assert(i2c_mutex); + LSM6DSO_ACC_Enable(&imu); + LSM6DSO_GYRO_Enable(&imu); + LSM6DSO_GYRO_Set_Power_Mode(&imu, LSM6DSO_GY_HIGH_PERFORMANCE); return 0; } @@ -138,33 +139,31 @@ void strain2_read(uint32_t strain2) #endif #ifdef SENSOR_IMU -int8_t accel_read(uint16_t accel[3]) +int8_t accel_read(LSM6DSO_Axes_t* accel) { osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever); if (mut_stat) return mut_stat; - - HAL_StatusTypeDef hal_stat = lsm6dso_read_accel(&imu); + HAL_StatusTypeDef hal_stat = LSM6DSO_ACC_GetAxes(&imu, accel); if (hal_stat) return hal_stat; - memcpy(accel, imu.accel_data, 3); + //memcpy(accel, imu.accel, 3); osMutexRelease(i2c_mutex); return 0; } -int8_t gyro_read(uint16_t gyro[3]) +int8_t gyro_read(LSM6DSO_Axes_t* gyro) { osStatus_t mut_stat = osMutexAcquire(i2c_mutex, osWaitForever); if (mut_stat) return mut_stat; - - HAL_StatusTypeDef hal_stat = lsm6dso_read_gyro(&imu); + HAL_StatusTypeDef hal_stat = LSM6DSO_GYRO_GetAxes(&imu, gyro); if (hal_stat) return hal_stat; - memcpy(gyro, imu.gyro_data, 3); + //memcpy(gyro, imu.gyro, 3); osMutexRelease(i2c_mutex); return 0; diff --git a/Makefile b/Makefile index 7985941..612ceb5 100644 --- a/Makefile +++ b/Makefile @@ -73,6 +73,7 @@ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.c \ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c \ Drivers/Embedded-Base/platforms/stm32f405/src/can.c \ Drivers/Embedded-Base/general/src/lsm6dso.c \ +Drivers/Embedded-Base/general/src/lsm6dso_reg.c \ Drivers/Embedded-Base/general/src/vl6180x_api.c \ Drivers/Embedded-Base/general/src/vl6180x_i2c.c \ Drivers/Embedded-Base/middleware/src/c_utils.c \