Switch lsm6dso to STM driver, test on MSB #191

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,44 +105,55 @@ 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 };
+	struct __attribute__((__packed__)) {
+		float_t temp;
+	} temperature_data;  
 
-	for (;;) {
-		/* Take measurement */
+	stmdev_ctx_t ctx;
+	stmdev_ctx_t aux_ctx;
+	//int16_t temperature_data_temp;
 
-		if (accel_read(accel_data_temp)) {
-			printf("Failed to get IMU acceleration\r\n");
-		}
+	lsm6dso_md_t imu_md_temp;
+	lsm6dso_data_t imu_data_temp;
+
+	/* Add parameters for formatting data */
+	imu_md_temp.ui.gy.fs = LSM6DSO_500dps;
+	imu_md_temp.ui.gy.odr = LSM6DSO_GY_UI_52Hz_LP;
+	imu_md_temp.ui.xl.fs = LSM6DSO_XL_UI_2g;
+	imu_md_temp.ui.xl.odr = LSM6DSO_XL_UI_52Hz_LP;
 
-		if (gyro_read(gyro_data_temp)) {
-			printf("Failed to get IMU gyroscope\r\n");
+	for (;;) {
+		/* Take measurement */
+		if (lsm6dso_data_get(&ctx, &aux_ctx, &imu_md_temp, &imu_data_temp)) {
+			printf("Failed to get IMU data \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 = imu_data_temp.ui.xl.mg[0];
+		accel_data.accel_y = imu_data_temp.ui.xl.mg[1];
+		accel_data.accel_z = imu_data_temp.ui.xl.mg[2];
+		gyro_data.gyro_x = imu_data_temp.ui.gy.mdps[0];
+		gyro_data.gyro_y = imu_data_temp.ui.gy.mdps[1];
+		gyro_data.gyro_z = imu_data_temp.ui.gy.mdps[2];
+		temperature_data.temp = imu_data_temp.ui.heat.deg_c;
 
 #ifdef LOG_VERBOSE
 		printf("IMU Accel x: %d y: %d z: %d \r\n", accel_data.accel_x,
 		       accel_data.accel_y, accel_data.accel_z);
 		printf("IMU Gyro x: %d y: %d z: %d \r\n", gyro_data.gyro_x,
 		       gyro_data.gyro_y, gyro_data.gyro_z);
+		printf("IMU Temp: %3.2f °C \r\n", temperature_data.temp);
 #endif
 
 		/* convert to big endian */

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,14 @@ int8_t msb_init()
 	i2c_mutex = osMutexNew(&msb_i2c_mutex_attr);
 	assert(i2c_mutex);
 
+	/* Setup IMU Accelerometer */
+	LSM6DSO_ACC_Enable(&imu);
+
+	/* Setup IMU Gyroscope */
+	LSM6DSO_GYRO_Enable(&imu);
+
+	LSM6DSO_FIFO_Set_Mode(&imu, 0);
+	LSM6DSO_ACC_Disable_Inactivity_Detection(&imu);
 	return 0;
 }
 
@@ -138,33 +144,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 \