diff --git a/LoRaWAN/App/lora_app.c b/LoRaWAN/App/lora_app.c index 79c589d..e290931 100644 --- a/LoRaWAN/App/lora_app.c +++ b/LoRaWAN/App/lora_app.c @@ -628,7 +628,7 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) //sts_lamp_bar_color = prev_color; sts_status_color = STS_VACANT_COLOR; sts_lamp_bar_color = STS_VACANT_COLOR; - APP_LOG(TS_OFF, VLEVEL_M,"\r\n EXIT_CALLBACK, Door Open: sts status color =%d, lampbar_color=%d \r\n", sts_status_color, sts_lamp_bar_color); + //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); } // 3) combine states and colors //STS_YunhornSTSEventP1_Process(); @@ -1237,6 +1237,17 @@ static void SendTxData(void) AppData.Buffer[i++] = fhmos_data.state_hall_1; AppData.Buffer[i++] = fhmos_data.state_hall_2; AppData.Buffer[i++] = fhmos_data.state_PIR; + + if (fhmos_data.state_fall_released == 1) + { + fhmos_data.state_fall_released == 0; + + AppData.Buffer[i++] = (uint8_t) 0xff&(fhmos_data.time_stamp_fall_confirmed>>24); + AppData.Buffer[i++] = (uint8_t) 0xff&(fhmos_data.time_stamp_fall_confirmed>>16); + AppData.Buffer[i++] = (uint8_t) 0xff&(fhmos_data.time_stamp_fall_confirmed>>8); + AppData.Buffer[i++] = (uint8_t) 0xff&(fhmos_data.time_stamp_fall_confirmed); + + } } // AppData.Buffer[i++] = 0x01; // payload type, 0x01= regular payload #if 0 diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin b/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin index aacf249..85c046f 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 26c8cee..bc5c16f 100644 Binary files a/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf and b/STM32CubeIDE/Release/WLE5CC_NODE_STS.elf differ diff --git a/STS/Core/Inc/yunhorn_sts_sensors.h b/STS/Core/Inc/yunhorn_sts_sensors.h index 1b66377..cf068d6 100644 --- a/STS/Core/Inc/yunhorn_sts_sensors.h +++ b/STS/Core/Inc/yunhorn_sts_sensors.h @@ -296,10 +296,10 @@ typedef struct sts_fhmos_sensor_data uint8_t color_occupancy; /* 0/blue: no occupy, 1/green:occupy yet normal, 3/red: over stay */ uint8_t color_sos_alarm; /* 1/green, sos on-duty, 2/yellow, sos button pressed 3/red, alarm */ - uint8_t state_human_movement_updated_tag; - uint8_t state_occupancy_updated_tag; - uint8_t state_sos_alarm_updated_tag; - uint8_t state_fall_updated_tag; + uint8_t state_human_movement_released; + uint8_t state_occupancy_released; + uint8_t state_sos_alarm_released; + uint8_t state_fall_released; uint8_t state_hall_1; uint8_t state_hall_2; @@ -351,8 +351,8 @@ typedef struct sts_fhmos_sensor_config uint8_t th_motionless_short_15sec; // 5 min - 10 min uint8_t th_motionless_long_15sec; // 10 min - 30 min uint8_t th_occupancy_overstay_15sec; // 10 min - 30 min - uint8_t th_gesture_mask_off_height_cm; // motion power level - uint8_t th_fhmos_cfg_reserve1; // reserved 1 + uint8_t th_gesture_mask_off_height_cm; // gesture mask off height cm + uint8_t th_fall_body_min_height_cm; // fall body min height from floor cm } sts_fhmos_sensor_config_t; diff --git a/STS/Core/Src/sts_cmox_hmac_sha.c b/STS/Core/Src/sts_cmox_hmac_sha.c index 20eb025..1e5e0c6 100644 --- a/STS/Core/Src/sts_cmox_hmac_sha.c +++ b/STS/Core/Src/sts_cmox_hmac_sha.c @@ -52,7 +52,7 @@ uint32_t sts_hmac_verify(void) hmac_result.ac_pass = (ret == 0x0)?1U:0U; - //APP_LOG(TS_OFF, VLEVEL_L, "\r\nHMAC Verify Success = %u \r\n", hmac_result.ac_pass); + APP_LOG(TS_OFF, VLEVEL_M, "\r\nHMAC Verify Success = %u \r\n", hmac_result.ac_pass); return ret; } diff --git a/STS/Core/Src/yunhorn_sts_process.c b/STS/Core/Src/yunhorn_sts_process.c index e593c7d..ddeba5e 100644 --- a/STS/Core/Src/yunhorn_sts_process.c +++ b/STS/Core/Src/yunhorn_sts_process.c @@ -62,6 +62,7 @@ volatile uint8_t sts_fall_detection_acc_threshold = 30, //0.3g sts_fall_detection_depth_threshold=20, sts_fall_confirm_threshold_in_10sec=1, sts_occupancy_overtime_threshold_in_10min=2; +extern volatile uint32_t sts_low_threshold, sts_high_threshold; extern volatile sts_fhmos_sensor_ambient_height_t fhmos_bg, fhmos_gesture; extern volatile uint8_t sts_status_color; extern volatile uint8_t sts_lamp_bar_color; //puColor @@ -151,7 +152,7 @@ volatile sts_cfg_nvm_t sts_cfg_nvm = { 0x50, // uint8_t fhmos_cfg_6; 80*15=1200sec, =20min 0x14, // th_gesture_mask_off_height_cm - 0x00, // reserved + 0x19, // reserved #elif 0x06, //reserve6 alarm_mute_or_reset_expire_timer_in_10sec, 60 seconds 0x23, //reserve7 alarm Lamp Bar Flashing color define, 0x20, 2==STS_RED, 0 = STS_DARK, 0x23, 2=STS_RED, 3=STS_BLUE @@ -545,7 +546,7 @@ void STS_YunhornSTSEventP5_Process(void) UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0); } - if (sts_fhmos_bitmap_pending == 0x01) + if (sts_fhmos_bitmap_pending == 1) { STS_FHMOS_sensor_upload_map(0x02); @@ -808,7 +809,7 @@ void USER_APP_Parse_CMD_P(uint8_t *parse_buffer, uint8_t parse_buffer_size) break; case 8: - fhmos_cfg.th_fhmos_cfg_reserve1 = sts_fhmos_cfg_value;; + fhmos_cfg.th_fall_body_min_height_cm = sts_fhmos_cfg_value;; break; case 9: // cmd for upload messages @@ -823,14 +824,14 @@ void USER_APP_Parse_CMD_P(uint8_t *parse_buffer, uint8_t parse_buffer_size) } if (sts_fhmos_cfg_index < 9) { - sts_cfg_nvm.fhmos_cfg_1 = fhmos_cfg.th_head_level_height_cm; - sts_cfg_nvm.fhmos_cfg_2 = fhmos_cfg.th_fall_duration_potential_15sec; - sts_cfg_nvm.fhmos_cfg_3 = fhmos_cfg.th_fall_duration_confirm_15sec; - sts_cfg_nvm.fhmos_cfg_4 = fhmos_cfg.th_motionless_short_15sec; - sts_cfg_nvm.fhmos_cfg_5 = fhmos_cfg.th_motionless_long_15sec; - sts_cfg_nvm.fhmos_cfg_6 = fhmos_cfg.th_occupancy_overstay_15sec; - sts_cfg_nvm.fhmos_cfg_7 = fhmos_cfg.th_gesture_mask_off_height_cm; - sts_cfg_nvm.fhmos_cfg_8 = fhmos_cfg.th_fhmos_cfg_reserve1; + sts_cfg_nvm.fhmos_cfg_1 = fhmos_cfg.th_head_level_height_cm; // default:70 -- 70 cm + sts_cfg_nvm.fhmos_cfg_2 = fhmos_cfg.th_fall_duration_potential_15sec; // default:04 -- 1 min + sts_cfg_nvm.fhmos_cfg_3 = fhmos_cfg.th_fall_duration_confirm_15sec; // default:08 -- 2 min + sts_cfg_nvm.fhmos_cfg_4 = fhmos_cfg.th_motionless_short_15sec; // default:08 -- 2 min + sts_cfg_nvm.fhmos_cfg_5 = fhmos_cfg.th_motionless_long_15sec; // default:14 --26 min + sts_cfg_nvm.fhmos_cfg_6 = fhmos_cfg.th_occupancy_overstay_15sec; // default:50 -- 20 min + sts_cfg_nvm.fhmos_cfg_7 = fhmos_cfg.th_gesture_mask_off_height_cm; // default:14 -- 20 cm + sts_cfg_nvm.fhmos_cfg_8 = fhmos_cfg.th_fall_body_min_height_cm; // default:19 -- 25 cm OnStoreSTSCFGContextRequest(); @@ -1240,6 +1241,9 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s break; case 'S': /* "YZS": Self Function Testing */ STS_SENSOR_Function_Test_Process(); + sts_cfg_nvm.sensor_install_height_in_10cm = sts_sensor_install_height/100; //in 10 cm, say 4500mm=450cm=45 dm + APP_LOG(TS_OFF, VLEVEL_M, "\n STS CFG NVM -> SENSOR INSTALL HEIGHT STORED = %d dm(10cm)\n", sts_cfg_nvm.sensor_install_height_in_10cm); + OnStoreSTSCFGContextRequest(); break; case 'C': /* "YZC": report LORAWAN CLASS A/B/C */ @@ -1730,11 +1734,11 @@ void OnStoreSTSCFGContextRequest(void) #endif if ((nvm_store_value[NVM_AC_CODE_START]!= 0x0) && (nvm_store_value[NVM_AC_CODE_START+19]!=0x0)) { - //APP_LOG(TS_OFF, VLEVEL_M, "\n\r Transfer good NVM Stored ac_code to NVM_STORE_VALUE\r\n"); + APP_LOG(TS_OFF, VLEVEL_M, "\n\r Transfer good NVM Stored ac_code to NVM_STORE_VALUE\r\n"); UTIL_MEM_cpy_8((void*)&to_store__value[NVM_AC_CODE_START], (void*)&nvm_store_value[NVM_AC_CODE_START],YUNHORN_STS_AC_CODE_SIZE); } else if ((sts_ac_code[0]!=0x0) && (sts_ac_code[YUNHORN_STS_AC_CODE_SIZE-1]!=0x0)) { - //APP_LOG(TS_OFF, VLEVEL_M, "\n\r Transfer new generated ac_code to NVM_STORE_VALUE\r\n"); + APP_LOG(TS_OFF, VLEVEL_M, "\n\r Transfer new generated ac_code to NVM_STORE_VALUE\r\n"); UTIL_MEM_cpy_8((void*)&to_store__value[NVM_AC_CODE_START], (void*)sts_ac_code,YUNHORN_STS_AC_CODE_SIZE); UTIL_MEM_cpy_8((void*)&nvm_store_value[NVM_AC_CODE_START], (void*)sts_ac_code,YUNHORN_STS_AC_CODE_SIZE); } @@ -1908,11 +1912,11 @@ void OnRestoreSTSCFGContextProcess(void) #endif - //for (uint8_t j=0; j< YUNHORN_STS_AC_CODE_SIZE; j++) - //{ - // sts_ac_code[j] = sts_cfg_nvm.ac[j]; - //} - //UTIL_MEM_cpy_8((void*)sts_ac_code,(void*)sts_cfg_nvm.ac, YUNHORN_STS_AC_CODE_SIZE); + for (uint8_t j=0; j< YUNHORN_STS_AC_CODE_SIZE; j++) + { + sts_ac_code[j] = sts_cfg_nvm.ac[j]; + } + UTIL_MEM_cpy_8((void*)sts_ac_code,(void*)sts_cfg_nvm.ac, YUNHORN_STS_AC_CODE_SIZE); #ifdef YUNHORN_STS_O6_ENABLED if ((sts_version == sts_cfg_nvm.version)&& (NVM_CFG_PARAMETER_SIZE == sts_cfg_nvm.length)) @@ -1942,8 +1946,10 @@ void STS_FHMOS_sensor_config_init(void) fhmos_cfg.th_occupancy_overstay_15sec = 80; // 80*15 = 1200 sec, 20 min. fhmos_cfg.th_gesture_mask_off_height_cm = 20; // gesture mask off height in cm - fhmos_cfg.th_fhmos_cfg_reserve1=0; // reserve + 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; } void STS_FHMOS_sensor_config_update() @@ -1955,8 +1961,8 @@ void STS_FHMOS_sensor_config_update() fhmos_cfg.th_motionless_short_15sec = sts_cfg_nvm.fhmos_cfg_4; // 4*15 = 60 sec , 1 min. fhmos_cfg.th_motionless_long_15sec = sts_cfg_nvm.fhmos_cfg_5; // 20*15 = 300 sec, 5 min. fhmos_cfg.th_occupancy_overstay_15sec = sts_cfg_nvm.fhmos_cfg_6; // 80*15 = 1200 sec, 20 min. - fhmos_cfg.th_gesture_mask_off_height_cm = sts_cfg_nvm.fhmos_cfg_7; - fhmos_cfg.th_fhmos_cfg_reserve1 = sts_cfg_nvm.fhmos_cfg_8; + 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 } @@ -2058,8 +2064,23 @@ void STS_SENSOR_Distance_Test_Process(void) STSWakeupScanTimerStop(); //STS_TOF_L8_Process(); + uint32_t range_once=0; + //MX_TOF_Process(); - sts_sensor_install_height = (uint16_t)MX_TOF_Ranging_Process(); + uint8_t rpt=0; + for (rpt=0; rpt<3; rpt++) + { + range_once = range_once + (uint16_t)MX_TOF_Ranging_Process(); + //APP_LOG(TS_OFF, VLEVEL_M, "\r\n Range_once=%d \r\n", range_once); + HAL_Delay(150); + } + + range_once /= 3; + + //APP_LOG(TS_OFF, VLEVEL_M, "\r\n Range_once=%d \r\n", range_once); + + sts_sensor_install_height = range_once; //(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); @@ -2234,12 +2255,12 @@ void OnYunhornSTSOORSSWakeUpTimerEvent(void *context) * @brief Yunhorn STS Sensor Heart Beat Timer callback function * @param context ptr of STS Sampling Check context */ -#if 0 +#if 1 void OnYunhornSTSHeartBeatTimerEvent(void *context) { -#if 0 +#if 1 uint8_t appDataPort, appBufferSize, appDataBuffer[32]; - appDataPort = YUNHORN_STS_P2_LORA_APP_HTBT_PORT; + appDataPort = YUNHORN_STS_L8_LORA_APP_HTBT_PORT; appBufferSize = 2; appDataBuffer[0]=0x80; appDataBuffer[1]=0x63; //TODO XXX change to battery level % @@ -2332,7 +2353,7 @@ void YunhornSTSDurationCheckTimer(void) sts_lamp_bar_color = STS_OCCUPANCY_OVERSTAY_COLOR; // APP_LOG(TS_OFF, VLEVEL_M, "\r\nSensor1 overstay set color to %d color:%s\r\n",sts_lamp_bar_color, (char*)code2color[sts_lamp_bar_color]); - over_threshold |= 1<<1; + over_threshold |= 1<<0; // APP_LOG(TS_OFF, VLEVEL_M, "\r\n 1<<1 Over Threshold =0x%02x \r\n", over_threshold); } @@ -2371,7 +2392,7 @@ void YunhornSTSDurationCheckTimer(void) // 2024-07-15 update, no overwrite sensor1 duration value //sts_o7_sensorData.over_stay_duration = sts_o7_sensorData.event_sensor2_duration; // - over_threshold |= 1<<2; + over_threshold |= 1<<1; // APP_LOG(TS_OFF, VLEVEL_M, "\r\n 1<<2 Over Threshold =0x%02x \r\n", over_threshold); // to be defiend later for SOS threshold TODO XXXX } else @@ -2456,8 +2477,8 @@ void YunhornSTSDurationCheckTimer(void) //sts_lamp_bar_color = STS_HUMAN_MOVEMENT_MOTIONLESS_NORMAL; } else { - over_threshold &= ~0x04; - over_threshold &= ~0x08; + over_threshold &= ~0x10; + over_threshold &= ~0x20; } @@ -2476,7 +2497,7 @@ void YunhornSTSDurationCheckTimer(void) sts_o7_sensorData.event_sensor3_fall_duration = current_time.Seconds - sts_o7_sensorData.event_sensor3_fall_start_time; //fhmos_data.head_low_level_duration = current_time.Seconds - fhmos_data.head_low_level_start_time; -#if 0 +#if 1 APP_LOG(TS_OFF, VLEVEL_M, "\r\n Head Low duration =%d \r\n Potential threshold=%d sec \r\n Confirm threshold=%d sec", sts_o7_sensorData.event_sensor3_fall_duration, fhmos_cfg.th_fall_duration_potential_15sec*15, @@ -2487,6 +2508,7 @@ void YunhornSTSDurationCheckTimer(void) { uint32_t time_stamp=STS_Get_Date_Time_Stamp(); //uint8_t datetimestamp[8]={0}; fhmos_data.state_fall = STS_FHMOS_FALL_STATE_CONFIRMED; + fhmos_data.state_fall_released = 0; fhmos_data.time_stamp_fall_confirmed = time_stamp; // APP_LOG(TS_OFF, VLEVEL_M, "\r\n 003 fall state set to %d \r\n",fhmos_data.state_fall); @@ -2510,6 +2532,7 @@ void YunhornSTSDurationCheckTimer(void) { // stay this state until > long threshold fhmos_data.state_fall = STS_FHMOS_FALL_STATE_POTENTIAL; + fhmos_data.state_fall_released = 0; // APP_LOG(TS_OFF, VLEVEL_M, "\r\n 002 Fall State =%d \r\n", fhmos_data.state_fall); // sts_lamp_bar_color = STS_FALL_SUSPICIOUS_COLOR; // APP_LOG(TS_OFF, VLEVEL_M, "\r\nSensor L8 set FALL POTENTIAL color to %d color:%s \r\n",sts_lamp_bar_color, (char*) code2color[sts_lamp_bar_color]); @@ -2524,9 +2547,10 @@ void YunhornSTSDurationCheckTimer(void) //fhmos_data.head_low_level_start_time = 0; sts_o7_sensorData.event_sensor3_fall_duration = 0; fhmos_data.state_fall = STS_FHMOS_FALL_STATE_NORMAL; + fhmos_data.state_fall_released = 1; sts_fhmos_bitmap_pending = 0; - over_threshold &= ~0x20; over_threshold &= ~0x40; + over_threshold &= ~0x80; } @@ -2632,7 +2656,8 @@ void YunhornSTSDurationCheckTimer(void) sts_cmb_result = sts_sensor1_changed<<0 |sts_sensor2_changed<<1 |sts_sensor3_changed<<2|sts_sensor4_changed<<3; // APP_LOG(TS_OFF, VLEVEL_M, "\r\n Over_threshold=0x%02x, sts cmb result =0x%02x, last_cmb_result=0x%02x \r\n", over_threshold, sts_cmb_result, last_sts_cmb_result); - if ((over_threshold != 0 ) && (last_sts_cmb_result != sts_cmb_result)) + //if ((over_threshold != 0 ) && (last_sts_cmb_result != sts_cmb_result)) + if ((last_sts_cmb_result != sts_cmb_result)) { // APP_LOG(TS_OFF, VLEVEL_M, "\r\n Over Threshold =0x%02x \r\n", over_threshold); last_sts_cmb_result = sts_cmb_result; @@ -2992,8 +3017,9 @@ 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_M, "\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; @@ -3003,8 +3029,9 @@ 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 ++++++++++ \r\n"); + APP_LOG(TS_OFF, VLEVEL_M, "\r\n Head Level Rise up ++++++++++ \r\n"); sts_fhmos_state_changed = 1; + fhmos_data.state_fall_released = 1; } } diff --git a/STS/TOF/App/app_tof.c b/STS/TOF/App/app_tof.c index b92ac84..7c5309c 100644 --- a/STS/TOF/App/app_tof.c +++ b/STS/TOF/App/app_tof.c @@ -40,7 +40,7 @@ volatile uint8_t fhmos_fall=0, fhmos_human_movement=0, fhmos_occupancy=0, fhmos_ volatile uint32_t fhmos_fall_counter=0; volatile uint32_t sts_low_threshold=1500, sts_high_threshold=2800, sts_occupancy_threshold=2300; extern volatile uint8_t sts_head_level_low; -volatile sts_fhmos_sensor_config_t fhmos_cfg={70,4,8,8,20,80,20,0}; +volatile sts_fhmos_sensor_config_t fhmos_cfg={70,4,8,8,20,80,20,25}; volatile sts_fhmos_sensor_cmd_t fhmos_cmd={0x0}; volatile sts_fhmos_sensor_ambient_height_t fhmos_bg={0x0}, fhmos_gesture={0x0}, fhmos_net={0x0}; volatile sts_fhmos_sensor_data_t fhmos_data={0}; @@ -160,6 +160,10 @@ void MX_TOF_Init(void) MX_53L8A1_ThresholdDetection_Init(); MX_53L8A1_ThresholdDetection_Process(); STS_FHMOS_sensor_config_init(); + //sts_high_threshold = sts_cfg_nvm.sensor_install_height_in_10cm ; + //sts_low_threshold = sts_cfg_nvm.sensor_install_height_in_10cm -1400; + STS_TOF_L8_Reconfig(); + #elif defined(STS_R1D) XWL55_WLE5_53L0X_Init(); #endif @@ -195,7 +199,24 @@ void STS_LMZ_Ambient_Height_Scan_Process(void) sts_low_threshold = sts_sensor_install_height-1400; STS_TOF_L8_Reconfig(); - STS_TOF_L8_Process(); + status = VL53L8A1_RANGING_SENSOR_Start(VL53L8A1_DEV_CENTER, RS_MODE_ASYNC_CONTINUOUS); + + if (status != BSP_ERROR_NONE) + { + printf("VL53L8A1_RANGING_SENSOR_Start failed\n"); + while (1); + } + + if (ToF_EventDetected != 0) + { + ToF_EventDetected = 0; + status = VL53L8A1_RANGING_SENSOR_GetDistance(VL53L8A1_DEV_CENTER, &Result); + if (status != BSP_ERROR_NONE) + { + APP_LOG(TS_OFF, VLEVEL_M, "\r\n x \r\n"); + } + } + //STS_TOF_L8_Process(); APP_LOG(TS_OFF, VLEVEL_M, "\r\n ----------------------" @@ -204,12 +225,12 @@ void STS_LMZ_Ambient_Height_Scan_Process(void) for (uint8_t i = 0; i < 64; i++) { /* Print distance and status */ - if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n"); + if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\nPosition from Floor [cm]| "); 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; // in 2 cm if (2*fhmos_bg.h2cm[i] < fhmos_cfg.th_gesture_mask_off_height_cm) { @@ -221,17 +242,17 @@ void STS_LMZ_Ambient_Height_Scan_Process(void) //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]); + APP_LOG(TS_OFF, VLEVEL_M, "|%3d ", fhmos_bg.h2cm[i]*2); } else { fhmos_bg.h2cm[i] = 0; - APP_LOG(TS_OFF, VLEVEL_M, " .%d. ", i); + APP_LOG(TS_OFF, VLEVEL_M, "|%3d ", fhmos_bg.h2cm[i]); } } - APP_LOG(TS_OFF, VLEVEL_M, "\r\n ------- Mask off matrix \r\n"); + APP_LOG(TS_OFF, VLEVEL_M, "\r\n\n ------- Mask off matrix \r\n"); for (i=0; i<64; i++) { @@ -274,8 +295,9 @@ void sts_generate_fall_gesture_map(void) sts_fall_head_position = range_distance; // simply find out the head level head_xy = i; // head x, y coordination in 8x8 matrix } - 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.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])<<(i%8); // debug // if (i%8==0) printf("\r\n"); @@ -287,6 +309,16 @@ void sts_generate_fall_gesture_map(void) if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n"); APP_LOG(TS_OFF, VLEVEL_M, "|%4d ", fhmos_gesture.h2cm[i]); } + + 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 ", fhmos_gesture.maskoff[i]); + } + + for (i=0; i<8; i++) + APP_LOG(TS_OFF, VLEVEL_M, "%02x",fhmos_gesture_bitmap[i]); + APP_LOG(TS_OFF, VLEVEL_M, "\r\n"); for (i = 0; i < 32; i++) { @@ -342,7 +374,9 @@ uint16_t MX_TOF_Ranging_Process(void) { 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, "\r\n status=%d, x \r\n", status); + return 0; } } @@ -484,8 +518,12 @@ static void MX_53L8A1_ThresholdDetection_Process(void) /* threshold parameters */ ITConfig.Criteria = RS_IT_IN_WINDOW; - ITConfig.LowThreshold = LOW_THRESHOLD; /* mm */ - ITConfig.HighThreshold = HIGH_THRESHOLD; /* mm */ + //ITConfig.LowThreshold = LOW_THRESHOLD; /* mm */ + //ITConfig.HighThreshold = HIGH_THRESHOLD; /* mm */ + + ITConfig.LowThreshold = sts_low_threshold; + ITConfig.HighThreshold = sts_high_threshold; + VL53L8A1_RANGING_SENSOR_ConfigIT(VL53L8A1_DEV_CENTER, &ITConfig); @@ -531,11 +569,15 @@ uint32_t STS_Get_Center_Range_Distance(RANGING_SENSOR_Result_t *Result) for (uint8_t i=0; i<4; i++) { + APP_LOG(TS_OFF, VLEVEL_M, "\r\nCenter[%d]: roi=%d Distance=%d \r\n", i, center_roi[i],Result->ZoneResult[center_roi[i]].Distance[0]); + center_range_distance +=(uint32_t)(Result->ZoneResult[center_roi[i]].Distance[0]); + } center_range_distance /= 4; + APP_LOG(TS_OFF, VLEVEL_M, "\r\n Center Range Average=%d mm\r\n", center_range_distance); return center_range_distance; } @@ -586,14 +628,14 @@ static void print_result(RANGING_SENSOR_Result_t *Result) #endif //int32_t roi_distance =(uint32_t)center_range_distance/4; 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; + 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; sts_head_level_low = 0; - +#if 0 for (i=0; i<64; i++) { - if (i%8 ==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n"); + if (i%8 ==0) APP_LOG(TS_OFF, VLEVEL_M, "Phase I \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) @@ -602,20 +644,47 @@ static void print_result(RANGING_SENSOR_Result_t *Result) // find out the min_distance or the highest position level } } + head_height_level_from_floor = sts_sensor_install_height - head_distance_from_ceiling; - 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 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); + APP_LOG(TS_OFF, VLEVEL_M, "\r\n #### Head distance from ceiling = %4d \r\n Head distance from floor =%d \r\n", + head_distance_from_ceiling, head_height_level_from_floor); #endif - 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) )) + //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}; + + for (i=0; i<30; i++) + fhmos_bg.maskoff[rio_edge[i]] = 1; + + for (i=0; i<64; i++) + { + //if (i%8 ==0) APP_LOG(TS_OFF, VLEVEL_M, "Phase II \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_height_level_from_floor = sts_sensor_install_height - head_distance_from_ceiling; +#if 0 + APP_LOG(TS_OFF, VLEVEL_M, "\r\n #### Head distance from ceiling = %4d \r\n Head distance from floor =%d \r\n", + head_distance_from_ceiling, head_height_level_from_floor); +#endif + /* state tree */ +#if 0 + APP_LOG(TS_OFF, VLEVEL_M,"\r\n factor1_floor_level_from_ceiling=%d mm\r\n, \r\nFactor2 (factor2_head_level_from_floor)=%d \r\n Min body height=%d cm\r\n", + factor1_floor_level_from_ceiling, factor2_head_level_from_floor, fhmos_cfg.th_fall_body_min_height_cm); + +#endif + APP_LOG(TS_OFF, VLEVEL_M, "\r\nBodyLow=%d < Obj: %d > Head_Max=%d\r\n", 10*fhmos_cfg.th_fall_body_min_height_cm, head_height_level_from_floor, (10*fhmos_cfg.th_head_level_height_cm)); + 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 ))) { - 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)); + APP_LOG(TS_OFF, VLEVEL_M,"\r\n YELLOW/RED state: (head level from floor=%d (mm)\r\n", head_height_level_from_floor); sts_head_level_low = 1; @@ -685,6 +754,12 @@ void STS_FHMOS_sensor_read(sts_fhmos_sensor_data_t *sts_data) sts_data->time_stamp_fall_confirmed = fhmos_data.time_stamp_fall_confirmed; sts_data->time_stamp_motionless_confirmed = fhmos_data.time_stamp_motionless_confirmed; sts_data->time_stamp_overstay_confirmed = fhmos_data.time_stamp_overstay_confirmed; + sts_data->time_stamp_fall_released = fhmos_data.time_stamp_fall_released; + sts_data->time_stamp_overstay_released = fhmos_data.time_stamp_overstay_released; + sts_data->time_stamp_motionless_released= fhmos_data.time_stamp_motionless_released; + sts_data->state_fall_released = fhmos_data.state_fall_released; + sts_data->state_occupancy_released = fhmos_data.state_occupancy_released; + sts_data->state_human_movement_released = fhmos_data.state_human_movement_released; sts_data->lamp_bar_color = sts_lamp_bar_color; sts_data->state_hall_1 = sts_hall1_read;