wip L8

STM32CubeIDE/Release/makefile

@@ -14,13 +14,11 @@ 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/Target/subdir.mk
 -include Application/User/STS/TOF/App/subdir.mk
--include Application/User/STS/RSS/subdir.mk
 -include Application/User/STS/Core/Src/subdir.mk
 -include Application/User/LoRaWAN/Target/subdir.mk
 -include Application/User/LoRaWAN/App/subdir.mk

STM32CubeIDE/Release/sources.mk

@@ -27,13 +27,11 @@ Application/User/Core \
 Application/User/LoRaWAN/App \
 Application/User/LoRaWAN/Target \
 Application/User/STS/Core/Src \
-Application/User/STS/RSS \
 Application/User/STS/TOF/App \
 Application/User/STS/TOF/Target \
 Application/User/Startup \
 Drivers/BSP/53L8A1 \
 Drivers/BSP/Components \
-Drivers/BSP/STM32WLxx_Nucleo \
 Drivers/CMSIS \
 Drivers/STM32WLxx_HAL_Driver \
 Middlewares/LoRaWAN \

STS/Core/Inc/yunhorn_sts_sensors.h

@@ -531,7 +531,6 @@ void STS_SENSOR_Function_Test_Process(void);
 void 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 STS_TOF_L8_Process(void);
 void STS_TOF_L8_Reconfig(void);
 void MX_53L8A1_ThresholdDetection_ConfigIT(uint32_t sts_low_threshold, uint32_t sts_high_threshold);

STS/Core/Src/yunhorn_sts_process.c

@@ -179,7 +179,7 @@ volatile uint8_t sensor_data_ready=0;
 #if	defined(STS_R1)||defined(STS_R5)||defined(STS_R4)||defined(STS_R1D)
 volatile uint32_t STS_TOFScanPeriod_msec=50, STS_TxPeriod_sec=30, STS_HeartBeatTimerPeriod_sec=3600;
 #elif	defined(STS_L8)||defined(STS_P2)||defined(STS_O6T)||defined(STS_T6)||defined(O1L)
-volatile uint32_t STS_TOFScanPeriod_msec=1200, STS_TxPeriod_sec=300, STS_HeartBeatTimerPeriod_sec=3600;
+volatile uint32_t STS_TOFScanPeriod_msec=5000, STS_TxPeriod_sec=300, STS_HeartBeatTimerPeriod_sec=3600;
 #endif
 //volatile uint8_t sts_pir_state = 0, sts_pir_result=0;
 volatile uint8_t last_sts_pir_read=0;
@@ -2056,10 +2056,11 @@ void STS_SENSOR_Distance_Test_Process(void)
 #if	defined(VL53LX)||defined(L8)
 		//MX_TOF_Init();
 		STSWakeupScanTimerStop();
-		STS_TOF_L8_Process();
+		//STS_TOF_L8_Process();
 
 		//MX_TOF_Process();
 		sts_sensor_install_height = (uint16_t)MX_TOF_Ranging_Process();
+		//sts_sensor_install_height = (uint16_t)STS_Get_Center_Range_Distance();
 		APP_LOG(TS_OFF, VLEVEL_M, "\n STS SENSOR INSTALLATION HEIGHT =%d mm\n\r", (uint16_t)sts_sensor_install_height);
 
 		STS_LMZ_Ambient_Height_Scan_Process();

STS/TOF/App/app_tof.c

@@ -56,6 +56,7 @@ extern volatile uint8_t sts_fhmos_bitmap_pending;
 #endif
 #include "stm32wlxx_nucleo.h"
 
+static uint32_t STS_Get_Center_Range_Distance(RANGING_SENSOR_Result_t *Result);
 /* Private typedef -----------------------------------------------------------*/
 /*
  * The application is to showcase the threshold detection
@@ -197,13 +198,13 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
 	STS_TOF_L8_Process();
 
 
-	APP_LOG(TS_OFF, VLEVEL_H, "\r\n ----------------------"
+	APP_LOG(TS_OFF, VLEVEL_M, "\r\n ----------------------"
-				"\r\n------BGHM in 2cm-----"
 				"\r\n------Threshold %d cm--\r\n", fhmos_cfg.th_gesture_mask_off_height_cm);
 
 	for (uint8_t i = 0; i < 64; i++)
 	{
 				  /* Print distance and status */
+			if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
 			if ((Result.ZoneResult[i].NumberOfTargets > 0))
 			{
 				range_distance = (uint32_t)Result.ZoneResult[i].Distance[0];
@@ -219,22 +220,27 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
 				}
 				//sts_mask_bitmap[(uint8_t)(i/8)] |= (fhmos_bg.maskoff[i])<<(7-i%8);
 				sts_mask_bitmap[(uint8_t)(i/8)] |= (fhmos_bg.maskoff[i])<<(i%8);
-						if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
+				// if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
 				APP_LOG(TS_OFF, VLEVEL_M, "|%4d ", fhmos_bg.h2cm[i]);
 
 			}
 			else {
 					fhmos_bg.h2cm[i] = 0;
-						//printf(" .%d. ", i);
+					APP_LOG(TS_OFF, VLEVEL_M, " .%d. ", i);
 				}
 
 		}
 
+	APP_LOG(TS_OFF, VLEVEL_M, "\r\n ------- Mask off matrix \r\n");
+
 		for (i=0; i<64; i++)
 		{
 			if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
 			APP_LOG(TS_OFF, VLEVEL_M, "|%d ", (uint8_t)fhmos_bg.maskoff[i]);
 		}
+
+		APP_LOG(TS_OFF, VLEVEL_M, "\r\nmask bitmap \r\n");
+
 		for (i=0; i<8; i++)
 			APP_LOG(TS_OFF, VLEVEL_M, "%02X\r\n",sts_mask_bitmap[i]);
 
@@ -325,41 +331,22 @@ uint16_t MX_TOF_Ranging_Process(void)
 	return (uint16_t) range_distance;
 #elif defined(L8)
 	uint32_t range_distance=0;
-	uint8_t center_roi[4] = {27,28,35,36};
+	//uint8_t center_roi[4] = {27,28,35,36};
-	// uint8_t range_mode = 2; //STS_TOF_LONG_RANGE;
-	// int status = VL53L8A1_RANGING_SENSOR_GetDistance(VL53L8A1_DEV_CENTER, &Result);
 
-	// printf("\r|    27    |    28    |    35    |    36    |\r\n");
+		 if (ToF_EventDetected != 0)
-	for (uint8_t k=0; k<1; k++)
 		 {
-		STS_TOF_L8_Process();
+		      ToF_EventDetected = 0;
+				  status = VL53L8A1_RANGING_SENSOR_GetDistance(VL53L8A1_DEV_CENTER, &Result);
 
-		// int status = VL53L8A1_RANGING_SENSOR_GetDistance(VL53L8A1_DEV_CENTER, &Result);
+				  if (status == BSP_ERROR_NONE)
-		// printf("\r\n status =%d \r\n", status);
-
-
-		for (uint8_t zone_nbr = 0; zone_nbr < 4; zone_nbr++)
 				  {
-				#if 0
+					  range_distance= STS_Get_Center_Range_Distance(&Result);
-						  /* Print distance and status */
+				  } else {
-							if (Result.ZoneResult[center_roi[zone_nbr]].NumberOfTargets > 0)
+					  APP_LOG(TS_OFF, VLEVEL_M, "\r\n x \r\n");
-							{
-								APP_LOG(TS_OFF, VLEVEL_M, "|  %04ld  %2ld",
-									 (long)Result.ZoneResult[center_roi[zone_nbr]].Distance[RANGING_SENSOR_NB_TARGET_PER_ZONE-1],
-									 (long)Result.ZoneResult[center_roi[zone_nbr]].Status[RANGING_SENSOR_NB_TARGET_PER_ZONE-1]);
-							}
-							else {
-								APP_LOG(TS_OFF, VLEVEL_M, "|    --    ");
-							}
-				#endif
-			range_distance += Result.ZoneResult[center_roi[zone_nbr]].Distance[RANGING_SENSOR_NB_TARGET_PER_ZONE-1];
 				  }
 		}
 
-	APP_LOG(TS_OFF, VLEVEL_M, "\n\r");
 
-    range_distance /=4;
-    APP_LOG(TS_OFF, VLEVEL_M, "| %u mm\r\n", (uint16_t)range_distance);
 
 	return (uint16_t) range_distance;
 #endif
@@ -388,11 +375,13 @@ void MX_TOF_Process(void)
   /* USER CODE END TOF_Process_PostTreatment */
 }
 #ifdef L8
+
 void STS_TOF_L8_Init(void)
 {
 	MX_53L8A1_ThresholdDetection_Init();
 	MX_53L8A1_ThresholdDetection_Process();
 }
+
 void STS_TOF_L8_Process(void)
 {
 	  //while (1)
@@ -535,6 +524,21 @@ static void MX_53L8A1_ThresholdDetection_Process(void)
 #endif
 }
 
+uint32_t STS_Get_Center_Range_Distance(RANGING_SENSOR_Result_t *Result)
+{
+	  uint32_t center_range_distance=0;
+	  uint8_t center_roi[4] = {27,28,35,36};
+
+	  for (uint8_t i=0; i<4; i++)
+	  {
+		  center_range_distance +=(uint32_t)(Result->ZoneResult[center_roi[i]].Distance[0]);
+	  }
+
+	  center_range_distance /= 4;
+
+	return center_range_distance;
+}
+
 static void print_result(RANGING_SENSOR_Result_t *Result)
 {
   int8_t i;
@@ -543,7 +547,7 @@ static void print_result(RANGING_SENSOR_Result_t *Result)
   uint8_t center_roi[4] = {27,28,35,36};
   //static uint32_t prev_distance[64]={0};
   //uint32_t motion_diff=0, motion_power=0;
-  uint16_t	 head_distance=8000;
+  uint16_t	 head_distance_from_ceiling=8000;
   //uint32_t motion_power_threshold = 3200; // 64*50mm
 
   //SysTime_t sensor_event_time = SysTimeGetMcuTime();
@@ -574,36 +578,47 @@ static void print_result(RANGING_SENSOR_Result_t *Result)
                     (Profile.RangingProfile == RS_PROFILE_8x8_CONTINUOUS)) ? 8 : 4;
 #endif
   	  //printf("%c[2H", 27); /* clear screen */
+#if 0
   for (i=0; i<4; i++)
   {
-
 	  center_range_distance +=(uint32_t)(Result->ZoneResult[center_roi[i]].Distance[0]);
-
   }
-
+#endif
   //int32_t roi_distance =(uint32_t)center_range_distance/4;
-  uint16_t factor1_floor_level = sts_sensor_install_height - 50; // 50mm min body height
+  uint16_t factor1_floor_level_from_ceiling = sts_sensor_install_height; // 50mm min body height
-  uint16_t factor2_head_level = (sts_sensor_install_height - 10*fhmos_cfg.th_head_level_height_cm);
+  uint16_t factor2_head_level_from_floor = (sts_sensor_install_height - 10*fhmos_cfg.th_head_level_height_cm);
+  uint16_t head_distance_from_floor=0;
+
   sts_head_level_low = 0;
 
   for (i=0; i<64; i++)
   {
-	  if (0 == fhmos_bg.maskoff[i])
+	  if (i%8 ==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
-	  {
+	  APP_LOG(TS_OFF, VLEVEL_M, "%2d__%4d ", fhmos_bg.maskoff[i],Result->ZoneResult[i].Distance[0]);
-		  if (Result->ZoneResult[i].Distance[0] < head_distance)
+	  if (0 == fhmos_bg.maskoff[i])		// only within the non-mask-off blocks
-			  head_distance = Result->ZoneResult[i].Distance[0];		// find out the min_distance
+	  {									// Result.ZoneResult[i].NumberOfTargets > 0)
+		  if ((Result->ZoneResult[i].Distance[0] < head_distance_from_ceiling)&& (Result->ZoneResult[i].NumberOfTargets > 0))
+			  head_distance_from_ceiling = Result->ZoneResult[i].Distance[0];
+		  // find out the min_distance or the highest position level
 	  }
   }
+
+  head_distance_from_floor = sts_sensor_install_height - head_distance_from_ceiling;
+
+  APP_LOG(TS_OFF, VLEVEL_M, "\r\n #### Head distance from ceiling = %4d \r\n Head distance from floor\r\n",head_distance_from_ceiling, head_distance_from_floor);
   /* state tree */
-#if 0
+#if 2
-  printf("\r\n Factor1_install_height=%d mm, F2 th_high=%d, th_head=%d, Factor2 (gap)=%d \r\n",
+  APP_LOG(TS_OFF, VLEVEL_M,"\r\n factor1_floor_level_from_ceiling=%d mm\r\n, F2 th_high=%d, th_head=%d, \r\nFactor2 (factor2_head_level_from_floor)=%d \r\n",
-		  factor1_floor_level, (int)sts_high_threshold, (int)10*fhmos_cfg.th_head_level_height_cm, factor2_head_level);
+		  factor1_floor_level_from_ceiling, (int)sts_high_threshold, (int)10*fhmos_cfg.th_head_level_height_cm, factor2_head_level_from_floor);
 #endif
-  if ((head_distance <= factor1_floor_level) && (head_distance >= factor2_head_level))
+
+  if ((head_distance_from_ceiling <= factor1_floor_level_from_ceiling) && ((sts_sensor_install_height - head_distance_from_ceiling) <=  (10*fhmos_cfg.th_head_level_height_cm) ))
 	 {
 
-		//	  printf("\r\n YELLOW OR RED Distance=%d \r\n", head_distance);
+	  	  APP_LOG(TS_OFF, VLEVEL_M,"\r\n YELLOW OR RED (head from ceiling) =%d (mm)\r\n Head from floor (mm) \r\n", head_distance_from_ceiling, (sts_sensor_install_height - head_distance_from_ceiling));
+
 		  sts_head_level_low = 1;
+
 #if 0
   		fhmos_data.state_fall = STS_FHMOS_FALL_STATE_POTENTIAL;
   		fhmos_data.color_fall = STS_FALL_SUSPICIOUS_COLOR;
@@ -612,7 +627,7 @@ static void print_result(RANGING_SENSOR_Result_t *Result)
 
   		//sts_fhmos_state_changed |=1;
 
-	} else if ((head_distance < (factor2_head_level - 150))) // TODO XXX  50mm gap to avoid flapping back and forth
+	} 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;
 			  fhmos_data.state_fall = STS_FHMOS_FALL_STATE_NORMAL;