diff --git a/LoRaWAN/App/lora_app.c b/LoRaWAN/App/lora_app.c index 75d22eb..83f4844 100644 --- a/LoRaWAN/App/lora_app.c +++ b/LoRaWAN/App/lora_app.c @@ -664,7 +664,8 @@ void STS_Sensor_Init(void) /* VVVVVV migrated to yunhorn_sts_process.c */ APP_LOG(TS_OFF, VLEVEL_M, "\r\n ------- L8 Init\r\n"); - STS_TOF_L8_Init(); + MX_TOF_Init(); + //STS_TOF_L8_Init(); } void STS_Sensor_Prepare(void) @@ -677,15 +678,17 @@ void STS_Sensor_Prepare(void) void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { + static uint8_t prev_sts_hall1_read=0; switch (GPIO_Pin) { case BUT1_Pin: /* Note: when "EventType == TX_ON_TIMER" this GPIO is not initialized */ HAL_Delay(100); - __HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin); + //__HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin); //__HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin); sts_hall1_read = HALL1_STATE; + APP_LOG(TS_OFF, VLEVEL_M,"\r\n EXIT_CALLBACK, Door state =%d \r\n", sts_hall1_read); if (sts_hall1_read == STS_Status_Door_Close) { sts_lamp_bar_color = STS_OCCUPY_COLOR; @@ -699,6 +702,9 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) // APP_LOG(TS_OFF, VLEVEL_M,"\r\n EXTI_CALLBACK, Door Open: sts status color =%d, lampbar_color=%d \r\n", sts_status_color, sts_lamp_bar_color); } + if (sts_hall1_read == prev_sts_hall1_read) return; + prev_sts_hall1_read = sts_hall1_read; + //if (last_sts_hall1_read != sts_hall1_read) // to eliminate power drop issue of PCB with long REEDSWITCH CABLES { // 1) record event start/stop time diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin b/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin index 1355970..6697eeb 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 55ca246..9a87f14 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 87c24fb..30bf60b 100644 --- a/STS/Core/Src/yunhorn_sts_process.c +++ b/STS/Core/Src/yunhorn_sts_process.c @@ -564,7 +564,8 @@ void STS_YunhornSTSEventP5_Process(void) { 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); - HAL_Delay(2000); + } else if (sts_fhmos_bitmap_pending == FHMOS_BITMAP_2D_FINISHED) { + APP_LOG(TS_OFF, VLEVEL_M, "\r\n Upload Fall Gesture Type 4 , status=%d \r\n", sts_fhmos_bitmap_pending); STS_FHMOS_sensor_upload_map(0x04); } @@ -1944,7 +1945,8 @@ void OnRestoreSTSCFGContextProcess(void) #endif #ifdef L8 - STS_FHMOS_sensor_config_update(); + STS_FHMOS_sensor_config_init(); + //STS_FHMOS_sensor_config_update(); #endif } @@ -1965,9 +1967,9 @@ void STS_FHMOS_sensor_config_init(void) fhmos_cfg.th_gesture_mask_off_height_cm = 20; // gesture mask off height in cm fhmos_cfg.th_fall_body_min_height_cm = 25; // fall body min height from floor 25 cm - sts_high_threshold = sts_cfg_nvm.sensor_install_height_in_10cm*100; - sts_low_threshold = sts_high_threshold - 1400; - APP_LOG(TS_OFF, VLEVEL_M, "\r\n FHMOS cfg high= %4d low= %4d \r\n", sts_high_threshold, sts_low_threshold); + //sts_high_threshold = sts_cfg_nvm.sensor_install_height_in_10cm*100; + //sts_low_threshold = sts_high_threshold - 1400; + //APP_LOG(TS_OFF, VLEVEL_M, "\r\n FHMOS cfg high= %4d low= %4d \r\n", sts_high_threshold, sts_low_threshold); } void STS_FHMOS_sensor_config_update() @@ -1982,6 +1984,10 @@ void STS_FHMOS_sensor_config_update() fhmos_cfg.th_gesture_mask_off_height_cm = sts_cfg_nvm.fhmos_cfg_7; // 20 cm fhmos_cfg.th_fall_body_min_height_cm = sts_cfg_nvm.fhmos_cfg_8; // 25 cm + sts_high_threshold = sts_cfg_nvm.sensor_install_height_in_10cm*100; + sts_low_threshold = sts_high_threshold - 1400; + APP_LOG(TS_OFF, VLEVEL_M, "\r\n FHMOS cfg high= %4d low= %4d \r\n", sts_high_threshold, sts_low_threshold); + } void STS_FHMOS_sensor_upload_map(uint8_t map_index) @@ -2055,7 +2061,11 @@ void STS_FHMOS_sensor_upload_map(uint8_t map_index) int status=STS_SENSOR_Upload_Message(YUNHORN_STS_L8_LORA_APP_DATA_PORT, i, (uint8_t *)tstbuf); if ((map_index ==0x02)&& (status ==0)) { - sts_fhmos_bitmap_pending = FHMOS_BITMAP_FINISHED; // 2; + sts_fhmos_bitmap_pending = FHMOS_BITMAP_2D_FINISHED; // 2; + } else if ((map_index ==0x04)&& (status ==0)) + { + + sts_fhmos_bitmap_pending = FHMOS_BITMAP_FINISHED; // 4; } APP_LOG(TS_OFF, VLEVEL_M, "\r\n +++++++++++++++++++++++ \r\nBitmap Pending uploaded, fhmos_bitmap_statue=%d\r\n", sts_fhmos_bitmap_pending); } diff --git a/STS/TOF/App/app_tof.c b/STS/TOF/App/app_tof.c index 809ab18..75b25d3 100644 --- a/STS/TOF/App/app_tof.c +++ b/STS/TOF/App/app_tof.c @@ -57,8 +57,22 @@ extern volatile uint8_t sts_fhmos_bitmap_pending; #include "stm32wlxx_nucleo.h" static int to_confirm = 0; static uint32_t STS_Get_Center_Range_Distance(RANGING_SENSOR_Result_t *Result); +uint16_t bg_distance[64]={0}; +uint8_t idx[64]={0}; +uint8_t IDX_LEN=10; + #if defined(NARROW_CUBICLE) -static 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}; +//static 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}; +#define RIO_EDGE_SIZE 34 +static uint8_t rio_edge[RIO_EDGE_SIZE]={0, 1, 2, 3, 4, 5, 6, 7, //first line + 8,14,15, + 16,22,23, + 24,30,31, + 32,38,39, + 40,46,47, + 48,54,55, + 56,57,58,59,60,61,62,63}; // last line + #endif //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}; /* Private typedef -----------------------------------------------------------*/ @@ -189,9 +203,6 @@ void STS_LMZ_Ambient_Height_Scan_Process(void) uint8_t i=0; uint32_t range_distance =0; uint8_t j=0; - uint16_t bg_distance[64]={0}; - uint8_t idx[64]={0}; - uint8_t IDX_LEN=10; uint8_t prev_sts_lamp_bar_color = sts_lamp_bar_color; sts_lamp_bar_color = STS_BLUE; @@ -200,6 +211,8 @@ void STS_LMZ_Ambient_Height_Scan_Process(void) { fhmos_bg.h2cm[i] = 0; fhmos_bg.maskoff[i] = 0; + bg_distance[i] = 0; + idx[i] = 0; } for (i=0;i<8;i++) sts_mask_bitmap[i] =0x0; @@ -242,13 +255,13 @@ void STS_LMZ_Ambient_Height_Scan_Process(void) for (i = 0; i < 64; i++) { if (i % 8 ==0) { - APP_LOG(TS_OFF, VLEVEL_H, "\r\n[cm] "); + APP_LOG(TS_OFF, VLEVEL_M, "\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_M, "|%3d ", Result.ZoneResult[i].Distance[0]/10); //} //else APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", 0); @@ -259,12 +272,12 @@ void STS_LMZ_Ambient_Height_Scan_Process(void) HAL_Delay(10); } - APP_LOG(TS_OFF, VLEVEL_H, "\r\n\n\nAverage Distance==\r\n"); + APP_LOG(TS_OFF, VLEVEL_M, "\r\n\n\n=========Average Distance=========\r\n\n"); 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_M, "\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_M, "|%3d ", bg_distance[i]/10); } //STS_TOF_L8_Process(); @@ -295,10 +308,10 @@ void STS_LMZ_Ambient_Height_Scan_Process(void) if ((uint16_t)fhmos_bg.h2cm[i] < (uint16_t)(fhmos_cfg.th_gesture_mask_off_height_cm)) { - fhmos_bg.maskoff[i] = 0; + fhmos_bg.maskoff[(7-i%8)*8+i/8] = 0; // update 2025 05 27 } else { - fhmos_bg.maskoff[i] = 1; + fhmos_bg.maskoff[(7-i%8)*8+i/8] = 1; } //sts_mask_bitmap[(uint8_t)(i/8)] |= (fhmos_bg.maskoff[i])<<(7-i%8); @@ -321,12 +334,12 @@ void STS_LMZ_Ambient_Height_Scan_Process(void) for (i=0; i<64; i++) { sts_mask_bitmap[(uint8_t)(i/8)] |= (fhmos_bg.maskoff[i])<<(7-i%8); // 2025-JAN-03 update - 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]); + 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]); } #if defined(NARROW_CUBICLE) - for (i=0; i<34; i++) + for (i=0; i 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); + } + } + } + HAL_Delay(10); + } + + for (i=0; i< 64; 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); + } + + for (uint8_t i = 0; i < 64; i++) { - if ((Result.ZoneResult[i].NumberOfTargets > 0) && (fhmos_bg.maskoff[i]==0)) + //if ((Result.ZoneResult[i].NumberOfTargets > 0) && (fhmos_gesture.maskoff[i]==0)) { - range_distance = (uint32_t)Result.ZoneResult[i].Distance[0]; + // range_distance = (uint32_t)Result.ZoneResult[i].Distance[0]; + range_distance = (uint32_t)bg_distance[i]; if (range_distance < sts_fall_head_position) { sts_fall_head_position = range_distance; // simply find out the head level @@ -382,14 +428,15 @@ void sts_generate_fall_gesture_map(void) fhmos_gesture.h2cm[i] = (uint8_t)abs(sts_sensor_install_height - range_distance)/20; //fhmos_gesture.maskoff[i] = ((2*fhmos_gesture.h2cm[i])< fhmos_cfg.th_gesture_mask_off_height_cm)? 0:1; fhmos_gesture.maskoff[i] = (fhmos_gesture.h2cm[i]==0)? 0:1; - fhmos_gesture_bitmap[(uint8_t)(i/8)] |= (fhmos_gesture.maskoff[i])<<(7 - i%8); // 2025-JAN-03 UPDATE + //fhmos_gesture_bitmap[(uint8_t)(i/8)] |= (fhmos_gesture.maskoff[i])<<(7 - i%8); // 2025-JAN-03 UPDATE + fhmos_gesture_bitmap[(uint8_t)((7-i/8)*8+i%8)] |= (fhmos_gesture.maskoff[i])<<(i%8); // 2025-05-27 UPDATE // debug // if (i%8==0) printf("\r\n"); // printf("|%4ld %4d ", range_distance, fhmos_gesture.h2cm[i]); } - else { - fhmos_gesture.h2cm[i] = 0; - } + //else { + //fhmos_gesture.h2cm[i] = 0; + //} if (i%8==0) APP_LOG(TS_OFF, VLEVEL_L, "\r\n"); APP_LOG(TS_OFF, VLEVEL_L, "|%4d ", fhmos_gesture.h2cm[i]); } @@ -397,7 +444,7 @@ void sts_generate_fall_gesture_map(void) 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 ", fhmos_gesture.maskoff[i]); + APP_LOG(TS_OFF, VLEVEL_M, "|%d ", fhmos_gesture.maskoff[i]); } for (i=0; i<8; i++) @@ -410,10 +457,11 @@ void sts_generate_fall_gesture_map(void) if (i%4 == 0) APP_LOG(TS_OFF, VLEVEL_L, "\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 = fhmos_gesture.h2cm[2*i+0]; - l4 = fhmos_gesture.h2cm[2*i+1]; + h4 = 0xff&(fhmos_gesture.h2cm[2*i+0]); + l4 = 0xff&(fhmos_gesture.h2cm[2*i+1]); + fhmos_gesture.cube[i] = ((h4&0x0f)<<4)|(l4&0x0f); - APP_LOG(TS_OFF, VLEVEL_L, " [%2d_%2d]", (fhmos_gesture.cube[i]>>4)&0x0f, (fhmos_gesture.cube[i]&0x0f)); + APP_LOG(TS_OFF, VLEVEL_M, " [%2d_%2d]", (fhmos_gesture.cube[i]>>4)&0x0f, (fhmos_gesture.cube[i]&0x0f)); } fhmos_gesture.head_level = 0xff&((sts_sensor_install_height - sts_fall_head_position)/10); // head level from floor in CM @@ -502,18 +550,18 @@ void STS_TOF_L8_Init(void) { MX_53L8A1_ThresholdDetection_Init(); MX_53L8A1_ThresholdDetection_Process(); - } void STS_TOF_L8_Process(void) { - + //status = VL53L8A1_RANGING_SENSOR_Start(VL53L8A1_DEV_CENTER, RS_MODE_ASYNC_CONTINUOUS); //while (1) { /* interrupt mode */ if (ToF_EventDetected != 0) + //while(ToF_EventDetected==0){}; { ToF_EventDetected = 0; #if 1 @@ -614,7 +662,7 @@ static void MX_53L8A1_ThresholdDetection_Process(void) //ITConfig.HighThreshold = HIGH_THRESHOLD; /* mm */ sts_high_threshold = sts_sensor_install_height+ 400; - sts_low_threshold = sts_high_threshold - 1400; + sts_low_threshold = sts_high_threshold - 1550; APP_LOG(TS_OFF, VLEVEL_H, "\r\n Threshold High=%4d, Low=%4d \r\n", sts_high_threshold, sts_low_threshold); ITConfig.LowThreshold = sts_low_threshold; @@ -685,11 +733,11 @@ static void print_result(RANGING_SENSOR_Result_t *Result) { int8_t i; - uint16_t head_distance_from_ceiling=8000; + uint16_t head_distance_from_ceiling_mm=8000; //printf("%c[2H", 27); /* clear screen */ - uint16_t factor1_floor_level_from_ceiling = sts_sensor_install_height; // 50mm min body height - uint16_t factor2_head_level_from_floor = (10*fhmos_cfg.th_head_level_height_cm); //(sts_sensor_install_height - 10*fhmos_cfg.th_head_level_height_cm); - uint16_t head_height_level_from_floor=0; + uint16_t factor1_floor_level_from_ceiling_mm = sts_sensor_install_height; // 50mm min body height + uint16_t factor2_head_level_from_floor_mm = (10*fhmos_cfg.th_head_level_height_cm); //(sts_sensor_install_height - 10*fhmos_cfg.th_head_level_height_cm); + uint16_t head_height_level_from_floor_mm=0; sts_head_level_low = 0; @@ -697,28 +745,37 @@ static void print_result(RANGING_SENSOR_Result_t *Result) //APP_LOG(TS_OFF, VLEVEL_M, "\r\n Eliminate edge \r\n"); //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}; #if defined(NARROW_CUBICLE) - for (i=0; i<36; i++) + for (i=0; iZoneResult[i].Distance[0], Result->ZoneResult[i].NumberOfTargets, fhmos_bg.maskoff[i]); 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]; + if ((Result->ZoneResult[i].Distance[0] < head_distance_from_ceiling_mm) + && (Result->ZoneResult[i].NumberOfTargets > 0)) + head_distance_from_ceiling_mm = Result->ZoneResult[i].Distance[0]; // find out the min_distance or the highest position level } } + //head_distance_from_ceiling = MIN(4000,head_distance_from_ceiling); + APP_LOG(TS_OFF, VLEVEL_M, "\r\n Head Distance from ceiling= %4d mm \r\n", head_distance_from_ceiling_mm); - head_height_level_from_floor = MIN(1400, abs(sts_sensor_install_height - head_distance_from_ceiling)); + //head_height_level_from_floor_mm = MIN(1400, abs(sts_sensor_install_height - head_distance_from_ceiling_mm)); - APP_LOG(TS_OFF, VLEVEL_L, "\r\n Updown distance=%d cm, Floor Up head =%d cm\r\n", head_distance_from_ceiling/10, head_height_level_from_floor/10); + APP_LOG(TS_OFF, VLEVEL_M, "\r\n Updown distance=%d cm, Floor Up head =%d cm\r\n", head_distance_from_ceiling_mm/10, head_height_level_from_floor_mm/10); /* 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 ))) + if ((head_distance_from_ceiling_mm <= factor1_floor_level_from_ceiling_mm) + && ((head_height_level_from_floor_mm) <= (10*fhmos_cfg.th_head_level_height_cm)) + && (head_height_level_from_floor_mm >= (10*fhmos_cfg.th_fall_body_min_height_cm ))) { to_confirm ++; if (to_confirm >=3 ) @@ -727,7 +784,7 @@ static void print_result(RANGING_SENSOR_Result_t *Result) // 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 + } 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; diff --git a/STS/TOF/App/app_tof.h b/STS/TOF/App/app_tof.h index 3da9110..0dd1d51 100644 --- a/STS/TOF/App/app_tof.h +++ b/STS/TOF/App/app_tof.h @@ -58,6 +58,7 @@ typedef enum { typedef enum { FHMOS_BITMAP_BLANK=0, FHMOS_BITMAP_GENERATED, + FHMOS_BITMAP_2D_FINISHED, FHMOS_BITMAP_FINISHED } fhmos_bitmap_state;