Merge remote-tracking branch 'refs/remotes/cedbossneo/feature/ds0'

jeremysalwen · May 6, 2024 · 512f926 · 512f926
2 parents cf839e2 + 2c7aff0
commit 512f926
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 0 deletions.
diff --git 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
@@ -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)