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

workable L8A with state upload 1/2/3

This commit is contained in:
Yunhorn 2025-05-29 13:49:50 +08:00
parent 77059d0e1c
commit 1f83a5c4c7
4 changed files with 51 additions and 29 deletions

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.

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

@ -550,10 +550,11 @@ void STS_YunhornSTSEventP5_Process(void)
STS_TOF_L8_Process();
OnSensorL8AStateChanged();
if (sts_fhmos_state_changed)
if (sts_fhmos_state_changed ==1)
{
sts_fhmos_state_changed = 0;
sts_fhmos_state_changed =0;
APP_LOG(TS_OFF, VLEVEL_M, "\r\n Upload Fall State Type 1 \r\n");
@ -564,6 +565,8 @@ void STS_YunhornSTSEventP5_Process(void)
#if 1
if (sts_fhmos_bitmap_pending == FHMOS_BITMAP_GENERATED)
{
UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0);
APP_LOG(TS_OFF, VLEVEL_M, "\r\n Upload Fall Gesture Type 2 , status=%d \r\n", sts_fhmos_bitmap_pending);
STS_FHMOS_sensor_upload_map(0x02);
} else if (sts_fhmos_bitmap_pending == FHMOS_BITMAP_2D_FINISHED) {
@ -2597,15 +2600,8 @@ void YunhornSTSDurationCheckTimer(void)
LED_RED_ON;
over_threshold |= 1<<6;
// upload state confirm message
if (sts_fhmos_bitmap_pending == FHMOS_BITMAP_BLANK)
{
APP_LOG(TS_OFF, VLEVEL_M, "\r\nGenerate Fall Gesture Map, bitmap state=%d \r\n", sts_fhmos_bitmap_pending);
// UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0);
UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0);
//APP_LOG(TS_OFF, VLEVEL_M, "\r\nGenerate Fall Gesture Map\r\n");
sts_generate_fall_gesture_map();
}
} else
{
@ -2620,6 +2616,16 @@ void YunhornSTSDurationCheckTimer(void)
// if (sts_fhmos_bitmap_pending == 1)
//APP_LOG(TS_OFF, VLEVEL_M, "\r\nFall state KEPT .....\r\n");
LED_RED_ON;
// upload state confirm message
if (sts_fhmos_bitmap_pending == FHMOS_BITMAP_BLANK)
{
APP_LOG(TS_OFF, VLEVEL_M, "\r\nGenerate Fall Gesture Map, bitmap state=%d \r\n", sts_fhmos_bitmap_pending);
// UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0);
//APP_LOG(TS_OFF, VLEVEL_M, "\r\nGenerate Fall Gesture Map\r\n");
sts_generate_fall_gesture_map();
}
break;
case STS_FHMOS_FALL_STATE_NORMAL:
@ -3142,9 +3148,9 @@ void OnSensorL8AStateChanged(void)
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;
} else {
sts_fhmos_state_changed = 0;
}
} //else {
// sts_fhmos_state_changed = 0;
//}
}
last_head_level_low_state = sts_head_level_low;

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

@ -55,7 +55,9 @@ 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 int to_confirm = 0;
static uint8_t yes_count=0, no_count=0;
#define TIME_C 2
static uint32_t STS_Get_Center_Range_Distance(RANGING_SENSOR_Result_t *Result);
uint16_t bg_distance[64]={0};
uint8_t idx[64]={0};
@ -424,12 +426,12 @@ void sts_generate_fall_gesture_map(void)
{
for (i = 0; i < 64; i++)
{
if (i % 8 ==0) APP_LOG(TS_OFF, VLEVEL_H, "\r\n[cm] ");
//if (i % 8 ==0) APP_LOG(TS_OFF, VLEVEL_H, "\r\n[cm] ");
if ((Result.ZoneResult[i].NumberOfTargets > 0))
{
bg_distance[i] += (uint16_t) Result.ZoneResult[i].Distance[0];
idx[i] ++;
APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", Result.ZoneResult[i].Distance[0]/10);
//APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", Result.ZoneResult[i].Distance[0]/10);
}
}
}
@ -438,9 +440,9 @@ void sts_generate_fall_gesture_map(void)
for (i=0; i< 64; i++)
{
if (i%8==0) APP_LOG(TS_OFF, VLEVEL_H, "\r\n %2d ", i);
//if (i%8==0) APP_LOG(TS_OFF, VLEVEL_H, "\r\n %2d ", i);
bg_distance[i] /= idx[i];
APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", bg_distance[i]/10);
//APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", bg_distance[i]/10);
}
@ -458,7 +460,7 @@ void sts_generate_fall_gesture_map(void)
}
fhmos_gesture.h2cm[i] = (uint8_t)abs(sts_sensor_install_height - range_distance)/10;
//APP_LOG(TS_OFF, VLEVEL_M, "\r\nGesture h_2cm:%4d maskoff height_cm: %4d \r\n", fhmos_gesture.h2cm[i], fhmos_cfg.th_gesture_mask_off_height_cm);
// APP_LOG(TS_OFF, VLEVEL_M, "\r\nGesture h_2cm:%4d maskoff height_cm: %4d \r\n", fhmos_gesture.h2cm[i], fhmos_cfg.th_gesture_mask_off_height_cm);
if (fhmos_gesture.h2cm[i] > fhmos_cfg.th_gesture_mask_off_height_cm)
{
fhmos_gesture.maskoff[i] = 1;
@ -502,8 +504,8 @@ void sts_generate_fall_gesture_map(void)
if (i%4 == 0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
//h4 = MIN(fhmos_gesture.h2cm[2*i+0],sts_gesture_mask_cap_height_2cm)/8;
//l4 = MIN(fhmos_gesture.h2cm[2*i+1],sts_gesture_mask_cap_height_2cm)/8;
h4 = 0xff&(fhmos_gesture.h2cm[2*i+0]);
l4 = 0xff&(fhmos_gesture.h2cm[2*i+1]);
h4 = 0xff&(fhmos_gesture.h2cm[2*i+0]/5); // 1 cm to 5 cm max 80 cm
l4 = 0xff&(fhmos_gesture.h2cm[2*i+1]/5);
fhmos_gesture.cube[i] = ((h4&0x0f)<<4)|(l4&0x0f);
APP_LOG(TS_OFF, VLEVEL_M, " [%2d_%2d]", (fhmos_gesture.cube[i]>>4)&0x0f, (fhmos_gesture.cube[i]&0x0f));
@ -616,6 +618,7 @@ void STS_TOF_L8_Process(void)
if (status == BSP_ERROR_NONE)
{
print_result(&Result);
} else {
APP_LOG(TS_OFF, VLEVEL_M, "\r\n x \r\n");
}
@ -825,23 +828,36 @@ static void print_result(RANGING_SENSOR_Result_t *Result)
&& (head_height_level_from_floor_mm >= (10*fhmos_cfg.th_fall_body_min_height_cm )))
{
//to_confirm ++;
if (to_confirm++ >= 3 )
if (yes_count < TIME_C)
{
sts_head_level_low = 1;
// to_confirm = 0;
yes_count ++;
} else {
no_count =0;
}
} else if ((head_distance_from_ceiling_mm < (factor2_head_level_from_floor_mm - 150))) // TODO XXX 50mm gap to avoid flapping back and forth
{
sts_head_level_low = 0;
sts_fhmos_bitmap_pending = FHMOS_BITMAP_BLANK;
to_confirm = 0;
APP_LOG(TS_OFF, VLEVEL_M, "\r\n return zero statues, bitmap status=%d \r\n", sts_fhmos_bitmap_pending);
if (no_count < TIME_C)
{
no_count ++;
} else {
yes_count =0;
}
}
if (yes_count > (TIME_C -1))
{
sts_head_level_low = 1;
} else if (no_count > (TIME_C -1))
{
OnSensorL8AStateChanged();
sts_head_level_low = 0;
sts_fhmos_bitmap_pending = FHMOS_BITMAP_BLANK;
APP_LOG(TS_OFF, VLEVEL_M, "\r\n return zero statues, bitmap status=%d \r\n", sts_fhmos_bitmap_pending);
}
//OnSensorL8AStateChanged();
}