diff --git a/LoRaWAN/App/lora_app.c b/LoRaWAN/App/lora_app.c
index 2cc1f37..9dd3174 100644
--- a/LoRaWAN/App/lora_app.c
+++ b/LoRaWAN/App/lora_app.c
@@ -952,7 +952,7 @@ void STS_SENSOR_Upload_Message(uint8_t appDataPort, uint8_t appBufferSize, uint8
 	LmHandlerErrorStatus_t status = LORAMAC_HANDLER_ERROR;
 	UTIL_TIMER_Time_t nextTxIn = 0;
 
-	if (LmHandlerIsBusy() == false)
+	//if (LmHandlerIsBusy() == false)
 	{
 
 		for (uint8_t i=0;i<appBufferSize; i++) {
diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin b/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin
index 3e3d89c..e9f8235 100644
Binary files a/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin and b/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin differ
diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf
index 1dc85df..501c1bc 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 658d092..fd43f8b 100644
--- a/STS/Core/Inc/yunhorn_sts_sensors.h
+++ b/STS/Core/Inc/yunhorn_sts_sensors.h
@@ -360,6 +360,7 @@ typedef struct sts_fhmos_sensor_config
 typedef struct sts_fhmos_sensor_ambient_height
 {
 	uint16_t	h2cm[64];	//height in 2cm scan data, ensure it less than 250*2=500cm, 5meter
+	uint8_t		cube[32];	// height cube (4*8) in 10cm of scanned data; 4*(high 4, low 4)*8
 	uint8_t 	maskoff[64];
 	uint16_t	head_level;
 	uint8_t		head_xy;
diff --git a/STS/Core/Src/yunhorn_sts_process.c b/STS/Core/Src/yunhorn_sts_process.c
index d669265..a8ad28a 100644
--- a/STS/Core/Src/yunhorn_sts_process.c
+++ b/STS/Core/Src/yunhorn_sts_process.c
@@ -172,7 +172,7 @@ volatile uint8_t sts_service_mask=STS_SERVICE_MASK_L0;
 volatile uint8_t sts_work_mode=STS_TOF_LMZ_RSS_MODE;
 volatile uint32_t rfac_timer=0;
 volatile uint16_t sts_sensor_install_height=3000;
-volatile uint8_t  sts_gesture_mask_off_height_cm=10;
+volatile uint8_t  sts_gesture_mask_off_height_cm=10, sts_gesture_mask_cap_height_cm=160;
 volatile uint8_t sensor_data_ready=0;
 
 #if	defined(STS_R1)||defined(STS_R5)||defined(STS_R4)||defined(STS_R1D)
@@ -544,9 +544,10 @@ void STS_YunhornSTSEventP5_Process(void)
 		UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0);
 
 	}
-	if (sts_fhmos_bitmap_pending == 1)
-	{
 
+	if (sts_fhmos_bitmap_pending == 0x01)
+	{
+		APP_LOG(TS_OFF, VLEVEL_M, "\r\n +++++++++++++++++++++++ \r\nBitmap Pending to upload flag=%02x \r\n", sts_fhmos_bitmap_pending);
 
 		uint8_t tstbuf[32] ={0x0}; uint8_t i=0;
 
@@ -575,6 +576,7 @@ void STS_YunhornSTSEventP5_Process(void)
 			STS_SENSOR_Upload_Message(YUNHORN_STS_L8_LORA_APP_DATA_PORT, i, (uint8_t *)tstbuf);
 
 			sts_fhmos_bitmap_pending = 2;
+			APP_LOG(TS_OFF, VLEVEL_M, "\r\n +++++++++++++++++++++++ \r\nBitmap Pending to uploaded\r\n");
 	}
 
 
@@ -1809,7 +1811,8 @@ void STS_REBOOT_CONFIG_Init(void)
 		}
 
 	}
-  	//STS_Show_STS_CFG_NVM((uint8_t*)nvm_store_value);
+
+  	STS_Show_STS_CFG_NVM((uint8_t*)nvm_store_value);
 	OnRestoreSTSCFGContextProcess();
 
   /* USER CODE END OnRestoreContextRequest_Last */
diff --git a/STS/TOF/App/app_tof.c b/STS/TOF/App/app_tof.c
index 643d496..04e99fd 100644
--- a/STS/TOF/App/app_tof.c
+++ b/STS/TOF/App/app_tof.c
@@ -39,7 +39,7 @@ extern "C" {
 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=1500, sts_high_threshold=2800, sts_occupancy_threshold=2300;
-extern volatile uint8_t  sts_gesture_mask_off_height_cm;
+extern volatile uint8_t  sts_gesture_mask_off_height_cm, sts_gesture_mask_cap_height_cm;
 extern volatile uint8_t sts_head_level_low;
 volatile sts_fhmos_sensor_config_t fhmos_cfg={70,4,8,8,20,80,50,0};
 volatile sts_fhmos_sensor_ambient_height_t fhmos_bg={0x0}, fhmos_gesture={0x0}, fhmos_net={0x0};
@@ -237,7 +237,7 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
 
 void sts_generate_fall_gesture_map(void)
 {
-	uint8_t i=0,head_xy=0;
+	uint8_t i=0,head_xy=0, h4=0x0, l4=0x0;
 	uint32_t range_distance =0, sts_fall_head_position=8000; // assume max range 8000 mm, actual 4500mm
 
 	for (i=0; i<64; i++)
@@ -262,21 +262,35 @@ void sts_generate_fall_gesture_map(void)
 					head_xy = i;					// head x, y coordination in 8x8 matrix
 				}
 				fhmos_gesture.h2cm[i] =  (uint16_t)abs(sts_sensor_install_height - range_distance);
-
 				fhmos_gesture.maskoff[i]= ((fhmos_gesture.h2cm[i])<sts_gesture_mask_off_height_cm*10)? 0:1;
 				fhmos_gesture_bitmap[(uint8_t)(i/8)] |= (fhmos_gesture.maskoff[i])<<(7-i%8);
-
 				// debug
-				if (i%8==0) printf("\r\n");
-				printf("|%4ld %4d ", range_distance, fhmos_gesture.h2cm[i]);
+//				if (i%8==0) printf("\r\n");
+//				printf("|%4ld %4d ", range_distance, fhmos_gesture.h2cm[i]);
 			}
 			else {
 				fhmos_gesture.h2cm[i] = 0;
 			}
+			if (i%8==0) printf("\r\n");
+			printf("|%4ld __%4d ", range_distance, fhmos_gesture.h2cm[i]);
+	}
+	printf("\r\n");
+	for (i = 0; i < 32; i++)
+	{
+
+			if (i%4 == 0) printf("\r\n");
+			h4 = MAX(fhmos_gesture.h2cm[2*i+0],sts_gesture_mask_cap_height_cm);
+			l4 = MAX(fhmos_gesture.h2cm[2*i+1],sts_gesture_mask_cap_height_cm);
+			fhmos_gesture.cube[i] =  ((h4&0x0f)<<4)|(l4&0x0f);
+			printf(" [%02x] : [%02x] ", (fhmos_gesture.cube[i]>>4)&0x0f, (fhmos_gesture.cube[i]&0x0f));
 	}
 
 	fhmos_gesture.head_level = 0xff&((sts_sensor_install_height - sts_fall_head_position)/10); // head level from floor in CM
 	fhmos_gesture.head_xy = head_xy&0xff;
+
+
+	printf("\r\n Head level =%4d cm, Head_xy=%d X:Y = %2d : %2d \r\n", fhmos_gesture.head_level, head_xy, (head_xy%8), (head_xy/8));
+
 #if 0
 		for (i=0; i<64; i++)
 		{
@@ -288,7 +302,7 @@ void sts_generate_fall_gesture_map(void)
 #endif
 
 		sts_fhmos_bitmap_pending = 1;
-		printf("\r\n Fall Gesture bitmap Generated\r\n");
+		APP_LOG(TS_OFF, VLEVEL_M, "\r\n Fall Gesture bitmap Generated\r\n");
 }
 uint16_t MX_TOF_Ranging_Process(void)
 {