diff --git a/Core/Src/main.c b/Core/Src/main.c
index b01f0ea..90c0892 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -115,7 +115,6 @@ int main(void)
       MX_LoRaWAN_Init();
   }
 
-  //MX_TOF_Process();
 
   /* USER CODE BEGIN 2 */
 
diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf
index fdd85dc..e32c6c9 100644
Binary files a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf and b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf differ
diff --git a/STS/Core/Inc/yunhorn_sts_sensors.h b/STS/Core/Inc/yunhorn_sts_sensors.h
index 3d3f5e1..4c411a4 100644
--- a/STS/Core/Inc/yunhorn_sts_sensors.h
+++ b/STS/Core/Inc/yunhorn_sts_sensors.h
@@ -320,6 +320,7 @@ void STS_PRESENCE_SENSOR_Function_Test_Process(uint8_t *self_test_result, uint8_
 void STS_PRESENCE_SENSOR_Distance_Measure_Process(void);
 
 void STS_TOF_L8_Process(void);
+void STS_TOF_L8_Reconfig(void);
 void STS_LMZ_Ambient_Height_Scan_Process(void);
 void STS_FHMOS_sensor_read(sts_fhmos_sensor_data_t *sts_fhmos_data);
 void STS_FHMOS_sensor_config_init(void);
diff --git a/STS/TOF/App/app_tof.c b/STS/TOF/App/app_tof.c
index 67556c6..80496c8 100644
--- a/STS/TOF/App/app_tof.c
+++ b/STS/TOF/App/app_tof.c
@@ -28,7 +28,7 @@ extern "C" {
 #include "sys_app.h"
 #include <stdio.h>
 #include "usart.h"
-
+#include <stdlib.h>
 #if	(defined(STS_P2)||defined(STS_T6))
 #include "VL53L1X_api.h"
 #elif defined(L8)
@@ -37,9 +37,11 @@ extern "C" {
 #include "yunhorn_sts_sensors.h"
 volatile uint8_t fhmos_fall=0, fhmos_human_movement=0, fhmos_occupancy=0, fhmos_sos_alarm=0;
 volatile uint32_t fhmos_fall_counter=0;
+volatile uint32_t sts_low_threshold, sts_high_threshold;
 volatile sts_fhmos_sensor_data_t sts_fhmos_data;
 volatile sts_fhmos_sensor_config_t sts_fhmos_cfg;
 volatile sts_fhmos_sensor_ambient_height_t sts_fhmos_bg={0x0};
+volatile uint8_t mask_bitmap[8]={0x0};
 extern volatile uint16_t sts_sensor_install_height; //in mm
 #endif
 #include "stm32wlxx_nucleo.h"
@@ -162,7 +164,7 @@ void MX_TOF_Init(void)
 void STS_LMZ_Ambient_Height_Scan_Process(void)
 {
 
-	uint8_t i=0;
+	uint8_t i=0, repeat=1;
 	uint32_t range_distance =0;
 
 	for (i=0; i<64; i++)
@@ -172,7 +174,12 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
 	}
 //	printf("sts sensor install height = %4d \r\n", (int)sts_sensor_install_height);
 
-	for (uint8_t k=0; k<10; k++)
+
+	sts_high_threshold=sts_sensor_install_height;
+	sts_low_threshold=sts_sensor_install_height-1400;
+	STS_TOF_L8_Reconfig();
+	//printf("%c[2H", 27); /* clear screen */
+	for (uint8_t k=0; k<repeat; k++)
 	{
 		STS_TOF_L8_Process();
 		for (uint8_t i = 0; i < 64; i++)
@@ -182,10 +189,18 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
 				{
 					range_distance = (uint32_t)Result.ZoneResult[i].Distance[0];
 
-					if (sts_sensor_install_height > range_distance)
+					//if (sts_sensor_install_height > range_distance)
 						sts_fhmos_bg.h2cm[i] +=  ((uint32_t)sts_sensor_install_height - range_distance);
+						if (abs(sts_fhmos_bg.h2cm[i])<100){
+							sts_fhmos_bg.maskoff[i] = 0;
+						} else {
+							sts_fhmos_bg.maskoff[i] = 1;
+							mask_bitmap[i/8] |= 1<<(7-i%8);
+						}
+
+						if (i%8==0) printf("\r\n");
 					printf("|%4d %4d ", range_distance, sts_fhmos_bg.h2cm[i]);
-									if (i%8==0) printf("\r\n");
+
 				}
 				else {
 					sts_fhmos_bg.h2cm[i] += 0;
@@ -197,16 +212,11 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
 	}
 	for (i=0; i<64; i++)
 	{
-		sts_fhmos_bg.h2cm[i] /= 10;
-		if (fabs(sts_sensor_install_height - sts_fhmos_bg.h2cm[i]) <500) {
-			sts_fhmos_bg.maskoff[i] = 0;
-		} else {
-			sts_fhmos_bg.maskoff[i] = 1;
-		}
-
-		printf("|%d ", (uint32_t)sts_fhmos_bg.maskoff[i]);
 		if (i%8==0) printf("\r\n");
+		printf("|%d ", (uint8_t)sts_fhmos_bg.maskoff[i]);
 	}
+	for (i=0; i<8; i++)
+		printf("%02X\r\n",mask_bitmap[i]);
 
 }
 
@@ -308,6 +318,12 @@ void STS_TOF_L8_Process(void)
 	  }
 }
 
+void STS_TOF_L8_Reconfig(void)
+{
+	MX_53L8A1_ThresholdDetection_ConfigIT(sts_low_threshold, sts_high_threshold);
+}
+
+
 /*   VL53L8A1    */
 static void MX_53L8A1_ThresholdDetection_Init(void)
 {
@@ -336,6 +352,28 @@ static void MX_53L8A1_ThresholdDetection_Init(void)
   }
 
 }
+void MX_53L8A1_ThresholdDetection_ConfigIT(uint32_t low_threshold, uint32_t high_threshold)
+{
+
+	  RANGING_SENSOR_ITConfig_t ITConfig;
+
+	  status = VL53L8A1_RANGING_SENSOR_Stop(VL53L8A1_DEV_CENTER);
+
+	  /* threshold parameters */
+	  ITConfig.Criteria = RS_IT_IN_WINDOW;
+	  ITConfig.LowThreshold = low_threshold; /* mm */
+	  ITConfig.HighThreshold = high_threshold; /* mm */
+
+	  VL53L8A1_RANGING_SENSOR_ConfigIT(VL53L8A1_DEV_CENTER, &ITConfig);
+
+	  status = VL53L8A1_RANGING_SENSOR_Start(VL53L8A1_DEV_CENTER, RS_MODE_ASYNC_CONTINUOUS);
+
+	    if (status != BSP_ERROR_NONE)
+	    {
+	      printf("VL53L8A1_RANGING_SENSOR_Start failed\n");
+	      while (1);
+	    }
+}
 
 static void MX_53L8A1_ThresholdDetection_Process(void)
 {