From 2c7aff0a5fabff887aacb51e373dc5c53caf2eb2 Mon Sep 17 00:00:00 2001 From: Cedric Hauber Date: Mon, 6 May 2024 21:42:39 +0200 Subject: [PATCH] LSM6DSO Support --- stm32/ros_usbnode/include/imu/altimu-10v5.h | 3 +++ stm32/ros_usbnode/src/imu/altimu-10v5.c | 13 +++++++++++++ 2 files changed, 16 insertions(+) diff --git a/stm32/ros_usbnode/include/imu/altimu-10v5.h b/stm32/ros_usbnode/include/imu/altimu-10v5.h index 57acb5db..bb17acd6 100644 --- a/stm32/ros_usbnode/include/imu/altimu-10v5.h +++ b/stm32/ros_usbnode/include/imu/altimu-10v5.h @@ -9,6 +9,8 @@ #define LIS3MDL_GAUSS_FACTOR 1.0/6842 // LIS3MDL datasheet (page8) Gauss/LSB /* Gyro / Accelerometer */ +#define DS0_ADDRESS 0b1101011 +#define DS0_WHO_ID 0x6C // WHO_AM_I will report this value #define DS33_ADDRESS 0b1101011 #define DS33_WHO_ID 0x69 // WHO_AM_I will report this value @@ -23,6 +25,7 @@ /* Conversion functions */ #define IMU_MAG_TO_T(r) ((r)*LIS3MDL_GAUSS_FACTOR*T_PER_GAUSS) +#define DS0_WHO_AM_I 0x0F #define DS33_FUNC_CFG_ACCESS 0x01 #define DS33_FIFO_CTRL1 0x06 #define DS33_FIFO_CTRL2 0x07 diff --git a/stm32/ros_usbnode/src/imu/altimu-10v5.c b/stm32/ros_usbnode/src/imu/altimu-10v5.c index e503965c..c6a8714c 100644 --- a/stm32/ros_usbnode/src/imu/altimu-10v5.c +++ b/stm32/ros_usbnode/src/imu/altimu-10v5.c @@ -49,6 +49,19 @@ uint8_t ALTIMU10v5_TestDevice(void) debug_printf(" > [AltIMU-10v5] - Error probing for LSM6DS33 (Gyro / Accelerometer) at I2C addr=0x%0x\r\n", DS33_ADDRESS); return(0); } + + /* test the LSM6DS0 (gyro and accelerometer) */ + val = SW_I2C_UTIL_Read(DS0_ADDRESS, DS0_WHO_AM_I); + if (val == DS0_WHO_ID) + { + debug_printf(" > [AltIMU-10v5] - LSM6DS0 (Gyro / Accelerometer) FOUND at I2C addr=0x%0x\r\n", DS0_ADDRESS); + } + else + { + debug_printf(" > [AltIMU-10v5] - Error probing for LSM6DS0 (Gyro / Accelerometer) at I2C addr=0x%0x\r\n", DS0_ADDRESS); + return(0); + } + /* test the LIS3MDL (magnetometer) */ val = SW_I2C_UTIL_Read(LIS3MDL_ADDRESS, LIS3MDL_WHO_AM_I); if (val == LIS3MDL_WHO_ID)