diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin b/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin index a163f66..aacf249 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 23354ea..26c8cee 100644 Binary files a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf and b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf differ diff --git a/STM32CubeIDE/Release/makefile b/STM32CubeIDE/Release/makefile index 4f7e63f..36e5c5c 100644 --- a/STM32CubeIDE/Release/makefile +++ b/STM32CubeIDE/Release/makefile @@ -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 diff --git a/STM32CubeIDE/Release/sources.mk b/STM32CubeIDE/Release/sources.mk index dfd77e9..4110fdd 100644 --- a/STM32CubeIDE/Release/sources.mk +++ b/STM32CubeIDE/Release/sources.mk @@ -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 \ diff --git a/STS/Core/Inc/yunhorn_sts_sensors.h b/STS/Core/Inc/yunhorn_sts_sensors.h index 1b91589..1b66377 100644 --- a/STS/Core/Inc/yunhorn_sts_sensors.h +++ b/STS/Core/Inc/yunhorn_sts_sensors.h @@ -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); diff --git a/STS/Core/Src/yunhorn_sts_process.c b/STS/Core/Src/yunhorn_sts_process.c index 167595e..e593c7d 100644 --- a/STS/Core/Src/yunhorn_sts_process.c +++ b/STS/Core/Src/yunhorn_sts_process.c @@ -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(); diff --git a/STS/TOF/App/app_tof.c b/STS/TOF/App/app_tof.c index edddc5c..b92ac84 100644 --- a/STS/TOF/App/app_tof.c +++ b/STS/TOF/App/app_tof.c @@ -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;