diff --git a/Core/Inc/main.h b/Core/Inc/main.h
index fbf0a84..a0310bf 100644
--- a/Core/Inc/main.h
+++ b/Core/Inc/main.h
@@ -28,7 +28,7 @@ extern "C" {
/* Includes ------------------------------------------------------------------*/
#include "stm32wlxx_hal.h"
-
+#include "yunhorn_sts_prd_conf.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
@@ -77,6 +77,11 @@ void MX_TOF_Init(void);
#define PROB1_GPIO_Port GPIOB
#endif
+#if defined(STM32WLE5xx)
+#define LED1_Pin GPIO_PIN_8
+#define LED1_GPIO_Port GPIOA
+#endif
+
#define BUT1_Pin GPIO_PIN_0
#define BUT1_GPIO_Port GPIOA
#define BUT1_EXTI_IRQn EXTI0_IRQn
@@ -111,8 +116,10 @@ void MX_TOF_Init(void);
#endif
/* USER CODE BEGIN Private defines */
-#if defined(STM32WLE5xx) || defined(STM32WL55xx)
+
/* TOF_1 */
+
+#if defined(TOF_1)
#define TOF_C_INT_Pin GPIO_PIN_10
#define TOF_C_INT_GPIO_Port GPIOA
#define TOF_C_INT_EXTI_IRQn EXTI15_10_IRQn
@@ -120,8 +127,9 @@ void MX_TOF_Init(void);
#define TOF_C_XSHUT_GPIO_Port GPIOB
#endif
-#if defined(STM32WL55xx)
+
/* TOF_2 */
+#if defined(TOF_2) && defined(STM32WL55xx)
#define TOF_L_INT_Pin GPIO_PIN_7
#define TOF_L_INT_GPIO_Port GPIOC
#define TOF_L_INT_EXTI_IRQn EXTI9_5_IRQn
@@ -129,8 +137,9 @@ void MX_TOF_Init(void);
#define TOF_L_XSHUT_GPIO_Port GPIOB
#endif
-#if defined(STM32WLE5xx)
+
/* TOF_2 */
+#if defined(TOF_2) && defined(STM32WLE5xx)
#define TOF_L_INT_Pin GPIO_PIN_10
#define TOF_L_INT_GPIO_Port GPIOA
#define TOF_L_INT_EXTI_IRQn EXTI15_10_IRQn
@@ -138,8 +147,9 @@ void MX_TOF_Init(void);
#define TOF_L_XSHUT_GPIO_Port GPIOB
#endif
-#if defined(STM32WL55xx)
+
/* TOF_3 */
+#if defined(STM32WL55xx)&& defined(TOF_3)
#define TOF_R_INT_Pin GPIO_PIN_10
#define TOF_R_INT_GPIO_Port GPIOA
#define TOF_R_INT_EXTI_IRQn EXTI15_10_IRQn
@@ -147,8 +157,9 @@ void MX_TOF_Init(void);
#define TOF_R_XSHUT_GPIO_Port GPIOB
#endif
-#if defined(STM32WLE5xx)
+
/* TOF_3 */
+#if defined(STM32WLE5xx)&& defined(TOF_3)
#define TOF_R_INT_Pin GPIO_PIN_10
#define TOF_R_INT_GPIO_Port GPIOA
#define TOF_R_INT_EXTI_IRQn EXTI15_10_IRQn
diff --git a/Core/Inc/yunhorn_sts_prd_conf.h b/Core/Inc/yunhorn_sts_prd_conf.h
index 83a3cd2..993bb3c 100644
--- a/Core/Inc/yunhorn_sts_prd_conf.h
+++ b/Core/Inc/yunhorn_sts_prd_conf.h
@@ -43,6 +43,8 @@
//#define USE_OLED_SSD1306
//#define USE_TOF_VL53L1X
#define USE_TOF_VL53L0X
+
+ // TOF VL53LX number
//#define USE_MEMS_ADXL345
//#define USE_ACCONEER_A111
/* ########################## Product Selection ############################## */
diff --git a/Core/Src/gpio.c b/Core/Src/gpio.c
index d210751..8247b77 100644
--- a/Core/Src/gpio.c
+++ b/Core/Src/gpio.c
@@ -22,7 +22,7 @@
/* Includes ------------------------------------------------------------------*/
#include "gpio.h"
-
+#include "main.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
@@ -57,7 +57,7 @@ void MX_GPIO_Init(void)
|LED3_Pin, GPIO_PIN_RESET);
#endif
/*Configure GPIO pin Output Level */
-#if (defined(STS_USE_TOF_VL53L0X) || (defined(STS_USE_TOF_VL53L1X))||(defined(STM32WLE5xx)) || (defined(STM32WL55xx)))
+#if defined(TOF_1)
HAL_GPIO_WritePin(TOF_C_XSHUT_GPIO_Port, TOF_C_XSHUT_Pin, GPIO_PIN_SET);
#endif
#if defined(STM32WL55xx)
@@ -95,37 +95,46 @@ void MX_GPIO_Init(void)
/*Configure GPIO pins : TOF_C_INT_Pin */
+#if defined(TOF_1)
GPIO_InitStruct.Pin = TOF_C_INT_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(TOF_C_INT_GPIO_Port, &GPIO_InitStruct);
-#if defined(STM32WL55xx)||defined(STM32WLE5xx)
+#endif
+
/*Configure GPIO pins : TOF_L_INT_Pin */
+#if defined(TOF_2)
GPIO_InitStruct.Pin = TOF_L_INT_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(TOF_L_INT_GPIO_Port, &GPIO_InitStruct);
+#endif
/*Configure GPIO pins : TOF_R_INT_Pin */
+#if defined(TOF_3)
GPIO_InitStruct.Pin = TOF_R_INT_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(TOF_R_INT_GPIO_Port, &GPIO_InitStruct);
#endif
+
/*Configure GPIO pins : TOF_C_XSHUT_Pin */
+#if defined(TOF_1)
GPIO_InitStruct.Pin = TOF_C_XSHUT_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(TOF_C_XSHUT_GPIO_Port, &GPIO_InitStruct);
-
+#endif
/*Configure GPIO pins : TOF_L_XSHUT_Pin */
-#if defined(STM32WL55xx)||defined(STM32WLE5xx)
+#if defined(TOF_2)
GPIO_InitStruct.Pin = TOF_L_XSHUT_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(TOF_L_XSHUT_GPIO_Port, &GPIO_InitStruct);
+#endif
/*Configure GPIO pins : TOF_R_XSHUT_Pin */
+#if defined(TOF_3)
GPIO_InitStruct.Pin = TOF_R_XSHUT_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_PULLUP;
@@ -139,12 +148,17 @@ void MX_GPIO_Init(void)
HAL_NVIC_SetPriority(EXTI1_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI1_IRQn);
-
+#if defined(TOF_1)
HAL_NVIC_SetPriority(TOF_C_INT_EXTI_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(TOF_C_INT_EXTI_IRQn);
-#if defined(STM32WL55xx)||defined(STM32WLE5xx)
+#endif
+
+#if defined(TOF_2)
HAL_NVIC_SetPriority(TOF_L_INT_EXTI_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(TOF_L_INT_EXTI_IRQn);
+#endif
+
+#if defined(TOF_3)
HAL_NVIC_SetPriority(TOF_R_INT_EXTI_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(TOF_R_INT_EXTI_IRQn);
#endif
diff --git a/STM32CubeIDE/.cproject b/STM32CubeIDE/.cproject
index 6a0e046..fed010b 100644
--- a/STM32CubeIDE/.cproject
+++ b/STM32CubeIDE/.cproject
@@ -131,6 +131,8 @@