workable Fall detection with code 2/3 upload and gesture map
This commit is contained in:
parent
d289b7b1e9
commit
a80b70e3dd
LoRaWAN/App
STM32CubeIDE/Release
STS
|
|
@ -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
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue