From 1a4998a6596ae8c7921bf1e825573693a37d5c88 Mon Sep 17 00:00:00 2001
From: sundp <dp.s@yunhorn.com>
Date: Mon, 7 Oct 2024 20:59:37 +0800
Subject: [PATCH] wip

---
 STS/Core/Src/yunhorn_sts_process.c | 25 +++++++++++++++++++++++--
 STS/TOF/App/app_tof_peoplecount.c  |  2 +-
 2 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/STS/Core/Src/yunhorn_sts_process.c b/STS/Core/Src/yunhorn_sts_process.c
index 81607ef..871b090 100644
--- a/STS/Core/Src/yunhorn_sts_process.c
+++ b/STS/Core/Src/yunhorn_sts_process.c
@@ -44,9 +44,10 @@
 #ifdef	VL53L0
 #include "X-WL55_WLE5_53L0X.h"
 #endif
-#ifdef STS_P2
+#if	defined(STS_P2)||defined(STS_T6)
 #include "app_tof.h"
 #include "app_tof_peoplecount.h"
+extern volatile uint8_t sts_door_jam_profile;
 #endif
 volatile sts_cfg_nvm_t sts_cfg_nvm = {
 		sts_mtmcode1,
@@ -119,6 +120,9 @@ static void STS_Show_STS_CFG_NVM(uint8_t * nvm_store_value);
 extern volatile int sts_tof_distance_data[MAX_TOF_COUNT];
 volatile sts_tof_range_data_t sts_range_sensor_data={0x0,0x0,0x0};
 #endif
+#if	defined(STS_P2)||defined(STS_T6)
+volatile uint8_t sts_door_jam_profile = 0;
+#endif
 #ifdef STS_R4
 volatile uint8_t sts_soap_level_state=0;
 void STS_R4_sensor_read(sts_r_sensor_data_t *sts_r_sensor_data);
@@ -1702,7 +1706,7 @@ void STS_SENSOR_Function_Test_Process(void)
 	tstbuf[i++] = (uint8_t) sts_hardware_ver;
 	tstbuf[i++] = (uint8_t) (99*GetBatteryLevel()/254)&0xff;
 
-#ifdef STS_P2
+#if	(defined(STS_P2)||defined(STS_T6))
 	STS_SENSOR_MEMS_Get_ID(&sensor_id);
 
 	if (((sensor_id & 0xff)!= 0xCC) && (((sensor_id >>8) & 0xFF)!=0xEA))		// no VL53L1X found
@@ -1716,6 +1720,23 @@ void STS_SENSOR_Function_Test_Process(void)
 		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
 	}
+	/*                                                                                    */
+	/* for STS_P2, set ppc_config -> door_jam_profile and distance threshold accordingly  */
+	/*                                                                                    */
+
+	if ((sts_sensor_install_height >= 1500)&&(sts_sensor_install_height<=4000))
+		{
+		 sts_door_jam_profile = DOOR_JAM_2000;	// DOOR_JAM_2000: assume door_jame less 2000mm
+
+		 if (sts_sensor_install_height > 2000)
+			 sts_door_jam_profile ++;							//DOOR_JAM_2400
+		 if (sts_sensor_install_height > 2400)
+			 sts_door_jam_profile ++;							//DOOR_JAM_3000
+		 if (sts_sensor_install_height > 3000)
+			 sts_door_jam_profile ++;					//DOOR_JAM_3500
+		 if (sts_sensor_install_height > 3500)
+			 sts_door_jam_profile ++;					//DOOR_JAM_4000
+		}
 #endif
 #ifdef YUNHORN_STS_O6_ENABLED
 		tstbuf[i++] = (uint8_t)20; //length of following data
diff --git a/STS/TOF/App/app_tof_peoplecount.c b/STS/TOF/App/app_tof_peoplecount.c
index f6ba0f8..9f3133d 100644
--- a/STS/TOF/App/app_tof_peoplecount.c
+++ b/STS/TOF/App/app_tof_peoplecount.c
@@ -32,7 +32,7 @@ volatile sts_zone_center_by_rows_of_spads_t zone_center[3]={
 		{6, 159, 239},
 		{8, 175, 231},
 };
-volatile uint8_t sts_distance_mode = DOOR_JAM_3000;
+volatile uint8_t sts_distance_mode = sts_door_jam_profile; //DOOR_JAM_3000;
 volatile uint8_t sts_people_count_number_changed = 0;
 static uint32_t cnt_today=0, cnt_week=0;
 static uint8_t last_year=0, this_year=0,last_month=0, this_month=0,last_week=0, this_week=0,last_day=0, this_day=0;