diff --git a/Core/Src/dma.c b/Core/Src/dma.c
index 0aad753..cffe8a6 100644
--- a/Core/Src/dma.c
+++ b/Core/Src/dma.c
@@ -80,8 +80,8 @@ void MX_DMA_Init(void)
    //HAL_NVIC_SetPriority(DMA1_Channel7_IRQn, 0, 0);
    //HAL_NVIC_EnableIRQ(DMA1_Channel7_IRQn);
 
-  HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 2, 0);
-  HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn);
+  // HAL_NVIC_SetPriority(DMA1_Channel5_IRQn, 2, 0);
+  // HAL_NVIC_EnableIRQ(DMA1_Channel5_IRQn);
 
 
 }
diff --git a/Core/Src/gpio.c b/Core/Src/gpio.c
index 287b9b3..0bdc85f 100644
--- a/Core/Src/gpio.c
+++ b/Core/Src/gpio.c
@@ -55,6 +55,9 @@ void MX_GPIO_Init(void)
 #ifdef	L8
   HAL_GPIO_WritePin(VL53L8A1_PWR_EN_C_PORT, VL53L8A1_PWR_EN_C_PIN, GPIO_PIN_SET);
   HAL_GPIO_WritePin(VL53L8A1_LPn_C_PORT, VL53L8A1_LPn_C_PIN, GPIO_PIN_SET);
+#endif
+#ifdef STS_P2
+  HAL_GPIO_WritePin(TOF_C_XSHUT_GPIO_Port, TOF_C_XSHUT_Pin, GPIO_PIN_SET);
 #endif
   /*Configure GPIO pins : PBPin PBPin PBPin */
   GPIO_InitStruct.Pin = LED1_Pin|LED2_Pin|LED3_Pin;
@@ -140,13 +143,14 @@ void MX_GPIO_Init(void)
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
   HAL_GPIO_Init(TOF_C_XSHUT_GPIO_Port, &GPIO_InitStruct);
 
+#if 0
 	/*Configure GPIO pins : TOF_L_XSHUT_Pin */
   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
 
   HAL_NVIC_SetPriority(TOF_C_INT_EXTI_IRQn, 0, 0);
   HAL_NVIC_EnableIRQ(TOF_C_INT_EXTI_IRQn);
@@ -205,6 +209,8 @@ void MX_GPIO_Init(void)
   HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0);
   HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
 
+  HAL_NVIC_SetPriority(TOF_C_INT_EXTI_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(TOF_C_INT_EXTI_IRQn);
 
 //  HAL_NVIC_SetPriority(EXTI9_5_IRQn, 0, 0);
 //  HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
diff --git a/Core/Src/i2c.c b/Core/Src/i2c.c
index 2713947..d30aea8 100644
--- a/Core/Src/i2c.c
+++ b/Core/Src/i2c.c
@@ -46,7 +46,7 @@ void MX_I2C2_Init(void)
 #ifdef L8
   hi2c2.Init.Timing = I2C2_FAST_PLUS_1M;
 #else
-  hi2c2.Init.Timing = I2C2_STANDARD_100K;
+  hi2c2.Init.Timing = I2C2_FAST_400K;
 #endif
   hi2c2.Init.OwnAddress1 = 0;
   hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 390eb02..28a417a 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -114,10 +114,12 @@ int main(void)
 	  MX_DMA_Init();
       MX_LoRaWAN_Init();
   }
-//PME_ON;
-//WATER_LEAKAGE_ENABLE;
+
   /* USER CODE BEGIN 2 */
   //MX_USART2_UART_Init();
+
+  STS_TOF_VL53LX_PeopleCounting_Process_Init();
+
   /* USER CODE END 2 */
 
   /* Infinite loop */
diff --git a/Core/Src/sys_app.c b/Core/Src/sys_app.c
index 8b99d65..2e22904 100644
--- a/Core/Src/sys_app.c
+++ b/Core/Src/sys_app.c
@@ -129,7 +129,7 @@ void SystemApp_Init(void)
   MX_TOF_Init();
 #endif
 #if	defined(STS_P2)
-  STS_TOF_VL53LX_PeopleCounting_Process_Init();
+  //STS_TOF_VL53LX_PeopleCounting_Process_Init();
 #endif
 
   /*Init low power manager*/
diff --git a/LoRaWAN/App/lora_app.c b/LoRaWAN/App/lora_app.c
index 2d04230..aeb475a 100644
--- a/LoRaWAN/App/lora_app.c
+++ b/LoRaWAN/App/lora_app.c
@@ -1096,7 +1096,7 @@ static void OnTxTimerEvent(void *context)
   /* USER CODE BEGIN OnTxTimerEvent_1 */
 
   /* USER CODE END OnTxTimerEvent_1 */
-
+	  UTIL_TIMER_Stop(&TxTimer);
 #ifdef STS_R4
   UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_YunhornSTSEventP6), CFG_SEQ_Prio_0);
 #elif defined(STS_M1)
diff --git a/STM32CubeIDE/.cproject b/STM32CubeIDE/.cproject
index 61e853c..6b533d7 100644
--- a/STM32CubeIDE/.cproject
+++ b/STM32CubeIDE/.cproject
@@ -99,7 +99,7 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry excluding="Application/User/STS/TOF/vl53l0x" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+						<entry excluding="Drivers/BSP/Components|Drivers/BSP/53L8A1|Application/User/STS/TOF/App/X-WL55_WLE5_53L0X.c|Application/User/STS/TOF/App/app_tof_vl53l0x_range.c|Application/User/STS/TOF/vl53l0x" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
@@ -213,7 +213,7 @@
 						</toolChain>
 					</folderInfo>
 					<sourceEntries>
-						<entry excluding="Application/User/STS/TOF/vl53l0x" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
+						<entry excluding="Drivers/BSP/Components|Drivers/BSP/53L8A1|Application/User/STS/TOF/App/X-WL55_WLE5_53L0X.c|Application/User/STS/TOF/App/app_tof_vl53l0x_range.c|Application/User/STS/TOF/vl53l0x" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name=""/>
 					</sourceEntries>
 				</configuration>
 			</storageModule>
diff --git a/STM32CubeIDE/.project b/STM32CubeIDE/.project
index d7be60c..7a0a531 100644
--- a/STM32CubeIDE/.project
+++ b/STM32CubeIDE/.project
@@ -442,11 +442,6 @@
 			<type>1</type>
 			<locationURI>copy_PARENT/Core/Src/stm32wlxx_it.c</locationURI>
 		</link>
-		<link>
-			<name>Application/User/Core/stm32wlxx_nucleo_bus.c</name>
-			<type>1</type>
-			<locationURI>PARENT-1-PROJECT_LOC/Core/Src/stm32wlxx_nucleo_bus.c</locationURI>
-		</link>
 		<link>
 			<name>Application/User/Core/subghz.c</name>
 			<type>1</type>
@@ -580,7 +575,7 @@
 		<link>
 			<name>Application/User/STS/TOF/App/app_tof.c</name>
 			<type>1</type>
-			<location>D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/STS/TOF/App/app_tof.c</location>
+			<locationURI>copy_PARENT/STS/TOF/App/app_tof.c</locationURI>
 		</link>
 		<link>
 			<name>Application/User/STS/TOF/App/app_tof.h</name>
diff --git a/STM32CubeIDE/.settings/language.settings.xml b/STM32CubeIDE/.settings/language.settings.xml
index 0ec3934..f1b15d8 100644
--- a/STM32CubeIDE/.settings/language.settings.xml
+++ b/STM32CubeIDE/.settings/language.settings.xml
@@ -5,7 +5,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1048471218779752885" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1581009204476899933" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
@@ -16,7 +16,7 @@
 			<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
 			<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
 			<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
-			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="1048471218779752885" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
+			<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="-1581009204476899933" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
 				<language-scope id="org.eclipse.cdt.core.gcc"/>
 				<language-scope id="org.eclipse.cdt.core.g++"/>
 			</provider>
diff --git a/STM32CubeIDE/Release/Application/User/Core/subdir.mk b/STM32CubeIDE/Release/Application/User/Core/subdir.mk
index 838f30b..9aac18b 100644
--- a/STM32CubeIDE/Release/Application/User/Core/subdir.mk
+++ b/STM32CubeIDE/Release/Application/User/Core/subdir.mk
@@ -16,7 +16,6 @@ D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_S
 D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/Core/Src/stm32_lpm_if.c \
 D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/Core/Src/stm32wlxx_hal_msp.c \
 D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/Core/Src/stm32wlxx_it.c \
-D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/Core/Src/stm32wlxx_nucleo_bus.c \
 D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/Core/Src/subghz.c \
 D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/Core/Src/sys_app.c \
 D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/Core/Src/sys_debug.c \
@@ -39,7 +38,6 @@ OBJS += \
 ./Application/User/Core/stm32_lpm_if.o \
 ./Application/User/Core/stm32wlxx_hal_msp.o \
 ./Application/User/Core/stm32wlxx_it.o \
-./Application/User/Core/stm32wlxx_nucleo_bus.o \
 ./Application/User/Core/subghz.o \
 ./Application/User/Core/sys_app.o \
 ./Application/User/Core/sys_debug.o \
@@ -62,7 +60,6 @@ C_DEPS += \
 ./Application/User/Core/stm32_lpm_if.d \
 ./Application/User/Core/stm32wlxx_hal_msp.d \
 ./Application/User/Core/stm32wlxx_it.d \
-./Application/User/Core/stm32wlxx_nucleo_bus.d \
 ./Application/User/Core/subghz.d \
 ./Application/User/Core/sys_app.d \
 ./Application/User/Core/sys_debug.d \
@@ -97,8 +94,6 @@ Application/User/Core/stm32wlxx_hal_msp.o: D:/ONEDRIVE/STM32WLV13/Projects/NUCLE
 	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -DSTS_P2 -DCORE_CM4 -DUSE_HAL_DRIVER -DSTM32WLE5xx -c -I../../Core/Inc -I../../STS/Core/Inc -I../../STS/TOF/App -I../../STS/TOF/Target -I../../STS/TOF/vl53l1x_uld -I../../STS/TOF/vl53l0x -I../../LoRaWAN/App -I../../LoRaWAN/Target -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc/Legacy -I../../../../../../../Utilities/trace/adv_trace -I../../../../../../../Utilities/misc -I../../../../../../../Utilities/sequencer -I../../../../../../../Utilities/timer -I../../../../../../../Utilities/lpm/tiny_lpm -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler/Packages -I../../../../../../../Drivers/CMSIS/Device/ST/STM32WLxx/Include -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Crypto -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac/Region -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Utilities -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy/stm32_radio_driver -I../../../../../../../Middlewares/ST/STM32_Cryptographic/include -I../../../../../../../Drivers/CMSIS/Include -I../../../../../../../Drivers/BSP/STM32WLxx_Nucleo -I../../../../../../../Drivers/BSP/Components/vl53l8cx/porting -I../../../../../../../Drivers/BSP/Components/Common -I../../../../../../../Drivers/BSP/53L8A1 -I../../../../../../../Drivers/BSP/Components/vl53l8cx/modules -I../../../../../../../Drivers/BSP/Components/vl53l8cx -Os -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfloat-abi=soft -mthumb -o "$@"
 Application/User/Core/stm32wlxx_it.o: D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/Core/Src/stm32wlxx_it.c Application/User/Core/subdir.mk
 	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -DSTS_P2 -DCORE_CM4 -DUSE_HAL_DRIVER -DSTM32WLE5xx -c -I../../Core/Inc -I../../STS/Core/Inc -I../../STS/TOF/App -I../../STS/TOF/Target -I../../STS/TOF/vl53l1x_uld -I../../STS/TOF/vl53l0x -I../../LoRaWAN/App -I../../LoRaWAN/Target -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc/Legacy -I../../../../../../../Utilities/trace/adv_trace -I../../../../../../../Utilities/misc -I../../../../../../../Utilities/sequencer -I../../../../../../../Utilities/timer -I../../../../../../../Utilities/lpm/tiny_lpm -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler/Packages -I../../../../../../../Drivers/CMSIS/Device/ST/STM32WLxx/Include -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Crypto -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac/Region -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Utilities -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy/stm32_radio_driver -I../../../../../../../Middlewares/ST/STM32_Cryptographic/include -I../../../../../../../Drivers/CMSIS/Include -I../../../../../../../Drivers/BSP/STM32WLxx_Nucleo -I../../../../../../../Drivers/BSP/Components/vl53l8cx/porting -I../../../../../../../Drivers/BSP/Components/Common -I../../../../../../../Drivers/BSP/53L8A1 -I../../../../../../../Drivers/BSP/Components/vl53l8cx/modules -I../../../../../../../Drivers/BSP/Components/vl53l8cx -Os -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfloat-abi=soft -mthumb -o "$@"
-Application/User/Core/stm32wlxx_nucleo_bus.o: D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/Core/Src/stm32wlxx_nucleo_bus.c Application/User/Core/subdir.mk
-	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -DSTS_P2 -DCORE_CM4 -DUSE_HAL_DRIVER -DSTM32WLE5xx -c -I../../Core/Inc -I../../STS/Core/Inc -I../../STS/TOF/App -I../../STS/TOF/Target -I../../STS/TOF/vl53l1x_uld -I../../STS/TOF/vl53l0x -I../../LoRaWAN/App -I../../LoRaWAN/Target -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc/Legacy -I../../../../../../../Utilities/trace/adv_trace -I../../../../../../../Utilities/misc -I../../../../../../../Utilities/sequencer -I../../../../../../../Utilities/timer -I../../../../../../../Utilities/lpm/tiny_lpm -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler/Packages -I../../../../../../../Drivers/CMSIS/Device/ST/STM32WLxx/Include -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Crypto -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac/Region -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Utilities -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy/stm32_radio_driver -I../../../../../../../Middlewares/ST/STM32_Cryptographic/include -I../../../../../../../Drivers/CMSIS/Include -I../../../../../../../Drivers/BSP/STM32WLxx_Nucleo -I../../../../../../../Drivers/BSP/Components/vl53l8cx/porting -I../../../../../../../Drivers/BSP/Components/Common -I../../../../../../../Drivers/BSP/53L8A1 -I../../../../../../../Drivers/BSP/Components/vl53l8cx/modules -I../../../../../../../Drivers/BSP/Components/vl53l8cx -Os -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfloat-abi=soft -mthumb -o "$@"
 Application/User/Core/subghz.o: D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/Core/Src/subghz.c Application/User/Core/subdir.mk
 	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -DSTS_P2 -DCORE_CM4 -DUSE_HAL_DRIVER -DSTM32WLE5xx -c -I../../Core/Inc -I../../STS/Core/Inc -I../../STS/TOF/App -I../../STS/TOF/Target -I../../STS/TOF/vl53l1x_uld -I../../STS/TOF/vl53l0x -I../../LoRaWAN/App -I../../LoRaWAN/Target -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc/Legacy -I../../../../../../../Utilities/trace/adv_trace -I../../../../../../../Utilities/misc -I../../../../../../../Utilities/sequencer -I../../../../../../../Utilities/timer -I../../../../../../../Utilities/lpm/tiny_lpm -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler/Packages -I../../../../../../../Drivers/CMSIS/Device/ST/STM32WLxx/Include -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Crypto -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac/Region -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Utilities -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy/stm32_radio_driver -I../../../../../../../Middlewares/ST/STM32_Cryptographic/include -I../../../../../../../Drivers/CMSIS/Include -I../../../../../../../Drivers/BSP/STM32WLxx_Nucleo -I../../../../../../../Drivers/BSP/Components/vl53l8cx/porting -I../../../../../../../Drivers/BSP/Components/Common -I../../../../../../../Drivers/BSP/53L8A1 -I../../../../../../../Drivers/BSP/Components/vl53l8cx/modules -I../../../../../../../Drivers/BSP/Components/vl53l8cx -Os -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfloat-abi=soft -mthumb -o "$@"
 Application/User/Core/sys_app.o: D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/Core/Src/sys_app.c Application/User/Core/subdir.mk
@@ -119,7 +114,7 @@ Application/User/Core/usart_if.o: D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/
 clean: clean-Application-2f-User-2f-Core
 
 clean-Application-2f-User-2f-Core:
-	-$(RM) ./Application/User/Core/adc.cyclo ./Application/User/Core/adc.d ./Application/User/Core/adc.o ./Application/User/Core/adc.su ./Application/User/Core/adc_if.cyclo ./Application/User/Core/adc_if.d ./Application/User/Core/adc_if.o ./Application/User/Core/adc_if.su ./Application/User/Core/dma.cyclo ./Application/User/Core/dma.d ./Application/User/Core/dma.o ./Application/User/Core/dma.su ./Application/User/Core/flash_if.cyclo ./Application/User/Core/flash_if.d ./Application/User/Core/flash_if.o ./Application/User/Core/flash_if.su ./Application/User/Core/gpio.cyclo ./Application/User/Core/gpio.d ./Application/User/Core/gpio.o ./Application/User/Core/gpio.su ./Application/User/Core/i2c.cyclo ./Application/User/Core/i2c.d ./Application/User/Core/i2c.o ./Application/User/Core/i2c.su ./Application/User/Core/main.cyclo ./Application/User/Core/main.d ./Application/User/Core/main.o ./Application/User/Core/main.su ./Application/User/Core/rtc.cyclo ./Application/User/Core/rtc.d ./Application/User/Core/rtc.o ./Application/User/Core/rtc.su ./Application/User/Core/stm32_lpm_if.cyclo ./Application/User/Core/stm32_lpm_if.d ./Application/User/Core/stm32_lpm_if.o ./Application/User/Core/stm32_lpm_if.su ./Application/User/Core/stm32wlxx_hal_msp.cyclo ./Application/User/Core/stm32wlxx_hal_msp.d ./Application/User/Core/stm32wlxx_hal_msp.o ./Application/User/Core/stm32wlxx_hal_msp.su ./Application/User/Core/stm32wlxx_it.cyclo ./Application/User/Core/stm32wlxx_it.d ./Application/User/Core/stm32wlxx_it.o ./Application/User/Core/stm32wlxx_it.su ./Application/User/Core/stm32wlxx_nucleo_bus.cyclo ./Application/User/Core/stm32wlxx_nucleo_bus.d ./Application/User/Core/stm32wlxx_nucleo_bus.o ./Application/User/Core/stm32wlxx_nucleo_bus.su ./Application/User/Core/subghz.cyclo ./Application/User/Core/subghz.d ./Application/User/Core/subghz.o ./Application/User/Core/subghz.su ./Application/User/Core/sys_app.cyclo ./Application/User/Core/sys_app.d ./Application/User/Core/sys_app.o ./Application/User/Core/sys_app.su ./Application/User/Core/sys_debug.cyclo ./Application/User/Core/sys_debug.d ./Application/User/Core/sys_debug.o ./Application/User/Core/sys_debug.su ./Application/User/Core/sys_sensors.cyclo ./Application/User/Core/sys_sensors.d ./Application/User/Core/sys_sensors.o ./Application/User/Core/sys_sensors.su ./Application/User/Core/syscalls.cyclo ./Application/User/Core/syscalls.d ./Application/User/Core/syscalls.o ./Application/User/Core/syscalls.su ./Application/User/Core/sysmem.cyclo ./Application/User/Core/sysmem.d ./Application/User/Core/sysmem.o ./Application/User/Core/sysmem.su ./Application/User/Core/timer_if.cyclo ./Application/User/Core/timer_if.d ./Application/User/Core/timer_if.o ./Application/User/Core/timer_if.su ./Application/User/Core/usart.cyclo ./Application/User/Core/usart.d ./Application/User/Core/usart.o ./Application/User/Core/usart.su ./Application/User/Core/usart_if.cyclo ./Application/User/Core/usart_if.d ./Application/User/Core/usart_if.o ./Application/User/Core/usart_if.su
+	-$(RM) ./Application/User/Core/adc.cyclo ./Application/User/Core/adc.d ./Application/User/Core/adc.o ./Application/User/Core/adc.su ./Application/User/Core/adc_if.cyclo ./Application/User/Core/adc_if.d ./Application/User/Core/adc_if.o ./Application/User/Core/adc_if.su ./Application/User/Core/dma.cyclo ./Application/User/Core/dma.d ./Application/User/Core/dma.o ./Application/User/Core/dma.su ./Application/User/Core/flash_if.cyclo ./Application/User/Core/flash_if.d ./Application/User/Core/flash_if.o ./Application/User/Core/flash_if.su ./Application/User/Core/gpio.cyclo ./Application/User/Core/gpio.d ./Application/User/Core/gpio.o ./Application/User/Core/gpio.su ./Application/User/Core/i2c.cyclo ./Application/User/Core/i2c.d ./Application/User/Core/i2c.o ./Application/User/Core/i2c.su ./Application/User/Core/main.cyclo ./Application/User/Core/main.d ./Application/User/Core/main.o ./Application/User/Core/main.su ./Application/User/Core/rtc.cyclo ./Application/User/Core/rtc.d ./Application/User/Core/rtc.o ./Application/User/Core/rtc.su ./Application/User/Core/stm32_lpm_if.cyclo ./Application/User/Core/stm32_lpm_if.d ./Application/User/Core/stm32_lpm_if.o ./Application/User/Core/stm32_lpm_if.su ./Application/User/Core/stm32wlxx_hal_msp.cyclo ./Application/User/Core/stm32wlxx_hal_msp.d ./Application/User/Core/stm32wlxx_hal_msp.o ./Application/User/Core/stm32wlxx_hal_msp.su ./Application/User/Core/stm32wlxx_it.cyclo ./Application/User/Core/stm32wlxx_it.d ./Application/User/Core/stm32wlxx_it.o ./Application/User/Core/stm32wlxx_it.su ./Application/User/Core/subghz.cyclo ./Application/User/Core/subghz.d ./Application/User/Core/subghz.o ./Application/User/Core/subghz.su ./Application/User/Core/sys_app.cyclo ./Application/User/Core/sys_app.d ./Application/User/Core/sys_app.o ./Application/User/Core/sys_app.su ./Application/User/Core/sys_debug.cyclo ./Application/User/Core/sys_debug.d ./Application/User/Core/sys_debug.o ./Application/User/Core/sys_debug.su ./Application/User/Core/sys_sensors.cyclo ./Application/User/Core/sys_sensors.d ./Application/User/Core/sys_sensors.o ./Application/User/Core/sys_sensors.su ./Application/User/Core/syscalls.cyclo ./Application/User/Core/syscalls.d ./Application/User/Core/syscalls.o ./Application/User/Core/syscalls.su ./Application/User/Core/sysmem.cyclo ./Application/User/Core/sysmem.d ./Application/User/Core/sysmem.o ./Application/User/Core/sysmem.su ./Application/User/Core/timer_if.cyclo ./Application/User/Core/timer_if.d ./Application/User/Core/timer_if.o ./Application/User/Core/timer_if.su ./Application/User/Core/usart.cyclo ./Application/User/Core/usart.d ./Application/User/Core/usart.o ./Application/User/Core/usart.su ./Application/User/Core/usart_if.cyclo ./Application/User/Core/usart_if.d ./Application/User/Core/usart_if.o ./Application/User/Core/usart_if.su
 
 .PHONY: clean-Application-2f-User-2f-Core
 
diff --git a/STM32CubeIDE/Release/Application/User/STS/TOF/App/subdir.mk b/STM32CubeIDE/Release/Application/User/STS/TOF/App/subdir.mk
index dfe9ec4..21f8268 100644
--- a/STM32CubeIDE/Release/Application/User/STS/TOF/App/subdir.mk
+++ b/STM32CubeIDE/Release/Application/User/STS/TOF/App/subdir.mk
@@ -5,38 +5,28 @@
 
 # Add inputs and outputs from these tool invocations to the build variables 
 C_SRCS += \
-D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/STS/TOF/App/X-WL55_WLE5_53L0X.c \
 D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/STS/TOF/App/app_tof.c \
-D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/STS/TOF/App/app_tof_peoplecount.c \
-D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/STS/TOF/App/app_tof_vl53l0x_range.c 
+D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/STS/TOF/App/app_tof_peoplecount.c 
 
 OBJS += \
-./Application/User/STS/TOF/App/X-WL55_WLE5_53L0X.o \
 ./Application/User/STS/TOF/App/app_tof.o \
-./Application/User/STS/TOF/App/app_tof_peoplecount.o \
-./Application/User/STS/TOF/App/app_tof_vl53l0x_range.o 
+./Application/User/STS/TOF/App/app_tof_peoplecount.o 
 
 C_DEPS += \
-./Application/User/STS/TOF/App/X-WL55_WLE5_53L0X.d \
 ./Application/User/STS/TOF/App/app_tof.d \
-./Application/User/STS/TOF/App/app_tof_peoplecount.d \
-./Application/User/STS/TOF/App/app_tof_vl53l0x_range.d 
+./Application/User/STS/TOF/App/app_tof_peoplecount.d 
 
 
 # Each subdirectory must supply rules for building sources it contributes
-Application/User/STS/TOF/App/X-WL55_WLE5_53L0X.o: D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/STS/TOF/App/X-WL55_WLE5_53L0X.c Application/User/STS/TOF/App/subdir.mk
-	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -DSTS_P2 -DCORE_CM4 -DUSE_HAL_DRIVER -DSTM32WLE5xx -c -I../../Core/Inc -I../../STS/Core/Inc -I../../STS/TOF/App -I../../STS/TOF/Target -I../../STS/TOF/vl53l1x_uld -I../../STS/TOF/vl53l0x -I../../LoRaWAN/App -I../../LoRaWAN/Target -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc/Legacy -I../../../../../../../Utilities/trace/adv_trace -I../../../../../../../Utilities/misc -I../../../../../../../Utilities/sequencer -I../../../../../../../Utilities/timer -I../../../../../../../Utilities/lpm/tiny_lpm -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler/Packages -I../../../../../../../Drivers/CMSIS/Device/ST/STM32WLxx/Include -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Crypto -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac/Region -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Utilities -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy/stm32_radio_driver -I../../../../../../../Middlewares/ST/STM32_Cryptographic/include -I../../../../../../../Drivers/CMSIS/Include -I../../../../../../../Drivers/BSP/STM32WLxx_Nucleo -I../../../../../../../Drivers/BSP/Components/vl53l8cx/porting -I../../../../../../../Drivers/BSP/Components/Common -I../../../../../../../Drivers/BSP/53L8A1 -I../../../../../../../Drivers/BSP/Components/vl53l8cx/modules -I../../../../../../../Drivers/BSP/Components/vl53l8cx -Os -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfloat-abi=soft -mthumb -o "$@"
 Application/User/STS/TOF/App/app_tof.o: D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/STS/TOF/App/app_tof.c Application/User/STS/TOF/App/subdir.mk
 	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -DSTS_P2 -DCORE_CM4 -DUSE_HAL_DRIVER -DSTM32WLE5xx -c -I../../Core/Inc -I../../STS/Core/Inc -I../../STS/TOF/App -I../../STS/TOF/Target -I../../STS/TOF/vl53l1x_uld -I../../STS/TOF/vl53l0x -I../../LoRaWAN/App -I../../LoRaWAN/Target -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc/Legacy -I../../../../../../../Utilities/trace/adv_trace -I../../../../../../../Utilities/misc -I../../../../../../../Utilities/sequencer -I../../../../../../../Utilities/timer -I../../../../../../../Utilities/lpm/tiny_lpm -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler/Packages -I../../../../../../../Drivers/CMSIS/Device/ST/STM32WLxx/Include -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Crypto -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac/Region -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Utilities -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy/stm32_radio_driver -I../../../../../../../Middlewares/ST/STM32_Cryptographic/include -I../../../../../../../Drivers/CMSIS/Include -I../../../../../../../Drivers/BSP/STM32WLxx_Nucleo -I../../../../../../../Drivers/BSP/Components/vl53l8cx/porting -I../../../../../../../Drivers/BSP/Components/Common -I../../../../../../../Drivers/BSP/53L8A1 -I../../../../../../../Drivers/BSP/Components/vl53l8cx/modules -I../../../../../../../Drivers/BSP/Components/vl53l8cx -Os -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfloat-abi=soft -mthumb -o "$@"
 Application/User/STS/TOF/App/app_tof_peoplecount.o: D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/STS/TOF/App/app_tof_peoplecount.c Application/User/STS/TOF/App/subdir.mk
 	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -DSTS_P2 -DCORE_CM4 -DUSE_HAL_DRIVER -DSTM32WLE5xx -c -I../../Core/Inc -I../../STS/Core/Inc -I../../STS/TOF/App -I../../STS/TOF/Target -I../../STS/TOF/vl53l1x_uld -I../../STS/TOF/vl53l0x -I../../LoRaWAN/App -I../../LoRaWAN/Target -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc/Legacy -I../../../../../../../Utilities/trace/adv_trace -I../../../../../../../Utilities/misc -I../../../../../../../Utilities/sequencer -I../../../../../../../Utilities/timer -I../../../../../../../Utilities/lpm/tiny_lpm -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler/Packages -I../../../../../../../Drivers/CMSIS/Device/ST/STM32WLxx/Include -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Crypto -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac/Region -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Utilities -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy/stm32_radio_driver -I../../../../../../../Middlewares/ST/STM32_Cryptographic/include -I../../../../../../../Drivers/CMSIS/Include -I../../../../../../../Drivers/BSP/STM32WLxx_Nucleo -I../../../../../../../Drivers/BSP/Components/vl53l8cx/porting -I../../../../../../../Drivers/BSP/Components/Common -I../../../../../../../Drivers/BSP/53L8A1 -I../../../../../../../Drivers/BSP/Components/vl53l8cx/modules -I../../../../../../../Drivers/BSP/Components/vl53l8cx -Os -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfloat-abi=soft -mthumb -o "$@"
-Application/User/STS/TOF/App/app_tof_vl53l0x_range.o: D:/ONEDRIVE/STM32WLV13/Projects/NUCLEO-WL55JC/Applications/LoRaWAN/WLE5CC_NODE_STS/STS/TOF/App/app_tof_vl53l0x_range.c Application/User/STS/TOF/App/subdir.mk
-	arm-none-eabi-gcc "$<" -mcpu=cortex-m4 -std=gnu11 -DSTS_P2 -DCORE_CM4 -DUSE_HAL_DRIVER -DSTM32WLE5xx -c -I../../Core/Inc -I../../STS/Core/Inc -I../../STS/TOF/App -I../../STS/TOF/Target -I../../STS/TOF/vl53l1x_uld -I../../STS/TOF/vl53l0x -I../../LoRaWAN/App -I../../LoRaWAN/Target -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc -I../../../../../../../Drivers/STM32WLxx_HAL_Driver/Inc/Legacy -I../../../../../../../Utilities/trace/adv_trace -I../../../../../../../Utilities/misc -I../../../../../../../Utilities/sequencer -I../../../../../../../Utilities/timer -I../../../../../../../Utilities/lpm/tiny_lpm -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler/Packages -I../../../../../../../Drivers/CMSIS/Device/ST/STM32WLxx/Include -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Crypto -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac/Region -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Mac -I../../../../../../../Middlewares/Third_Party/LoRaWAN/LmHandler -I../../../../../../../Middlewares/Third_Party/LoRaWAN/Utilities -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy -I../../../../../../../Middlewares/Third_Party/SubGHz_Phy/stm32_radio_driver -I../../../../../../../Middlewares/ST/STM32_Cryptographic/include -I../../../../../../../Drivers/CMSIS/Include -I../../../../../../../Drivers/BSP/STM32WLxx_Nucleo -I../../../../../../../Drivers/BSP/Components/vl53l8cx/porting -I../../../../../../../Drivers/BSP/Components/Common -I../../../../../../../Drivers/BSP/53L8A1 -I../../../../../../../Drivers/BSP/Components/vl53l8cx/modules -I../../../../../../../Drivers/BSP/Components/vl53l8cx -Os -ffunction-sections -fdata-sections -Wall -fstack-usage -fcyclomatic-complexity -MMD -MP -MF"$(@:%.o=%.d)" -MT"$@" --specs=nano.specs -mfloat-abi=soft -mthumb -o "$@"
 
 clean: clean-Application-2f-User-2f-STS-2f-TOF-2f-App
 
 clean-Application-2f-User-2f-STS-2f-TOF-2f-App:
-	-$(RM) ./Application/User/STS/TOF/App/X-WL55_WLE5_53L0X.cyclo ./Application/User/STS/TOF/App/X-WL55_WLE5_53L0X.d ./Application/User/STS/TOF/App/X-WL55_WLE5_53L0X.o ./Application/User/STS/TOF/App/X-WL55_WLE5_53L0X.su ./Application/User/STS/TOF/App/app_tof.cyclo ./Application/User/STS/TOF/App/app_tof.d ./Application/User/STS/TOF/App/app_tof.o ./Application/User/STS/TOF/App/app_tof.su ./Application/User/STS/TOF/App/app_tof_peoplecount.cyclo ./Application/User/STS/TOF/App/app_tof_peoplecount.d ./Application/User/STS/TOF/App/app_tof_peoplecount.o ./Application/User/STS/TOF/App/app_tof_peoplecount.su ./Application/User/STS/TOF/App/app_tof_vl53l0x_range.cyclo ./Application/User/STS/TOF/App/app_tof_vl53l0x_range.d ./Application/User/STS/TOF/App/app_tof_vl53l0x_range.o ./Application/User/STS/TOF/App/app_tof_vl53l0x_range.su
+	-$(RM) ./Application/User/STS/TOF/App/app_tof.cyclo ./Application/User/STS/TOF/App/app_tof.d ./Application/User/STS/TOF/App/app_tof.o ./Application/User/STS/TOF/App/app_tof.su ./Application/User/STS/TOF/App/app_tof_peoplecount.cyclo ./Application/User/STS/TOF/App/app_tof_peoplecount.d ./Application/User/STS/TOF/App/app_tof_peoplecount.o ./Application/User/STS/TOF/App/app_tof_peoplecount.su
 
 .PHONY: clean-Application-2f-User-2f-STS-2f-TOF-2f-App
 
diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf
index 812681e..1f831c7 100644
Binary files a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf and b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf differ
diff --git a/STM32CubeIDE/Release/makefile b/STM32CubeIDE/Release/makefile
index db0fb8f..db9d122 100644
--- a/STM32CubeIDE/Release/makefile
+++ b/STM32CubeIDE/Release/makefile
@@ -14,9 +14,6 @@ RM := rm -rf
 -include Middlewares/LoRaWAN/subdir.mk
 -include Drivers/STM32WLxx_HAL_Driver/subdir.mk
 -include Drivers/CMSIS/subdir.mk
--include Drivers/BSP/STM32WLxx_Nucleo/subdir.mk
--include Drivers/BSP/Components/subdir.mk
--include Drivers/BSP/53L8A1/subdir.mk
 -include Application/User/Startup/subdir.mk
 -include Application/User/STS/TOF/vl53l1x_uld/subdir.mk
 -include Application/User/STS/TOF/Target/subdir.mk
diff --git a/STM32CubeIDE/Release/sources.mk b/STM32CubeIDE/Release/sources.mk
index cfb8dcf..709eaad 100644
--- a/STM32CubeIDE/Release/sources.mk
+++ b/STM32CubeIDE/Release/sources.mk
@@ -30,9 +30,6 @@ Application/User/STS/TOF/App \
 Application/User/STS/TOF/Target \
 Application/User/STS/TOF/vl53l1x_uld \
 Application/User/Startup \
-Drivers/BSP/53L8A1 \
-Drivers/BSP/Components \
-Drivers/BSP/STM32WLxx_Nucleo \
 Drivers/CMSIS \
 Drivers/STM32WLxx_HAL_Driver \
 Middlewares/LoRaWAN \
diff --git a/STS/TOF/App/app_tof_peoplecount.c b/STS/TOF/App/app_tof_peoplecount.c
index b51bd60..991ab2b 100644
--- a/STS/TOF/App/app_tof_peoplecount.c
+++ b/STS/TOF/App/app_tof_peoplecount.c
@@ -624,11 +624,12 @@ void STS_TOF_VL53LX_PeopleCounting_Process_Start(void)
 
 	//APP_LOG(TS_OFF, VLEVEL_M,"###############   TOF  VL53LX_ PEOPLE COUNTING SUB-PROCESS \r\n");
 	{
+		uint8_t status =0;
 		status = sts_tof_vl53lx_peoplecount_start();
 		if (status !=0)
 		{
 			sts_people_count_sensor_data.Count_Valid = 0;
-			APP_LOG(TS_OFF, VLEVEL_M,"# Counting time out... \r\n");
+			//APP_LOG(TS_OFF, VLEVEL_M,"# Counting time out... \r\n");
 		}
 	}
 
@@ -752,11 +753,11 @@ int sts_tof_vl53lx_peoplecount_init(void)
 
   // Those basic I2C read functions can be used to check your own I2C functions */
   status = VL53L1_RdByte(dev, 0x010F, &byteData);
-  APP_LOG(TS_OFF, VLEVEL_H,"VL53L1X Model_ID: %X\n", byteData);
+  APP_LOG(TS_OFF, VLEVEL_M,"VL53L1X Model_ID: %X\n", byteData);
   status = VL53L1_RdByte(dev, 0x0110, &byteData);
-  APP_LOG(TS_OFF, VLEVEL_H,"VL53L1X Module_Type: %X\n", byteData);
+  APP_LOG(TS_OFF, VLEVEL_M,"VL53L1X Module_Type: %X\n", byteData);
   status = VL53L1_RdWord(dev, 0x010F, &wordData);
-  APP_LOG(TS_OFF, VLEVEL_H,"VL53L1X: %X\n", wordData);
+  APP_LOG(TS_OFF, VLEVEL_M,"VL53L1X: %X\n", wordData);
   sensor_id = wordData;
   wordData = 0;
   while (sensorState == 0) {
@@ -768,7 +769,7 @@ int sts_tof_vl53lx_peoplecount_init(void)
     	return status;
     }
   }
-  APP_LOG(TS_OFF, VLEVEL_H,"\nChip booted\n\n");
+  APP_LOG(TS_OFF, VLEVEL_M,"\nChip booted\n\n");
   
   /* Initialize and configure the device according to people counting need */
   status = VL53L1X_SensorInit(dev);
@@ -806,6 +807,7 @@ int sts_tof_vl53lx_peoplecount_start(void)
 	  uint16_t Distance, Signal;
 	  uint8_t RangeStatus;
 	  uint8_t dataReady;
+	  uint8_t status =0;
 	  //char DisplayStr[5];
 
 	 /* read and display data */
@@ -817,6 +819,7 @@ int sts_tof_vl53lx_peoplecount_start(void)
       if (wordData > 30) return -1;  // 50 ms timer, so make this 60% to fail back
     }
     dataReady = 0;
+    status = 0; wordData =0;
     status += VL53L1X_GetRangeStatus(dev, &RangeStatus);
     status += VL53L1X_GetDistance(dev, &Distance);
     status += VL53L1X_GetSignalPerSpad(dev, &Signal);