Integrated new STM32 Driver

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

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,

Core/Src/msb.c

@@ -1,6 +1,7 @@
 #include "msb.h"
 #include "main.h"
 #include "lsm6dso.h"
+#include "lsm6dso_reg.h"
 #include <assert.h>
 #include <stdlib.h>
 #include <string.h>
@@ -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;

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 \