revised with edge removing for PWH jumbo roll issue

STS/Core/Src/yunhorn_sts_process.c

@@ -3031,7 +3031,7 @@ void OnSensorL8AStateChanged(void)
 			fhmos_data.head_low_level_start_time 					= sensor_event_time.Seconds;
 			fhmos_data.state_fall_released = 0;
 
-			APP_LOG(TS_OFF, VLEVEL_M, "\r\n Head Level Low --------  state =%d  last =%d \r\n", sts_head_level_low, last_head_level_low_state);
+			APP_LOG(TS_OFF, VLEVEL_L, "\r\n Head Level Low --------  state =%d  last =%d \r\n", sts_head_level_low, last_head_level_low_state);
 
 			sts_o7_sensorData.event_sensor3_fall_start_time			= sensor_event_time.Seconds;
 
@@ -3041,7 +3041,7 @@ void OnSensorL8AStateChanged(void)
 		{
 			fhmos_data.head_low_level_stop_time = sensor_event_time.Seconds;
 			fhmos_data.state_fall = STS_FHMOS_FALL_STATE_NORMAL;
-			APP_LOG(TS_OFF, VLEVEL_M, "\r\n Head Level Rise up or Out of Focus Area  ++++++++++ \r\n");
+			APP_LOG(TS_OFF, VLEVEL_L, "\r\n Head Level Rise up or Out of Focus Area  ++++++++++ \r\n");
 			sts_fhmos_state_changed = 1;
 			fhmos_data.state_fall_released = 1;
 		}

STS/TOF/App/app_tof.c

@@ -55,7 +55,7 @@ extern volatile uint8_t sts_fhmos_state_changed;
 extern volatile uint8_t sts_fhmos_bitmap_pending;
 #endif
 #include "stm32wlxx_nucleo.h"
-
+static int to_confirm = 0;
 static uint32_t STS_Get_Center_Range_Distance(RANGING_SENSOR_Result_t *Result);
 /* Private typedef -----------------------------------------------------------*/
 /*
@@ -258,12 +258,28 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
 			(uint8_t)fhmos_cfg.th_head_level_height_cm, (uint8_t)fhmos_cfg.th_gesture_mask_off_height_cm, (uint8_t)(fhmos_cfg.th_fall_body_min_height_cm));
 
 	APP_LOG(TS_OFF, VLEVEL_L, "\r\n\n ------- Mask off matrix \r\n");
-
+/*
 		for (i=0; i<64; i++)
 		{
 			if (i%8==0) APP_LOG(TS_OFF, VLEVEL_L, "\r\n");
 			APP_LOG(TS_OFF, VLEVEL_L, "|%d ", (uint8_t)fhmos_bg.maskoff[i]);
 		}
+*/
+	APP_LOG(TS_OFF, VLEVEL_L, "\r\n\n ------- Remove Edge \r\n");
+
+	uint8_t rio_edge[34]={0,1,2,3,4,5,6,7,8,15,16,23,24,31,32,39,40,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63};
+
+	  for (i=0; i<sizeof(rio_edge); i++)
+		  fhmos_bg.maskoff[rio_edge[i]] = 1;
+
+
+	for (i=0; i<64; i++)
+		{
+			if (i%8==0) APP_LOG(TS_OFF, VLEVEL_L, "\r\n");
+			APP_LOG(TS_OFF, VLEVEL_L, "|%d ", (uint8_t)fhmos_bg.maskoff[i]);
+		}
+
+	  APP_LOG(TS_OFF, VLEVEL_L, "\r\n");
 
 		APP_LOG(TS_OFF, VLEVEL_M, "\r\nmask bitmap \r\n");
 
@@ -606,12 +622,13 @@ static void print_result(RANGING_SENSOR_Result_t *Result)
 
   sts_head_level_low = 0;
 
+  /*
   //APP_LOG(TS_OFF, VLEVEL_M, "\r\n Eliminate edge \r\n");
-  uint8_t rio_edge[30]={0,1,2,3,4,5,6,7,8,15,16,23,24,31,32,39,40,47,48,55,56,57,58,59,60,61,62,63};
+  uint8_t rio_edge[36]={0,1,2,3,4,5,6,7,8,15,16,23,24,31,32,39,40,47,48,49,50,51,52,53,54,55,56,57,58,59,60,61,62,63};
 
-  for (i=0; i<30; i++)
+  for (i=0; i<36; i++)
 	  fhmos_bg.maskoff[rio_edge[i]] = 1;
-
+   */
   for (i=0; i<64; i++)
     {
   	  if (0 == fhmos_bg.maskoff[i])		// only within the non-mask-off blocks
@@ -624,20 +641,29 @@ static void print_result(RANGING_SENSOR_Result_t *Result)
 
 
   head_height_level_from_floor = MIN(1400, abs(sts_sensor_install_height - head_distance_from_ceiling));
+
+  APP_LOG(TS_OFF, VLEVEL_L, "\r\n Raw Head height=%d cm Calculated head level=%d cm\r\n", head_distance_from_ceiling, head_height_level_from_floor );
+
   /* state tree */
 
   if ((head_distance_from_ceiling <= factor1_floor_level_from_ceiling) && ((head_height_level_from_floor) <=  (10*fhmos_cfg.th_head_level_height_cm)) && (head_height_level_from_floor >= (10*fhmos_cfg.th_fall_body_min_height_cm )))
 	 {
-
+	  	  to_confirm ++;
-		  sts_head_level_low = 1;
+	  	  if (to_confirm >=3 )
+	  	  {
+	  		  sts_head_level_low = 1;
+	  		  // to_confirm = 0;
+	  	  }
 
 	} else if ((head_distance_from_ceiling < (factor2_head_level_from_floor - 150))) // TODO XXX  50mm gap to avoid flapping back and forth
 	{
 			  sts_head_level_low = 0;
 			  sts_fhmos_bitmap_pending = 0;
-
+			  to_confirm = 0;
 	}
 
+
+
 	OnSensorL8AStateChanged();
 
 }