diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin b/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin index 66720f3..df64575 100644 Binary files a/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin and b/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin differ diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf index f50763f..3c454ef 100644 Binary files a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf and b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf differ diff --git a/STS/Core/Src/yunhorn_sts_process.c b/STS/Core/Src/yunhorn_sts_process.c index 36a8498..86cc5bd 100644 --- a/STS/Core/Src/yunhorn_sts_process.c +++ b/STS/Core/Src/yunhorn_sts_process.c @@ -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; diff --git a/STS/TOF/App/app_tof.c b/STS/TOF/App/app_tof.c index 89577d5..7af13bd 100644 --- a/STS/TOF/App/app_tof.c +++ b/STS/TOF/App/app_tof.c @@ -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(); }