diff --git a/Core/Src/sts_lamp_bar.c b/Core/Src/sts_lamp_bar.c
index 01c7d0c..4419a90 100644
--- a/Core/Src/sts_lamp_bar.c
+++ b/Core/Src/sts_lamp_bar.c
@@ -220,7 +220,7 @@ void STS_Lamp_Bar_Self_Test_Simple(void)
 		do {
 			STS_Lamp_Bar_Set_STS_RGB_Color(color, lum_level);
 			HAL_Delay(50);
-			lum_level += 20;
+			lum_level += 10;
 		} while (lum_level < 99);
 		STS_Lamp_Bar_Set_Dark();
 	}
@@ -233,7 +233,7 @@ void STS_Lamp_Bar_Self_Test(void)
 
 	APP_LOG(TS_OFF, VLEVEL_M, "\r\n YunHorn STS Indicative Lamp Self Test\r\n");
 
-	//STS_Lamp_Bar_Self_Test_Simple();
+	STS_Lamp_Bar_Self_Test_Simple();
 
 	APP_LOG(TS_OFF, VLEVEL_M, "\r\n [#2] Scoller Testing\r\n");
 	lum_level=50;
diff --git a/Core/Src/sys_app.c b/Core/Src/sys_app.c
index 1c4673c..07ff13b 100644
--- a/Core/Src/sys_app.c
+++ b/Core/Src/sys_app.c
@@ -135,7 +135,7 @@ void SystemApp_Init(void)
 #if defined(STS_T6)
   //STS_TOF_VL53LX_PresenceDetection_Process_Init();
   //STS_Lamp_Bar_Self_Test_Simple();
-  //STS_Lamp_Bar_Self_Test();
+  STS_Lamp_Bar_Self_Test();
   LED_GREEN_ON;HAL_Delay(50);
   LED_RED_ON;HAL_Delay(50);
   LED_GREEN_OFF;HAL_Delay(50);
diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf
index b90af9c..5a6da59 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 c602e02..37818a9 100644
--- a/STS/Core/Inc/yunhorn_sts_sensors.h
+++ b/STS/Core/Inc/yunhorn_sts_sensors.h
@@ -288,7 +288,7 @@ void OnYunhornSTSHeartBeatPeriodicityChanged(uint32_t periodicity);
 void OnYunhornSTSTxPeriodicityChanged(uint32_t periodicity);
 void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_size);
 void STS_SENSOR_Function_Test_Process(void);
-void STS_SENSOR_Distance_Test_Process(void);
+int  STS_SENSOR_Distance_Test_Process(void);
 void STS_PRESENCE_SENSOR_Function_Test_Process(uint8_t *self_test_result, uint8_t count);
 void STS_PRESENCE_SENSOR_Distance_Measure_Process(void);
 void ppc_cfg_parameter_update(void);
diff --git a/STS/Core/Src/yunhorn_sts_process.c b/STS/Core/Src/yunhorn_sts_process.c
index 9083ae8..fb3ec65 100644
--- a/STS/Core/Src/yunhorn_sts_process.c
+++ b/STS/Core/Src/yunhorn_sts_process.c
@@ -678,6 +678,8 @@ void USER_APP_Parse_CMD_P(uint8_t *parse_buffer, uint8_t parse_buffer_size)
 			sts_color_occupy_vacant = sts_cfg_nvm.color_occupy_vacant;
 
 			APP_LOG(TS_OFF, VLEVEL_M, "\r\n Workmode=%d  Color Occupy=%02X  Vacant=%02X \r\n", sts_work_mode, STS_COLOR_DEFAULT_OCCUPY, STS_COLOR_DEFAULT_VACANT);
+
+			OnStoreSTSCFGContextRequest();
 		}
 
 			break;
@@ -1112,10 +1114,7 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s
 				STS_SENSOR_Upload_Message(YUNHORN_STS_USER_APP_CTRL_REPLY_PORT, i, (uint8_t *)outbuf);
 				break;
 			case 'D':				/* "YZD": Distance/Install height Measure 	*/
-				STS_SENSOR_Distance_Test_Process();
-				sts_cfg_nvm.sensor_install_height_in_10cm 	= sts_sensor_install_height/100;  //in 10 cm, say 4500mm=450cm=45 dm
-				APP_LOG(TS_OFF, VLEVEL_M, "\n STS CFG NVM -> SENSOR INSTALL HEIGHT STORED = %d dm(10cm)\n", sts_cfg_nvm.sensor_install_height_in_10cm);
-				OnStoreSTSCFGContextRequest();
+				uint16_t range_distance = STS_SENSOR_Distance_Test_Process();
 				i = 0;
 				outbuf[i++] 	= (uint8_t)'D';
 				outbuf[i++] 	= (uint8_t)sts_mtmcode1;
@@ -1123,23 +1122,35 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s
 				outbuf[i++] 	= (uint8_t)sts_version;
 				outbuf[i++] 	= (uint8_t)sts_hardware_ver;
 				outbuf[i++] 	= (uint8_t)(99*((GetBatteryLevel()/254)&0xff));
-				outbuf[i++] 	= (uint8_t)(2);	//two bytes for distance
-				#if	defined(VL53LX)||defined(VL53L0)
-#if 0
-				outbuf[i++] 	= (uint8_t)(sts_sensor_install_height/1000+0x30)&0xff;
-				outbuf[i++] 	= (uint8_t)((sts_sensor_install_height/100)%10+0x30)&0xff;
-				outbuf[i++] 	= (uint8_t)((sts_sensor_install_height/10)%10+0x30)&0xff;
-				outbuf[i++] 	= (uint8_t)((sts_sensor_install_height/1)%10+0x30)&0xff;
-#endif
-				outbuf[i++] 	= (uint8_t)(sts_sensor_install_height>>8)&0xFF;
-				outbuf[i++] 	= (uint8_t)(sts_sensor_install_height)&0xFF;
-				#endif
+
+				if (range_distance == 0 )
+				{
+					sts_cfg_nvm.sensor_install_height_in_10cm 	= sts_sensor_install_height/100;  //in 10 cm, say 4500mm=450cm=45 dm
+					APP_LOG(TS_OFF, VLEVEL_M, "\n STS CFG NVM -> SENSOR INSTALL HEIGHT STORED = %d dm(10cm)\n", sts_cfg_nvm.sensor_install_height_in_10cm);
+					OnStoreSTSCFGContextRequest();
+					outbuf[i++] 	= (uint8_t)(2);	//two bytes for distance
+					#if	defined(VL53LX)||defined(VL53L0)
+					#if 0
+					outbuf[i++] 	= (uint8_t)(sts_sensor_install_height/1000+0x30)&0xff;
+					outbuf[i++] 	= (uint8_t)((sts_sensor_install_height/100)%10+0x30)&0xff;
+					outbuf[i++] 	= (uint8_t)((sts_sensor_install_height/10)%10+0x30)&0xff;
+					outbuf[i++] 	= (uint8_t)((sts_sensor_install_height/1)%10+0x30)&0xff;
+					#endif
+					outbuf[i++] 	= (uint8_t)(sts_sensor_install_height>>8)&0xFF;
+					outbuf[i++] 	= (uint8_t)(sts_sensor_install_height)&0xFF;
+					#endif
+
+					#if	defined(STS_T6)
+									ppc_cfg_parameter_update();
+					#endif
+
+				} else
+				{
+					outbuf[i++] 	= (uint8_t)'X';
+				}
 
 				STS_SENSOR_Upload_Message(YUNHORN_STS_USER_APP_CTRL_REPLY_PORT, i, (uint8_t *)outbuf);
 
-#if	defined(STS_T6)
-				ppc_cfg_parameter_update();
-#endif
 				break;
 			case 'M':				/* "YZM": Mask level			 			*/
 				i = 0;
@@ -1818,7 +1829,7 @@ static void STS_Show_STS_CFG_NVM(uint8_t * store_value)
 	APP_LOG(TS_OFF, VLEVEL_H, "\n\r");
 	APP_LOG(TS_OFF, VLEVEL_H, "\n###############################################\n\r");
 }
-void STS_SENSOR_Distance_Test_Process(void)
+int STS_SENSOR_Distance_Test_Process(void)
 {
 #ifdef	YUNHORN_STS_O6_ENABLED
 	sts_distance_rss_distance =0;
@@ -1849,6 +1860,7 @@ void STS_SENSOR_Distance_Test_Process(void)
 		sts_sensor_install_height=sts_tof_distance_data[0]+sts_tof_distance_data[1];
 #endif
 
+		return status;
 }
 
 void ppc_cfg_parameter_update(void)
@@ -1893,14 +1905,14 @@ void STS_SENSOR_Function_Test_Process(void)
 
 	if (((sensor_id & 0xff)!= 0xCC) && (((sensor_id >>8) & 0xFF)!=0xEA))		// no VL53L1X found
 	{
-		tstbuf[i++] = (uint8_t) 'X'; // Slave MEMS Not Avaliable
+		tstbuf[i++] = (uint8_t) 'X'; // Slave MEMS Not Available
 	} else
 	{
 		//STS_SENSOR_Distance_Test_Process();
 		int status=0;
 		uint16_t range_distance =0;
 		status = sts_tof_vl53lx_range_distance(&range_distance);
-		if (status ==0)
+		if ((status ==0) && (range_distance !=0))
 		{
 			sts_sensor_install_height = range_distance;
 			APP_LOG(TS_OFF, VLEVEL_M, "\nSensor Install Height =%4d mm\n", sts_sensor_install_height);
@@ -1909,6 +1921,8 @@ void STS_SENSOR_Function_Test_Process(void)
 			tstbuf[i++] = (uint8_t) (sensor_id)&0xff;
 			tstbuf[i++] = (uint8_t) (sts_sensor_install_height>>8)&0xff;	// MSB of sensor height
 			tstbuf[i++] = (uint8_t) (sts_sensor_install_height)&0xff;		// LSB of sensor height
+		} else {
+			tstbuf[i++] = (uint8_t) 'x'; // Slave MEMS available yet not stable
 		}
 	}
 #endif
diff --git a/STS/TOF/App/app_tof_peoplecount.c b/STS/TOF/App/app_tof_peoplecount.c
index b879224..d73847e 100644
--- a/STS/TOF/App/app_tof_peoplecount.c
+++ b/STS/TOF/App/app_tof_peoplecount.c
@@ -1029,8 +1029,10 @@ int sts_tof_vl53lx_range_distance(uint16_t *range_distance)
   if (status != 0) {
   	APP_LOG(TS_OFF, VLEVEL_L,"Error in operating the device\n");
     return (-1);
+  } else if (Distance != 0 )
+  {
+	  APP_LOG(TS_OFF, VLEVEL_M,"\r\nVL53L1X RANGE DISTANCE =%d \r\n", Distance);
+	  *range_distance = Distance;
   }
-	APP_LOG(TS_OFF, VLEVEL_M,"\r\nVL53L1X RANGE DISTANCE =%d \r\n", Distance);
-  *range_distance = Distance;
 	return status;
 }