diff --git a/LoRaWAN/App/lora_app.c b/LoRaWAN/App/lora_app.c index 9dd3174..8aa1b36 100644 --- a/LoRaWAN/App/lora_app.c +++ b/LoRaWAN/App/lora_app.c @@ -962,18 +962,21 @@ void STS_SENSOR_Upload_Message(uint8_t appDataPort, uint8_t appBufferSize, uint8 AppData.Port = appDataPort; AppData.BufferSize = appBufferSize; + APP_LOG(TS_OFF, VLEVEL_M, "\n\r AppPort=%d BufferSize=%d \r\n", AppData.Port,AppData.BufferSize); status = LmHandlerSend(&AppData, LmHandlerParams.IsTxConfirmed, false); if (LORAMAC_HANDLER_SUCCESS == status) { - APP_LOG(TS_ON, VLEVEL_H, "SEND REQUEST\r\n"); + APP_LOG(TS_ON, VLEVEL_M, "SEND REQUEST\r\n"); } else if (LORAMAC_HANDLER_DUTYCYCLE_RESTRICTED == status) { + APP_LOG(TS_ON, VLEVEL_M, "DUTYCYCLE RESTRICTED\r\n"); + nextTxIn = LmHandlerGetDutyCycleWaitTime(); if (nextTxIn > 0) { - APP_LOG(TS_ON, VLEVEL_H, "Next Tx in : ~%d second(s)\r\n", (nextTxIn / 1000)); + APP_LOG(TS_ON, VLEVEL_M, "Next Tx in : ~%d second(s)\r\n", (nextTxIn / 1000)); } } @@ -1204,8 +1207,51 @@ static void SendTxData(void) #if defined(L8) //AppData.Buffer[i++] = 4; - AppData.Buffer[i++] = 9; - AppData.Buffer[i++] = 0x01; // payload type, 0x01= regular payload + + if ((fhmos_data.state_fall ==3)||(fhmos_data.state_occupancy ==3)||(fhmos_data.state_human_movement ==3)) + { + AppData.Buffer[i++] = 10; + AppData.Buffer[i++] = 0x03; // payload type, 0x01= regular payload + AppData.Buffer[i++] = fhmos_data.state_fall; + AppData.Buffer[i++] = fhmos_data.state_human_movement; + AppData.Buffer[i++] = fhmos_data.state_occupancy; + AppData.Buffer[i++] = fhmos_data.state_sos_alarm; + AppData.Buffer[i++] = fhmos_data.lamp_bar_color; + if (fhmos_data.state_fall ==3) + { + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_fall_confirmed>>24); + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_fall_confirmed>>16); + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_fall_confirmed>>8); + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_fall_confirmed); + } else if (fhmos_data.state_occupancy ==3){ + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_overstay_confirmed>>24); + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_overstay_confirmed>>16); + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_overstay_confirmed>>8); + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_overstay_confirmed); + + } else if (fhmos_data.state_human_movement ==3) + { + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_motionless_confirmed>>24); + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_motionless_confirmed>>16); + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_motionless_confirmed>>8); + AppData.Buffer[i++] = 0xff&(fhmos_data.time_stamp_motionless_confirmed); + } + + } else { + AppData.Buffer[i++] = 9; + AppData.Buffer[i++] = 0x01; + AppData.Buffer[i++] = fhmos_data.state_fall; + AppData.Buffer[i++] = fhmos_data.state_human_movement; + AppData.Buffer[i++] = fhmos_data.state_occupancy; + AppData.Buffer[i++] = fhmos_data.state_sos_alarm; + + AppData.Buffer[i++] = fhmos_data.lamp_bar_color; + AppData.Buffer[i++] = fhmos_data.state_hall_1; + AppData.Buffer[i++] = fhmos_data.state_hall_2; + AppData.Buffer[i++] = fhmos_data.state_PIR; + } + // AppData.Buffer[i++] = 0x01; // payload type, 0x01= regular payload +#if 0 AppData.Buffer[i++] = fhmos_data.state_fall; AppData.Buffer[i++] = fhmos_data.state_human_movement; AppData.Buffer[i++] = fhmos_data.state_occupancy; @@ -1215,6 +1261,7 @@ 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; +#endif #elif defined(L8) sts_data->lamp_bar_color = sts_lamp_bar_color; diff --git a/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin b/STM32CubeIDE/Release/WLE5CC_NODE_STS.bin index e9f8235..e082685 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 501c1bc..45a4994 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 fd43f8b..fc3927f 100644 --- a/STS/Core/Inc/yunhorn_sts_sensors.h +++ b/STS/Core/Inc/yunhorn_sts_sensors.h @@ -351,11 +351,19 @@ 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_motion_power_level; // motion power level + uint8_t th_gesture_mask_off_height_cm; // motion power level uint8_t th_fhmos_cfg_reserve1; // reserved 1 } sts_fhmos_sensor_config_t; +typedef struct sts_fhmos_sensor_cmd +{ + uint8_t cmd_index; + uint8_t cmd_value; + +} sts_fhmos_sensor_cmd_t; + + // volatile uint8_t sts_hall1_read=STS_Status_Door_Open,sts_hall2_read=STS_Status_SOS_Release; // Above hall1_read == reed_hall_result, hall2_read == emergency_button typedef struct sts_fhmos_sensor_ambient_height { @@ -502,6 +510,8 @@ void OnSensorL8DStateChanged(void); void OnSensorPIR1StateChanged(void); +void STS_FHMOS_sensor_upload_map(uint8_t map_index); + uint32_t STS_Get_Date_Time_Stamp(void);//uint32_t *time_stamp, uint8_t *datetimestamp); void STS_Reed_Hall_Presence_Detection(void); diff --git a/STS/Core/Src/yunhorn_sts_process.c b/STS/Core/Src/yunhorn_sts_process.c index a8ad28a..d990125 100644 --- a/STS/Core/Src/yunhorn_sts_process.c +++ b/STS/Core/Src/yunhorn_sts_process.c @@ -70,6 +70,7 @@ extern volatile uint8_t sts_lamp_bar_flashing_color; //0x23; RED_BLUE; volatile uint8_t sts_cloud_netcolor = STS_BLUE; //netColor extern volatile uint8_t sts_occupancy_status; extern volatile sts_fhmos_sensor_config_t fhmos_cfg; +extern volatile sts_fhmos_sensor_cmd_t fhmos_cmd; extern volatile uint8_t sts_status_color, sts_lamp_bar_color;//puColor extern uint8_t luminance_level; volatile uint8_t sts_fhmos_state_changed=0; @@ -148,7 +149,7 @@ volatile sts_cfg_nvm_t sts_cfg_nvm = { 0x50, // uint8_t fhmos_cfg_6; 80*15=1200sec, =20min - 0x32, // motion power level + 0x14, // th_gesture_mask_off_height_cm 0x00, // reserved #elif 0x06, //reserve6 alarm_mute_or_reset_expire_timer_in_10sec, 60 seconds @@ -172,7 +173,6 @@ volatile uint8_t sts_service_mask=STS_SERVICE_MASK_L0; volatile uint8_t sts_work_mode=STS_TOF_LMZ_RSS_MODE; volatile uint32_t rfac_timer=0; volatile uint16_t sts_sensor_install_height=3000; -volatile uint8_t sts_gesture_mask_off_height_cm=10, sts_gesture_mask_cap_height_cm=160; volatile uint8_t sensor_data_ready=0; #if defined(STS_R1)||defined(STS_R5)||defined(STS_R4)||defined(STS_R1D) @@ -544,7 +544,14 @@ void STS_YunhornSTSEventP5_Process(void) UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0); } + if (sts_fhmos_bitmap_pending == 0x01) + { + STS_FHMOS_sensor_upload_map(0x02); + + } + +#if 0 if (sts_fhmos_bitmap_pending == 0x01) { APP_LOG(TS_OFF, VLEVEL_M, "\r\n +++++++++++++++++++++++ \r\nBitmap Pending to upload flag=%02x \r\n", sts_fhmos_bitmap_pending); @@ -555,7 +562,6 @@ void STS_YunhornSTSEventP5_Process(void) tstbuf[i++] = (uint8_t) 0x80; tstbuf[i++] = (uint8_t) sts_mtmcode1; tstbuf[i++] = (uint8_t) sts_mtmcode2; - tstbuf[i++] = (uint8_t) sts_version; tstbuf[i++] = (uint8_t) sts_hardware_ver; tstbuf[i++] = (uint8_t) (99*GetBatteryLevel()/254)&0xff; @@ -578,7 +584,7 @@ void STS_YunhornSTSEventP5_Process(void) sts_fhmos_bitmap_pending = 2; APP_LOG(TS_OFF, VLEVEL_M, "\r\n +++++++++++++++++++++++ \r\nBitmap Pending to uploaded\r\n"); } - +#endif #endif @@ -796,31 +802,40 @@ void USER_APP_Parse_CMD_P(uint8_t *parse_buffer, uint8_t parse_buffer_size) fhmos_cfg.th_occupancy_overstay_15sec = sts_fhmos_cfg_value; break; case 7: - fhmos_cfg.th_motion_power_level = sts_fhmos_cfg_value; + fhmos_cfg.th_gesture_mask_off_height_cm = sts_fhmos_cfg_value; break; case 8: fhmos_cfg.th_fhmos_cfg_reserve1 = sts_fhmos_cfg_value;; break; + case 9: // cmd for upload messages + fhmos_cmd.cmd_index = sts_fhmos_cfg_index; + fhmos_cmd.cmd_value = sts_fhmos_cfg_value; - default: - break; + STS_FHMOS_sensor_upload_map(sts_fhmos_cfg_value); + + break; + default: + break; } + 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; - 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_motion_power_level; - sts_cfg_nvm.fhmos_cfg_8 = fhmos_cfg.th_fhmos_cfg_reserve1; + OnStoreSTSCFGContextRequest(); - OnStoreSTSCFGContextRequest(); - // 2024-11-18 - UTIL_MEM_set_8((void*)outbuf, 0x0, sizeof(outbuf)); - UTIL_MEM_cpy_8((void*)outbuf,(void*)parse_buffer,parse_buffer_size); - i = parse_buffer_size; - STS_SENSOR_Upload_Message(YUNHORN_STS_USER_APP_CTRL_REPLY_PORT, i, (uint8_t *)outbuf); + // 2024-11-18 + UTIL_MEM_set_8((void*)outbuf, 0x0, sizeof(outbuf)); + UTIL_MEM_cpy_8((void*)outbuf,(void*)parse_buffer,parse_buffer_size); + i = parse_buffer_size; + STS_SENSOR_Upload_Message(YUNHORN_STS_USER_APP_CTRL_REPLY_PORT, i, (uint8_t *)outbuf); + } } break; @@ -1922,7 +1937,7 @@ void STS_FHMOS_sensor_config_init(void) fhmos_cfg.th_motionless_long_15sec = 20; // 20*15 = 300 sec, 5 min. fhmos_cfg.th_occupancy_overstay_15sec = 80; // 80*15 = 1200 sec, 20 min. - fhmos_cfg.th_motion_power_level = 50; // motion power level average + fhmos_cfg.th_gesture_mask_off_height_cm = 20; // gesture mask off height in cm fhmos_cfg.th_fhmos_cfg_reserve1=0; // reserve } @@ -1936,12 +1951,68 @@ 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_motion_power_level = sts_cfg_nvm.fhmos_cfg_7; + 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; } +void STS_FHMOS_sensor_upload_map(uint8_t map_index) +{ +#if 1 + //if (sts_fhmos_bitmap_pending == 0x01) + { + APP_LOG(TS_OFF, VLEVEL_M, "\r\n +++++++++++++++++++++++ \r\nBitmap Pending to upload flag=%02x \r\n", sts_fhmos_bitmap_pending); + + uint8_t tstbuf[32] ={0x0}; uint8_t i=0; + + //tstbuf[i++] = (uint8_t) 'G'; + tstbuf[i++] = (uint8_t) 0x80; + tstbuf[i++] = (uint8_t) sts_mtmcode1; + tstbuf[i++] = (uint8_t) sts_mtmcode2; + tstbuf[i++] = (uint8_t) sts_hardware_ver; + tstbuf[i++] = (uint8_t) (99*GetBatteryLevel()/254)&0xff; + if (map_index == 0x02) // gesture bitmap + { + tstbuf[i++] = (uint8_t) (11)&0xff; //length of following data + tstbuf[i++] = (uint8_t) 0x02; // payload type 0x02=Gesture map + tstbuf[i++] = (uint8_t) (fhmos_gesture.head_level)&0xff; // head height in cm + tstbuf[i++] = (uint8_t) (fhmos_gesture.head_xy)&0xff; + // MASK OFF BITMAP + tstbuf[i++] = (uint8_t) (fhmos_gesture_bitmap[0]); + tstbuf[i++] = (uint8_t) (fhmos_gesture_bitmap[1]); + tstbuf[i++] = (uint8_t) (fhmos_gesture_bitmap[2]); + tstbuf[i++] = (uint8_t) (fhmos_gesture_bitmap[3]); + tstbuf[i++] = (uint8_t) (fhmos_gesture_bitmap[4]); + tstbuf[i++] = (uint8_t) (fhmos_gesture_bitmap[5]); + tstbuf[i++] = (uint8_t) (fhmos_gesture_bitmap[6]); + tstbuf[i++] = (uint8_t) (fhmos_gesture_bitmap[7]); + + sts_fhmos_bitmap_pending = 2; + + } else if (map_index == 0x01) + { + tstbuf[i++] = (uint8_t) (9)&0xff; //length of following data + tstbuf[i++] = (uint8_t) 0x01; // payload type 0x02=Gesture map + // MASK OFF background mask bitmap + tstbuf[i++] = (uint8_t) (sts_mask_bitmap[0]); + tstbuf[i++] = (uint8_t) (sts_mask_bitmap[1]); + tstbuf[i++] = (uint8_t) (sts_mask_bitmap[2]); + tstbuf[i++] = (uint8_t) (sts_mask_bitmap[3]); + tstbuf[i++] = (uint8_t) (sts_mask_bitmap[4]); + tstbuf[i++] = (uint8_t) (sts_mask_bitmap[5]); + tstbuf[i++] = (uint8_t) (sts_mask_bitmap[6]); + tstbuf[i++] = (uint8_t) (sts_mask_bitmap[7]); + + } + + STS_SENSOR_Upload_Message(YUNHORN_STS_L8_LORA_APP_DATA_PORT, i, (uint8_t *)tstbuf); + + APP_LOG(TS_OFF, VLEVEL_M, "\r\n +++++++++++++++++++++++ \r\nBitmap Pending uploaded\r\n"); + } +#endif + +} #endif @@ -2599,6 +2670,8 @@ void OnSensor1StateChanged(void) fhmos_data.state_fall = STS_FHMOS_FALL_STATE_NORMAL; fhmos_data.state_human_movement = STS_FHMOS_HUMAN_MOVEMENT_NORMAL; fhmos_data.state_sos_alarm = STS_FHMOS_SOS_ALARM_BUTTON_NORMAL; + sts_o7_sensorData.event_sensor3_fall_start_time = sensor_event_time.Seconds; + //sts_o7_sensorData.event_sensor3_no_movement_start_time = sensor_event_time.Seconds; sts_head_level_low = 0; } else if (sts_hall1_read==STS_Status_Door_Open) { @@ -2609,7 +2682,7 @@ void OnSensor1StateChanged(void) sts_o7_sensorData.event_sensor3_motion_start_time = 0; sts_o7_sensorData.event_sensor3_motion_stop_time =0; sts_o7_sensorData.event_sensor3_no_movement_start_time =0; - + //sts_o7_sensorData.event_sensor3_fall_start_time = sensor_event_time.Seconds; //sts_o7_sensorData.event_sensor1_duration = 0; sts_o7_sensorData.over_stay_state = 0; @@ -2918,6 +2991,7 @@ void OnSensorL8AStateChanged(void) } else if ((sts_head_level_low == 0) && (last_head_level_low_state ==1)) { 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"); sts_fhmos_state_changed = 1; } diff --git a/STS/TOF/App/app_tof.c b/STS/TOF/App/app_tof.c index 04e99fd..b12fad3 100644 --- a/STS/TOF/App/app_tof.c +++ b/STS/TOF/App/app_tof.c @@ -39,9 +39,9 @@ extern "C" { volatile uint8_t fhmos_fall=0, fhmos_human_movement=0, fhmos_occupancy=0, fhmos_sos_alarm=0; 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_gesture_mask_off_height_cm, sts_gesture_mask_cap_height_cm; extern volatile uint8_t sts_head_level_low; -volatile sts_fhmos_sensor_config_t fhmos_cfg={70,4,8,8,20,80,50,0}; +volatile sts_fhmos_sensor_config_t fhmos_cfg={70,4,8,8,20,80,20,0}; +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}; extern volatile uint8_t sts_fhmos_result; @@ -176,7 +176,7 @@ void STS_LMZ_Ambient_Height_Scan_Process(void) { #if 1 - uint8_t i=0, repeat=1; + uint8_t i=0; uint32_t range_distance =0; for (i=0; i<64; i++) @@ -193,45 +193,52 @@ void STS_LMZ_Ambient_Height_Scan_Process(void) sts_high_threshold = sts_sensor_install_height; sts_low_threshold = sts_sensor_install_height-1400; STS_TOF_L8_Reconfig(); - //printf("%c[2H", 27); /* clear screen */ - for (uint8_t k=0; k 0)) - { - range_distance = (uint32_t)Result.ZoneResult[i].Distance[0]; + /* Print distance and status */ + if ((Result.ZoneResult[i].NumberOfTargets > 0)) + { + range_distance = (uint32_t)Result.ZoneResult[i].Distance[0]; - //if (sts_sensor_install_height > range_distance) - fhmos_bg.h2cm[i] += ((uint32_t)sts_sensor_install_height - range_distance); - if (abs(fhmos_bg.h2cm[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 fhmos_gesture.head_xy = head_xy&0xff; - printf("\r\n Head level =%4d cm, Head_xy=%d X:Y = %2d : %2d \r\n", fhmos_gesture.head_level, head_xy, (head_xy%8), (head_xy/8)); + APP_LOG(TS_OFF, VLEVEL_M, "\r\n Head level =%4d cm, Head_xy=%d X:Y = %2d : %2d \r\n", fhmos_gesture.head_level, head_xy, (head_xy%8), (head_xy/8)); #if 0 for (i=0; i<64; i++) @@ -331,25 +340,26 @@ uint16_t MX_TOF_Ranging_Process(void) for (uint8_t zone_nbr = 0; zone_nbr < 4; zone_nbr++) { - /* Print distance and status */ - if (Result.ZoneResult[center_roi[zone_nbr]].NumberOfTargets > 0) - { - printf("| %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 { - printf("| -- "); - } - + #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]; } } - printf("\n\r"); + APP_LOG(TS_OFF, VLEVEL_M, "\n\r"); range_distance /=40; - printf("| %u mm\r\n", (uint16_t)range_distance); + APP_LOG(TS_OFF, VLEVEL_M, "| %u mm\r\n", (uint16_t)range_distance); return (uint16_t) range_distance; #endif @@ -400,7 +410,7 @@ void STS_TOF_L8_Process(void) { print_result(&Result); } else { - printf("\r\n x \r\n"); + APP_LOG(TS_OFF, VLEVEL_M, "\r\n x \r\n"); } } #endif @@ -528,29 +538,22 @@ static void MX_53L8A1_ThresholdDetection_Process(void) static void print_result(RANGING_SENSOR_Result_t *Result) { int8_t i; - int8_t j; - //int8_t k; - //int8_t l; - //uint8_t zones_per_line; - //static uint8_t prev_occupy_state=0; + //int8_t j; uint32_t center_range_distance=0; uint8_t center_roi[4] = {27,28,35,36}; - static uint32_t prev_distance[64]={0}; - uint32_t motion_diff=0, motion_power=0; + //static uint32_t prev_distance[64]={0}; + //uint32_t motion_diff=0, motion_power=0; + uint16_t head_distance=8000; //uint32_t motion_power_threshold = 3200; // 64*50mm //SysTime_t sensor_event_time = SysTimeGetMcuTime(); // uint32_t time_stamp=STS_Get_Date_Time_Stamp(); - - - //printf("\r\n Motion level Calculation \r\n"); - +#if 0 for (j=0; j<64; j++) { if (prev_distance[j] > (uint32_t)Result->ZoneResult[j].Distance[0]) motion_diff = (prev_distance[j] - (uint32_t)Result->ZoneResult[j].Distance[0]); else motion_diff = (uint32_t)Result->ZoneResult[j].Distance[0] - prev_distance[j]; - //printf("\r\nj=%d, diff=%d ", j, motion_diff); motion_power += motion_diff; prev_distance[j] = (uint16_t)Result->ZoneResult[j].Distance[0]; @@ -562,6 +565,7 @@ static void print_result(RANGING_SENSOR_Result_t *Result) } else { fhmos_human_movement =0; } +#endif #if 0 printf("\r\n Motion Power =%d Average=%d Human Movement =%d, th=%d \r\n", motion_power*64, motion_power, fhmos_human_movement, fhmos_cfg.th_motion_power_level); #endif @@ -576,23 +580,29 @@ static void print_result(RANGING_SENSOR_Result_t *Result) center_range_distance +=(uint32_t)(Result->ZoneResult[center_roi[i]].Distance[0]); } - //printf("\n\r Center Range =%4d mm\r\n",center_range_distance ); - //int32_t roi_distance =(uint32_t)(Result->ZoneResult[j + k].Distance[l]); - int32_t roi_distance =(uint32_t)center_range_distance/4; - uint32_t factor1 = sts_sensor_install_height; - uint32_t factor2 = (sts_high_threshold - 10*fhmos_cfg.th_head_level_height_cm); - /* state tree */ - //printf("\r\n th_head_level_cm=%d, factor2=high_th:%d - mm:%d\r\n", fhmos_cfg.th_head_level_height_cm, sts_high_threshold, factor2); - //printf("\r\n roi_D < Facot1 =%d , D factor2)) - { - //printf("\r\nThreshold: low =%d occupy=%d head level=%d high=%d\r\n", sts_low_threshold, sts_occupancy_threshold, fhmos_cfg.th_head_level_height_cm*10, sts_high_threshold); - printf("\r\n YELLOW OR RED Distance=%ld \r\n", roi_distance); - sts_head_level_low = 1; + 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 + } + } + /* state tree */ + printf("\r\n Factor1_install_height=%d mm, F2 th_high=%d, th_head=%d, Factor2 (gap)=%d \r\n", + factor1, (int)sts_high_threshold, (int)10*fhmos_cfg.th_head_level_height_cm, factor2); + + if ((head_distance <= factor1) && (head_distance >= factor2)) + { + + printf("\r\n YELLOW OR RED Distance=%d \r\n", head_distance); + sts_head_level_low = 1; #if 0 fhmos_data.state_fall = STS_FHMOS_FALL_STATE_POTENTIAL; fhmos_data.color_fall = STS_FALL_SUSPICIOUS_COLOR; @@ -601,22 +611,17 @@ static void print_result(RANGING_SENSOR_Result_t *Result) //sts_fhmos_state_changed |=1; - } else if ((roi_distance < factor2)) // TODO XXX - { - sts_head_level_low = 0; - fhmos_data.state_fall = STS_FHMOS_FALL_STATE_NORMAL; - sts_fhmos_bitmap_pending = 0; - //fhmos_data.color_fall = STS_FALL_NORMAL_COLOR; // HOLD THIS COLOR BEFORE ENTER YELLOW STATUS - //printf("\r\n NORMAL NO FALL Distance=%4ld COLOR =%d \r\n", roi_distance, fhmos_data.color_fall); + } else if ((head_distance < (factor2 - 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; + sts_fhmos_bitmap_pending = 0; + + } - } OnSensorL8AStateChanged(); - //sts_fhmos_result = 1; - - //STS_Combined_Status_Processing(); - } @@ -657,10 +662,13 @@ static void print_result(RANGING_SENSOR_Result_t *Result) void STS_FHMOS_sensor_read(sts_fhmos_sensor_data_t *sts_data) { //uint8_t fhmos_fall=0, fhmos_human_movement=0, fhmos_occupancy=0, fhmos_sos_alarm=0; - sts_data->state_occupancy = fhmos_data.state_occupancy; - sts_data->state_fall = fhmos_data.state_fall; - sts_data->state_human_movement = fhmos_data.state_human_movement; - sts_data->state_sos_alarm = fhmos_data.state_sos_alarm; + sts_data->state_occupancy = fhmos_data.state_occupancy; + sts_data->state_fall = fhmos_data.state_fall; + sts_data->state_human_movement = fhmos_data.state_human_movement; + sts_data->state_sos_alarm = fhmos_data.state_sos_alarm; + 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->lamp_bar_color = sts_lamp_bar_color; sts_data->state_hall_1 = sts_hall1_read;