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

revised with edge removing for PWH jumbo roll issue

This commit is contained in:
Yunhorn 2025-01-06 21:36:09 +08:00
parent 94fc44f3c8
commit 3fd8f4736d
4 changed files with 36 additions and 10 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.

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

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

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

@ -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 )))
{
sts_head_level_low = 1;
to_confirm ++;
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();
}