wip good progress
This commit is contained in:
parent
b725c385a3
commit
77059d0e1c
|
|
@ -702,13 +702,13 @@ 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;
|
||||
//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
|
||||
APP_LOG(TS_OFF, VLEVEL_M,"\r\n EXIT_CALLBACK, Door state =%d \r\n", sts_hall1_read);
|
||||
//APP_LOG(TS_OFF, VLEVEL_M,"\r\n EXIT_CALLBACK, Door state =%d \r\n", sts_hall1_read);
|
||||
OnSensor1StateChanged();
|
||||
// 2) change lamp bar color
|
||||
|
||||
|
|
@ -1563,7 +1563,8 @@ static void OnJoinRequest(LmHandlerJoinParams_t *joinParams)
|
|||
|
||||
UTIL_TIMER_Start(&YunhornSTSHeartBeatTimer);
|
||||
|
||||
UTIL_TIMER_Start(&YunhornSTSSelfFunctionTestTimer);
|
||||
// UTIL_TIMER_Start(&YunhornSTSSelfFunctionTestTimer);
|
||||
// update 2025 05 28 ... not to start given sts mask bitmap has been stored and restored
|
||||
|
||||
// UTIL_TIMER_Start(&TxTimer);
|
||||
// UTIL_TIMER_Start(&YunhornSTSWakeUpScanTimer);
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
|
|
@ -512,7 +512,7 @@ void OnSensorL8CStateChanged(void);
|
|||
void OnSensorL8DStateChanged(void);
|
||||
|
||||
void OnSensorPIR1StateChanged(void);
|
||||
|
||||
void sts_maskoff_update(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);
|
||||
|
|
|
|||
|
|
@ -117,10 +117,12 @@ volatile sts_cfg_nvm_t sts_cfg_nvm = {
|
|||
0x05, //inter frame deviation time const 0x05=[5]*0.1=0.5f
|
||||
0x0A, //inter frame fast cutoff 0x0A=[10] = 10U
|
||||
0x01, //inter frame slow cutoff,0x01=1[1]*0.01=0.01f
|
||||
|
||||
0x00, //intra frame time const [0]=0 Lower to reduce sensitivity, higher to increase sensitivity
|
||||
0x0A, //intra frame weight, 0x00=[0]*0.1=0.0F 0x0A=10, 10*0.1=1 FOR FAST MOVEMENT TRACKING FALL DETECTION
|
||||
0x05, //output time const 0x05=[5]*0.1=0.5
|
||||
0x02, //downsampling factor [2]=2
|
||||
|
||||
0x03, //power saving mode ACTIVE [3] = 3U
|
||||
0x00, //reserve --P[17]
|
||||
0x00, //reserve --P[18]
|
||||
|
|
@ -1269,6 +1271,7 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s
|
|||
break;
|
||||
case 'G': /* "YZG": Ambient Height scan process */
|
||||
STS_LMZ_Ambient_Height_Scan_Process();
|
||||
OnStoreSTSCFGContextRequest();
|
||||
i = 0;
|
||||
outbuf[i++] = (uint8_t)'G';
|
||||
outbuf[i++] = (uint8_t)sts_mtmcode1;
|
||||
|
|
@ -1718,10 +1721,18 @@ void OnStoreSTSCFGContextRequest(void)
|
|||
to_store__value[i++] = sts_cfg_nvm.sts_ioc_mask;
|
||||
to_store__value[i++] = (uint8_t) STS_CFG_PCFG_SIZE; //sts_cfg_nvm.length;
|
||||
|
||||
#if defined(L8)
|
||||
for (j = 0; j < 8; j++) {
|
||||
to_store__value[i++] = (sts_mask_bitmap[j]);
|
||||
}
|
||||
for (j = 0; j < STS_CFG_PCFG_SIZE-8; j++) {
|
||||
to_store__value[i++] = 0x0;
|
||||
}
|
||||
#else
|
||||
for (j = 0; j < STS_CFG_PCFG_SIZE; j++) {
|
||||
to_store__value[i++] = (sts_cfg_nvm.p[j]);
|
||||
}
|
||||
|
||||
#endif
|
||||
to_store__value[i++] = sts_cfg_nvm.reserve02;
|
||||
to_store__value[i++] = sts_cfg_nvm.reserve03;
|
||||
to_store__value[i++] = sts_cfg_nvm.sensor_install_height_in_10cm;
|
||||
|
|
@ -1820,7 +1831,22 @@ void STS_REBOOT_CONFIG_Init(void)
|
|||
for (uint8_t j=0; j< sts_cfg_nvm.length; j++) {
|
||||
sts_cfg_nvm.p[j] = (uint8_t)nvm_store_value[NVM_CFG_START+j];
|
||||
}
|
||||
|
||||
#if defined(L8)
|
||||
for (uint8_t j=0; j<8; j++) {
|
||||
sts_mask_bitmap[j] = (uint8_t)nvm_store_value[NVM_CFG_START+j];
|
||||
}
|
||||
for (uint8_t j=0; j < 64; j++)
|
||||
{
|
||||
//fhmos_bg.maskoff[(7-j%8)*8+j/8] = sts_mask_bitmap[j/8]>>(7-j%8)&0x01;
|
||||
fhmos_bg.maskoff[j] = sts_mask_bitmap[j/8]>>(7-j%8)&0x01;
|
||||
}
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\n Prev Stored mask bitmap ----------\r\n");
|
||||
for (uint8_t j=0; j<64; j++)
|
||||
{
|
||||
if (j%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "%2d ", fhmos_bg.maskoff[j]);
|
||||
}
|
||||
#endif
|
||||
sts_cfg_nvm.reserve02 =(uint8_t)nvm_store_value[NVM_RESERVE02];
|
||||
sts_cfg_nvm.reserve03 =(uint8_t)nvm_store_value[NVM_RESERVE03];
|
||||
sts_cfg_nvm.sensor_install_height_in_10cm =(uint8_t)nvm_store_value[NVM_SENSOR_INSTALL_HEIGHT];
|
||||
|
|
@ -1970,6 +1996,7 @@ void STS_FHMOS_sensor_config_init(void)
|
|||
//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_maskoff_update();
|
||||
}
|
||||
|
||||
void STS_FHMOS_sensor_config_update()
|
||||
|
|
@ -1988,6 +2015,7 @@ void STS_FHMOS_sensor_config_update()
|
|||
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_maskoff_update();
|
||||
}
|
||||
|
||||
void STS_FHMOS_sensor_upload_map(uint8_t map_index)
|
||||
|
|
@ -2572,8 +2600,8 @@ void YunhornSTSDurationCheckTimer(void)
|
|||
// upload state confirm message
|
||||
if (sts_fhmos_bitmap_pending == FHMOS_BITMAP_BLANK)
|
||||
{
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\nGenerate Fall Gesture Map, bitmap stateu=%d \r\n", sts_fhmos_bitmap_pending);
|
||||
UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0);
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\nGenerate Fall Gesture Map, bitmap state=%d \r\n", sts_fhmos_bitmap_pending);
|
||||
// UTIL_SEQ_SetTask((1 << CFG_SEQ_Task_LoRaSendOnTxTimerOrButtonEvent), CFG_SEQ_Prio_0);
|
||||
|
||||
//APP_LOG(TS_OFF, VLEVEL_M, "\r\nGenerate Fall Gesture Map\r\n");
|
||||
sts_generate_fall_gesture_map();
|
||||
|
|
|
|||
|
|
@ -59,11 +59,13 @@ 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;
|
||||
uint8_t IDX_LEN=20;
|
||||
|
||||
#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};
|
||||
#define RIO_EDGE_SIZE 34
|
||||
//static uint8_t rio_edge[RIO_EDGE_SIZE]={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};
|
||||
|
||||
// 2025 05 27
|
||||
static uint8_t rio_edge[RIO_EDGE_SIZE]={0, 1, 2, 3, 4, 5, 6, 7, //first line
|
||||
8,14,15,
|
||||
16,22,23,
|
||||
|
|
@ -73,6 +75,16 @@ static uint8_t rio_edge[RIO_EDGE_SIZE]={0, 1, 2, 3, 4, 5, 6, 7, //first line
|
|||
48,54,55,
|
||||
56,57,58,59,60,61,62,63}; // last line
|
||||
|
||||
/* 2025 05 28
|
||||
static uint8_t rio_edge[RIO_EDGE_SIZE]={0, 1, 2, 3, 4, 5, 6, 7, //first line
|
||||
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}; // 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 -----------------------------------------------------------*/
|
||||
|
|
@ -225,7 +237,7 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
|
|||
|
||||
sts_high_threshold = sts_sensor_install_height;
|
||||
sts_low_threshold = sts_sensor_install_height-1400;
|
||||
//APP_LOG(TS_OFF, VLEVEL_M, "\r\nReconfig ----> Threshold High=%d mm, Low=%d mm \r\n", sts_high_threshold, sts_low_threshold);
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\nReconfig ----> Threshold High=%d mm, Low=%d mm \r\n", sts_high_threshold, sts_low_threshold);
|
||||
|
||||
STS_TOF_L8_Reconfig();
|
||||
#if 1
|
||||
|
|
@ -254,14 +266,14 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
|
|||
//} else {
|
||||
for (i = 0; i < 64; i++)
|
||||
{
|
||||
if (i % 8 ==0) {
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\n[cm] ");
|
||||
}
|
||||
//if (i % 8 ==0) {
|
||||
// APP_LOG(TS_OFF, VLEVEL_H, "\r\n");
|
||||
//}
|
||||
//if ((Result.ZoneResult[i].NumberOfTargets > 0))
|
||||
//{
|
||||
bg_distance[i] += (uint16_t) Result.ZoneResult[i].Distance[0];
|
||||
idx[i] ++;
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "|%3d ", Result.ZoneResult[i].Distance[0]/10);
|
||||
// APP_LOG(TS_OFF, VLEVEL_H, " %3d ", Result.ZoneResult[i].Distance[0]/10);
|
||||
//}
|
||||
//else APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", 0);
|
||||
|
||||
|
|
@ -269,15 +281,16 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
|
|||
|
||||
//}
|
||||
}
|
||||
HAL_Delay(10);
|
||||
//HAL_Delay(1);
|
||||
}
|
||||
// status = VL53L8A1_RANGING_SENSOR_Stop(VL53L8A1_DEV_CENTER);
|
||||
|
||||
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_M, "\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_M, "|%3d ", bg_distance[i]/10);
|
||||
//APP_LOG(TS_OFF, VLEVEL_M, "|%3d ", bg_distance[i]/10);
|
||||
}
|
||||
//STS_TOF_L8_Process();
|
||||
|
||||
|
|
@ -287,7 +300,7 @@ 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_L, "\r\n[cm]|");
|
||||
if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n |");
|
||||
// if ((Result.ZoneResult[i].NumberOfTargets > 0))
|
||||
if (1)
|
||||
{
|
||||
|
|
@ -295,23 +308,29 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
|
|||
range_distance = (uint16_t)bg_distance[i]; // mm
|
||||
if (sts_sensor_install_height > range_distance)
|
||||
{
|
||||
fhmos_bg.h2cm[i] = (sts_sensor_install_height - range_distance)/10;
|
||||
//fhmos_bg.h2cm[i] = (sts_sensor_install_height - range_distance)/10;
|
||||
fhmos_bg.h2cm[(7-i%8)*8+i/8] = (sts_sensor_install_height - range_distance)/10;
|
||||
} else {
|
||||
|
||||
fhmos_bg.h2cm[i] = 0;
|
||||
//fhmos_bg.h2cm[i] = 0;
|
||||
fhmos_bg.h2cm[(7-i%8)*8+i/8] = 0;
|
||||
}
|
||||
|
||||
|
||||
//fhmos_bg.h2cm[i] = (uint16_t)(0, (sts_sensor_install_height/10 - range_distance)); // in cm
|
||||
|
||||
APP_LOG(TS_OFF, VLEVEL_L, "|%3d ", (uint16_t)fhmos_bg.h2cm[i]);
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "|%3d ", (uint16_t)fhmos_bg.h2cm[i]);
|
||||
|
||||
if ((uint16_t)fhmos_bg.h2cm[i] < (uint16_t)(fhmos_cfg.th_gesture_mask_off_height_cm))
|
||||
{
|
||||
fhmos_bg.maskoff[(7-i%8)*8+i/8] = 0; // update 2025 05 27
|
||||
// fhmos_bg.maskoff[(7-i%8)*8+i/8] = 0; // update 2025 05 27
|
||||
fhmos_bg.maskoff[i] = 0;
|
||||
//fhmos_bg.maskoff[(7-i%8)*8+i/8] = 0; // update 2025 05 27
|
||||
} else
|
||||
{
|
||||
fhmos_bg.maskoff[(7-i%8)*8+i/8] = 1;
|
||||
// fhmos_bg.maskoff[(7-i%8)*8+i/8] = 1;
|
||||
fhmos_bg.maskoff[i] = 1;
|
||||
//fhmos_bg.maskoff[(7-i%8)*8+i/8] = 1; // update 2025 05 27
|
||||
}
|
||||
|
||||
//sts_mask_bitmap[(uint8_t)(i/8)] |= (fhmos_bg.maskoff[i])<<(7-i%8);
|
||||
|
|
@ -322,50 +341,63 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
|
|||
}
|
||||
else {
|
||||
fhmos_bg.h2cm[i] = 0;
|
||||
APP_LOG(TS_OFF, VLEVEL_L, "|%3d ", fhmos_bg.h2cm[i]);
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "|%3d ", fhmos_bg.h2cm[i]);
|
||||
}
|
||||
|
||||
}
|
||||
APP_LOG(TS_OFF, VLEVEL_L, "\r\n Thresholds: Headlevel=%4d cm, Gesture Mask off=%d cm, Min body height=%d cm\r\n",
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\n Thresholds: Headlevel=%4d cm, Gesture Mask off=%d cm, Min body height=%d cm\r\n",
|
||||
(uint8_t)fhmos_cfg.th_head_level_height_cm, (uint8_t)fhmos_cfg.th_gesture_mask_off_height_cm, (uint8_t)(fhmos_cfg.th_fall_body_min_height_cm));
|
||||
#if 0
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\n\n ------- Mask off matrix before remove edge \r\n");
|
||||
|
||||
APP_LOG(TS_OFF, VLEVEL_L, "\r\n\n ------- Mask off matrix before remove edge \r\n");
|
||||
|
||||
for (i=0; i<64; i++)
|
||||
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_M, " |\r\n");
|
||||
}
|
||||
|
||||
#if defined(NARROW_CUBICLE)
|
||||
for (i=0; i<RIO_EDGE_SIZE; i++)
|
||||
{
|
||||
fhmos_bg.maskoff[rio_edge[i]] = 1;
|
||||
}
|
||||
APP_LOG(TS_OFF, VLEVEL_M, " %d ", (uint8_t)fhmos_bg.maskoff[i]);
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
sts_maskoff_update();
|
||||
|
||||
APP_LOG(TS_OFF, VLEVEL_L, "\r\n\n ------- After Remove Edge \r\n");
|
||||
|
||||
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]);
|
||||
//sts_mask_bitmap[(uint8_t)(i/8)] |= (fhmos_bg.maskoff[(7-i%8)*8+i/8])<<(7-i%8); // 2025-JAN-03 update
|
||||
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_L, "\r\n");
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
|
||||
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\nmask bitmap \r\n");
|
||||
|
||||
for (i=0; i<8; i++)
|
||||
APP_LOG(TS_OFF, VLEVEL_L, "%02X\r\n",sts_mask_bitmap[i]);
|
||||
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "%02X\r\n",sts_mask_bitmap[i]);
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
|
||||
|
||||
#endif
|
||||
|
||||
sts_lamp_bar_color = prev_sts_lamp_bar_color;
|
||||
|
||||
}
|
||||
void sts_maskoff_update(void)
|
||||
{
|
||||
|
||||
#if defined(NARROW_CUBICLE)
|
||||
for (uint8_t i=0; i<RIO_EDGE_SIZE; i++)
|
||||
{
|
||||
fhmos_bg.maskoff[rio_edge[i]] = 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
void sts_generate_fall_gesture_map(void)
|
||||
{
|
||||
uint8_t i=0,head_xy=0, h4=0x0, l4=0x0;
|
||||
|
|
@ -379,10 +411,8 @@ void sts_generate_fall_gesture_map(void)
|
|||
bg_distance[i] = 0;
|
||||
idx[i] = 0;
|
||||
}
|
||||
for (i=0; i<RIO_EDGE_SIZE; i++)
|
||||
fhmos_gesture.maskoff[i] = fhmos_bg.maskoff[rio_edge[i]]; // update mask off bitmap 2025 05 27
|
||||
|
||||
fhmos_gesture.head_level =2000; // 1000 mm
|
||||
fhmos_gesture.head_level =10; // 1000 mm
|
||||
fhmos_gesture.head_xy = 28; //center of FOV
|
||||
|
||||
for (i=0;i<8;i++)
|
||||
|
|
@ -403,7 +433,7 @@ void sts_generate_fall_gesture_map(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
HAL_Delay(10);
|
||||
//HAL_Delay(10);
|
||||
}
|
||||
|
||||
for (i=0; i< 64; i++)
|
||||
|
|
@ -417,6 +447,7 @@ void sts_generate_fall_gesture_map(void)
|
|||
for (uint8_t i = 0; i < 64; i++)
|
||||
{
|
||||
//if ((Result.ZoneResult[i].NumberOfTargets > 0) && (fhmos_gesture.maskoff[i]==0))
|
||||
if (fhmos_bg.maskoff[((7-i%8)*8+i/8)]==0)
|
||||
{
|
||||
// range_distance = (uint32_t)Result.ZoneResult[i].Distance[0];
|
||||
range_distance = (uint32_t)bg_distance[i];
|
||||
|
|
@ -425,11 +456,25 @@ 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.h2cm[i] = (uint8_t)abs(sts_sensor_install_height - range_distance)/10;
|
||||
//APP_LOG(TS_OFF, VLEVEL_M, "\r\nGesture h_2cm:%4d maskoff height_cm: %4d \r\n", fhmos_gesture.h2cm[i], fhmos_cfg.th_gesture_mask_off_height_cm);
|
||||
if (fhmos_gesture.h2cm[i] > fhmos_cfg.th_gesture_mask_off_height_cm)
|
||||
{
|
||||
fhmos_gesture.maskoff[i] = 1;
|
||||
if (fhmos_gesture.h2cm[i] >= fhmos_gesture.head_level)
|
||||
{
|
||||
fhmos_gesture.head_level = fhmos_gesture.h2cm[i];
|
||||
fhmos_gesture.head_xy = i;
|
||||
}
|
||||
|
||||
} else {
|
||||
fhmos_gesture.maskoff[i] = 0;
|
||||
}
|
||||
//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)((7-i/8)*8+i%8)] |= (fhmos_gesture.maskoff[i])<<(i%8); // 2025-05-27 UPDATE
|
||||
//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)((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]);
|
||||
|
|
@ -437,24 +482,24 @@ void sts_generate_fall_gesture_map(void)
|
|||
//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]);
|
||||
if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "|%4d :%2d", fhmos_gesture.h2cm[i], fhmos_gesture.maskoff[i]);
|
||||
}
|
||||
|
||||
#if 0
|
||||
for (i = 0; i < 64; i++)
|
||||
{
|
||||
if (i%8==0) APP_LOG(TS_OFF, VLEVEL_L, "\r\n");
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "|%d ", fhmos_gesture.maskoff[i]);
|
||||
}
|
||||
|
||||
#endif
|
||||
for (i=0; i<8; i++)
|
||||
APP_LOG(TS_OFF, VLEVEL_L, "%02x ",fhmos_gesture_bitmap[i]);
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "%02x ",fhmos_gesture_bitmap[i]);
|
||||
|
||||
APP_LOG(TS_OFF, VLEVEL_L, "\r\n");
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
|
||||
for (i = 0; i < 32; i++)
|
||||
{
|
||||
|
||||
if (i%4 == 0) APP_LOG(TS_OFF, VLEVEL_L, "\r\n");
|
||||
if (i%4 == 0) APP_LOG(TS_OFF, VLEVEL_M, "\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 = 0xff&(fhmos_gesture.h2cm[2*i+0]);
|
||||
|
|
@ -744,18 +789,20 @@ 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<RIO_EDGE_SIZE; i++)
|
||||
fhmos_bg.maskoff[rio_edge[i]] = 1;
|
||||
#endif
|
||||
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\n Head Distance from ceiling= %4d mm \r\n", head_distance_from_ceiling_mm);
|
||||
*/
|
||||
//APP_LOG(TS_OFF, VLEVEL_M, "\r\n Head Distance from ceiling= %4d mm \r\n", head_distance_from_ceiling_mm);
|
||||
//*/
|
||||
for (i=0; i<64; i++)
|
||||
{
|
||||
if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "| %4d_%2d %2d", Result->ZoneResult[i].Distance[0], Result->ZoneResult[i].NumberOfTargets, fhmos_bg.maskoff[i]);
|
||||
if (0 == fhmos_bg.maskoff[i]) // only within the non-mask-off blocks
|
||||
//if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
|
||||
//APP_LOG(TS_OFF, VLEVEL_M, "| %4d_%2d %2d", Result->ZoneResult[i].Distance[0], Result->ZoneResult[i].NumberOfTargets, fhmos_bg.maskoff[(7-i%8)*8+i/8]);
|
||||
//if (0 == fhmos_bg.maskoff[i]) // only within the non-mask-off blocks
|
||||
if (0 == fhmos_bg.maskoff[(7-i%8)*8+i/8])
|
||||
{ // Result.ZoneResult[i].NumberOfTargets > 0)
|
||||
if ((Result->ZoneResult[i].Distance[0] < head_distance_from_ceiling_mm)
|
||||
&& (Result->ZoneResult[i].NumberOfTargets > 0))
|
||||
|
|
@ -765,9 +812,9 @@ static void print_result(RANGING_SENSOR_Result_t *Result)
|
|||
}
|
||||
|
||||
//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);
|
||||
//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_mm = MIN(1400, abs(sts_sensor_install_height - head_distance_from_ceiling_mm));
|
||||
head_height_level_from_floor_mm = MIN(1400, abs(sts_sensor_install_height - head_distance_from_ceiling_mm));
|
||||
|
||||
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);
|
||||
|
||||
|
|
@ -777,8 +824,8 @@ static void print_result(RANGING_SENSOR_Result_t *Result)
|
|||
&& ((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 )
|
||||
//to_confirm ++;
|
||||
if (to_confirm++ >= 3 )
|
||||
{
|
||||
sts_head_level_low = 1;
|
||||
// to_confirm = 0;
|
||||
|
|
|
|||
Binary file not shown.
Loading…
Reference in New Issue