sundp
/
WLE5CC_NODE_STS
1
0
Fork 1
Code Issues Pull Requests 2 Packages Projects Releases Wiki Activity

wip L8

This commit is contained in:
Yunhorn 2024-12-13 19:47:54 +08:00
parent 2721055514
commit d289b7b1e9
7 changed files with 87 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
STM32CubeIDE/Release/WLE5CC_NODE_STS.bin
View File

Binary file not shown.

BIN
STM32CubeIDE/Release/WLE5CC_NODE_STS.elf
View File

Binary file not shown.

2
STM32CubeIDE/Release/makefile
View File

@ -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

2
STM32CubeIDE/Release/sources.mk
View File

@ -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 \

1
STS/Core/Inc/yunhorn_sts_sensors.h
View File

@ -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);

5
STS/Core/Src/yunhorn_sts_process.c
View File

@ -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();

153
STS/TOF/App/app_tof.c
View File

@ -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,44 +198,49 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
STS_TOF_L8_Process();
APP_LOG(TS_OFF, VLEVEL_H, "\r\n ----------------------"
"\r\n------BGHM in 2cm-----"
APP_LOG(TS_OFF, VLEVEL_M, "\r\n ----------------------"
"\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 ((Result.ZoneResult[i].NumberOfTargets > 0))
{
range_distance = (uint32_t)Result.ZoneResult[i].Distance[0];
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];
fhmos_bg.h2cm[i] = abs(sts_sensor_install_height - range_distance)/20;
fhmos_bg.h2cm[i] = abs(sts_sensor_install_height - range_distance)/20;
if (2*fhmos_bg.h2cm[i] < fhmos_cfg.th_gesture_mask_off_height_cm)
{
fhmos_bg.maskoff[i] = 0;
} else
{
fhmos_bg.maskoff[i] = 1;
}
//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");
APP_LOG(TS_OFF, VLEVEL_M, "|%4d ", fhmos_bg.h2cm[i]);
if (2*fhmos_bg.h2cm[i] < fhmos_cfg.th_gesture_mask_off_height_cm)
{
fhmos_bg.maskoff[i] = 0;
} else
{
fhmos_bg.maskoff[i] = 1;
}
//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");
APP_LOG(TS_OFF, VLEVEL_M, "|%4d ", fhmos_bg.h2cm[i]);
}
else {
fhmos_bg.h2cm[i] = 0;
//printf(" .%d. ", i);
}
}
else {
fhmos_bg.h2cm[i] = 0;
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 range_mode = 2; //STS_TOF_LONG_RANGE;
// int status = VL53L8A1_RANGING_SENSOR_GetDistance(VL53L8A1_DEV_CENTER, &Result);
//uint8_t center_roi[4] = {27,28,35,36};
// printf("\r| 27 | 28 | 35 | 36 |\r\n");
for (uint8_t k=0; k<1; k++)
{
STS_TOF_L8_Process();
if (ToF_EventDetected != 0)
{
ToF_EventDetected = 0;
status = VL53L8A1_RANGING_SENSOR_GetDistance(VL53L8A1_DEV_CENTER, &Result);
// int status = VL53L8A1_RANGING_SENSOR_GetDistance(VL53L8A1_DEV_CENTER, &Result);
// printf("\r\n status =%d \r\n", status);
for (uint8_t zone_nbr = 0; zone_nbr < 4; zone_nbr++)
{
#if 0
/* Print distance and status */
if (Result.ZoneResult[center_roi[zone_nbr]].NumberOfTargets > 0)
{
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];
if (status == BSP_ERROR_NONE)
{
range_distance= STS_Get_Center_Range_Distance(&Result);
} else {
APP_LOG(TS_OFF, VLEVEL_M, "\r\n x \r\n");
}
}
}
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 factor2_head_level = (sts_sensor_install_height - 10*fhmos_cfg.th_head_level_height_cm);
uint16_t factor1_floor_level_from_ceiling = sts_sensor_install_height; // 50mm min body height
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 (Result->ZoneResult[i].Distance[0] < head_distance)
head_distance = Result->ZoneResult[i].Distance[0]; // find out the min_distance
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 (0 == fhmos_bg.maskoff[i]) // only within the non-mask-off blocks
{ // 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
printf("\r\n Factor1_install_height=%d mm, F2 th_high=%d, th_head=%d, Factor2 (gap)=%d \r\n",
factor1_floor_level, (int)sts_high_threshold, (int)10*fhmos_cfg.th_head_level_height_cm, factor2_head_level);
#if 2
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_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);
sts_head_level_low = 1;
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;