sundp
/
WLE5CC_NODE_STS
1
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

wip...good start

This commit is contained in:
Yunhorn 2025-05-27 19:42:54 +08:00
parent ed9899355f
commit b725c385a3
6 changed files with 120 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
LoRaWAN/App/lora_app.c
View File

@ -664,7 +664,8 @@ void STS_Sensor_Init(void)
/* VVVVVV migrated to yunhorn_sts_process.c */ /* VVVVVV migrated to yunhorn_sts_process.c */
APP_LOG(TS_OFF, VLEVEL_M, "\r\n ------- L8 Init\r\n"); APP_LOG(TS_OFF, VLEVEL_M, "\r\n ------- L8 Init\r\n");
STS_TOF_L8_Init(); MX_TOF_Init();
//STS_TOF_L8_Init();
} }
void STS_Sensor_Prepare(void) void STS_Sensor_Prepare(void)
@ -677,15 +678,17 @@ void STS_Sensor_Prepare(void)
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{ {
static uint8_t prev_sts_hall1_read=0;
switch (GPIO_Pin) switch (GPIO_Pin)
{ {
case BUT1_Pin: case BUT1_Pin:
/* Note: when "EventType == TX_ON_TIMER" this GPIO is not initialized */ /* Note: when "EventType == TX_ON_TIMER" this GPIO is not initialized */
HAL_Delay(100); HAL_Delay(100);
__HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin); //__HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin);
//__HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin); //__HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin);
sts_hall1_read = HALL1_STATE; sts_hall1_read = HALL1_STATE;
APP_LOG(TS_OFF, VLEVEL_M,"\r\n EXIT_CALLBACK, Door state =%d \r\n", sts_hall1_read);
if (sts_hall1_read == STS_Status_Door_Close) if (sts_hall1_read == STS_Status_Door_Close)
{ {
sts_lamp_bar_color = STS_OCCUPY_COLOR; sts_lamp_bar_color = STS_OCCUPY_COLOR;
@ -699,6 +702,9 @@ 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); // 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;
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 //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 // 1) record event start/stop time

BIN
STM32CubeIDE/Release/WLE5CC_NODE_STS.bin
View File

Binary file not shown.

BIN
STM32CubeIDE/Release/WLE5CC_NODE_STS.elf
View File

Binary file not shown.

22
STS/Core/Src/yunhorn_sts_process.c
View File

@ -564,7 +564,8 @@ void STS_YunhornSTSEventP5_Process(void)
{ {
APP_LOG(TS_OFF, VLEVEL_M, "\r\n Upload Fall Gesture Type 2 , status=%d \r\n", sts_fhmos_bitmap_pending); APP_LOG(TS_OFF, VLEVEL_M, "\r\n Upload Fall Gesture Type 2 , status=%d \r\n", sts_fhmos_bitmap_pending);
STS_FHMOS_sensor_upload_map(0x02); STS_FHMOS_sensor_upload_map(0x02);
HAL_Delay(2000); } else if (sts_fhmos_bitmap_pending == FHMOS_BITMAP_2D_FINISHED) {
APP_LOG(TS_OFF, VLEVEL_M, "\r\n Upload Fall Gesture Type 4 , status=%d \r\n", sts_fhmos_bitmap_pending); APP_LOG(TS_OFF, VLEVEL_M, "\r\n Upload Fall Gesture Type 4 , status=%d \r\n", sts_fhmos_bitmap_pending);
STS_FHMOS_sensor_upload_map(0x04); STS_FHMOS_sensor_upload_map(0x04);
} }
@ -1944,7 +1945,8 @@ void OnRestoreSTSCFGContextProcess(void)
#endif #endif
#ifdef L8 #ifdef L8
STS_FHMOS_sensor_config_update(); STS_FHMOS_sensor_config_init();
//STS_FHMOS_sensor_config_update();
#endif #endif
} }
@ -1965,9 +1967,9 @@ void STS_FHMOS_sensor_config_init(void)
fhmos_cfg.th_gesture_mask_off_height_cm = 20; // gesture mask off height in cm fhmos_cfg.th_gesture_mask_off_height_cm = 20; // gesture mask off height in cm
fhmos_cfg.th_fall_body_min_height_cm = 25; // fall body min height from floor 25 cm 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_high_threshold = sts_cfg_nvm.sensor_install_height_in_10cm*100;
sts_low_threshold = sts_high_threshold - 1400; //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); //APP_LOG(TS_OFF, VLEVEL_M, "\r\n FHMOS cfg high= %4d low= %4d \r\n", sts_high_threshold, sts_low_threshold);
} }
void STS_FHMOS_sensor_config_update() void STS_FHMOS_sensor_config_update()
@ -1982,6 +1984,10 @@ void STS_FHMOS_sensor_config_update()
fhmos_cfg.th_gesture_mask_off_height_cm = sts_cfg_nvm.fhmos_cfg_7; // 20 cm 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 fhmos_cfg.th_fall_body_min_height_cm = sts_cfg_nvm.fhmos_cfg_8; // 25 cm
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);
} }
void STS_FHMOS_sensor_upload_map(uint8_t map_index) void STS_FHMOS_sensor_upload_map(uint8_t map_index)
@ -2055,7 +2061,11 @@ void STS_FHMOS_sensor_upload_map(uint8_t map_index)
int status=STS_SENSOR_Upload_Message(YUNHORN_STS_L8_LORA_APP_DATA_PORT, i, (uint8_t *)tstbuf); int status=STS_SENSOR_Upload_Message(YUNHORN_STS_L8_LORA_APP_DATA_PORT, i, (uint8_t *)tstbuf);
if ((map_index ==0x02)&& (status ==0)) if ((map_index ==0x02)&& (status ==0))
{ {
sts_fhmos_bitmap_pending = FHMOS_BITMAP_FINISHED; // 2; sts_fhmos_bitmap_pending = FHMOS_BITMAP_2D_FINISHED; // 2;
} else if ((map_index ==0x04)&& (status ==0))
{
sts_fhmos_bitmap_pending = FHMOS_BITMAP_FINISHED; // 4;
} }
APP_LOG(TS_OFF, VLEVEL_M, "\r\n +++++++++++++++++++++++ \r\nBitmap Pending uploaded, fhmos_bitmap_statue=%d\r\n", sts_fhmos_bitmap_pending); APP_LOG(TS_OFF, VLEVEL_M, "\r\n +++++++++++++++++++++++ \r\nBitmap Pending uploaded, fhmos_bitmap_statue=%d\r\n", sts_fhmos_bitmap_pending);
} }

133
STS/TOF/App/app_tof.c
View File

@ -57,8 +57,22 @@ extern volatile uint8_t sts_fhmos_bitmap_pending;
#include "stm32wlxx_nucleo.h" #include "stm32wlxx_nucleo.h"
static int to_confirm = 0; static int to_confirm = 0;
static uint32_t STS_Get_Center_Range_Distance(RANGING_SENSOR_Result_t *Result); 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;
#if defined(NARROW_CUBICLE) #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}; //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, //first line
8,14,15,
16,22,23,
24,30,31,
32,38,39,
40,46,47,
48,54,55,
56,57,58,59,60,61,62,63}; // last line
#endif #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}; //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 -----------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/
@ -189,9 +203,6 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
uint8_t i=0; uint8_t i=0;
uint32_t range_distance =0; uint32_t range_distance =0;
uint8_t j=0; uint8_t j=0;
uint16_t bg_distance[64]={0};
uint8_t idx[64]={0};
uint8_t IDX_LEN=10;
uint8_t prev_sts_lamp_bar_color = sts_lamp_bar_color; uint8_t prev_sts_lamp_bar_color = sts_lamp_bar_color;
sts_lamp_bar_color = STS_BLUE; sts_lamp_bar_color = STS_BLUE;
@ -200,6 +211,8 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
{ {
fhmos_bg.h2cm[i] = 0; fhmos_bg.h2cm[i] = 0;
fhmos_bg.maskoff[i] = 0; fhmos_bg.maskoff[i] = 0;
bg_distance[i] = 0;
idx[i] = 0;
} }
for (i=0;i<8;i++) for (i=0;i<8;i++)
sts_mask_bitmap[i] =0x0; sts_mask_bitmap[i] =0x0;
@ -242,13 +255,13 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
for (i = 0; i < 64; i++) for (i = 0; i < 64; i++)
{ {
if (i % 8 ==0) { if (i % 8 ==0) {
APP_LOG(TS_OFF, VLEVEL_H, "\r\n[cm] "); APP_LOG(TS_OFF, VLEVEL_M, "\r\n[cm] ");
} }
//if ((Result.ZoneResult[i].NumberOfTargets > 0)) //if ((Result.ZoneResult[i].NumberOfTargets > 0))
//{ //{
bg_distance[i] += (uint16_t) Result.ZoneResult[i].Distance[0]; bg_distance[i] += (uint16_t) Result.ZoneResult[i].Distance[0];
idx[i] ++; idx[i] ++;
APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", Result.ZoneResult[i].Distance[0]/10); APP_LOG(TS_OFF, VLEVEL_M, "|%3d ", Result.ZoneResult[i].Distance[0]/10);
//} //}
//else APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", 0); //else APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", 0);
@ -259,12 +272,12 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
HAL_Delay(10); HAL_Delay(10);
} }
APP_LOG(TS_OFF, VLEVEL_H, "\r\n\n\nAverage Distance==\r\n"); APP_LOG(TS_OFF, VLEVEL_M, "\r\n\n\n=========Average Distance=========\r\n\n");
for (i=0; i< 64; i++) for (i=0; i< 64; i++)
{ {
if (i%8==0) APP_LOG(TS_OFF, VLEVEL_H, "\r\n %2d ", i); if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\r\n%2d| ",i );
bg_distance[i] /= idx[i]; bg_distance[i] /= idx[i];
APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", bg_distance[i]/10); APP_LOG(TS_OFF, VLEVEL_M, "|%3d ", bg_distance[i]/10);
} }
//STS_TOF_L8_Process(); //STS_TOF_L8_Process();
@ -295,10 +308,10 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
if ((uint16_t)fhmos_bg.h2cm[i] < (uint16_t)(fhmos_cfg.th_gesture_mask_off_height_cm)) if ((uint16_t)fhmos_bg.h2cm[i] < (uint16_t)(fhmos_cfg.th_gesture_mask_off_height_cm))
{ {
fhmos_bg.maskoff[i] = 0; fhmos_bg.maskoff[(7-i%8)*8+i/8] = 0; // update 2025 05 27
} else } else
{ {
fhmos_bg.maskoff[i] = 1; fhmos_bg.maskoff[(7-i%8)*8+i/8] = 1;
} }
//sts_mask_bitmap[(uint8_t)(i/8)] |= (fhmos_bg.maskoff[i])<<(7-i%8); //sts_mask_bitmap[(uint8_t)(i/8)] |= (fhmos_bg.maskoff[i])<<(7-i%8);
@ -321,12 +334,12 @@ void STS_LMZ_Ambient_Height_Scan_Process(void)
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 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"); 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]); APP_LOG(TS_OFF, VLEVEL_L, " %d ", (uint8_t)fhmos_bg.maskoff[i]);
} }
#if defined(NARROW_CUBICLE) #if defined(NARROW_CUBICLE)
for (i=0; i<34; i++) for (i=0; i<RIO_EDGE_SIZE; i++)
{ {
fhmos_bg.maskoff[rio_edge[i]] = 1; fhmos_bg.maskoff[rio_edge[i]] = 1;
} }
@ -362,18 +375,51 @@ void sts_generate_fall_gesture_map(void)
{ {
fhmos_gesture.h2cm[i] = 0; fhmos_gesture.h2cm[i] = 0;
fhmos_gesture.maskoff[i] = 0; fhmos_gesture.maskoff[i] = 0;
//fhmos_gesture.maskoff[i] = fhmos_bg.maskoff[rio_edge[i]]; // update mask off bitmap 2025 05 27
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 =2000; // 1000 mm
fhmos_gesture.head_xy = 28; //center of FOV fhmos_gesture.head_xy = 28; //center of FOV
for (i=0;i<8;i++) for (i=0;i<8;i++)
fhmos_gesture_bitmap[i]=0x0; fhmos_gesture_bitmap[i]=0x0;
for (uint8_t j=0; j < IDX_LEN; j++)
{
MX_53L8A1_ThresholdDetection_Process();
{
for (i = 0; i < 64; i++)
{
if (i % 8 ==0) APP_LOG(TS_OFF, VLEVEL_H, "\r\n[cm] ");
if ((Result.ZoneResult[i].NumberOfTargets > 0))
{
bg_distance[i] += (uint16_t) Result.ZoneResult[i].Distance[0];
idx[i] ++;
APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", Result.ZoneResult[i].Distance[0]/10);
}
}
}
HAL_Delay(10);
}
for (i=0; i< 64; i++)
{
if (i%8==0) APP_LOG(TS_OFF, VLEVEL_H, "\r\n %2d ", i);
bg_distance[i] /= idx[i];
APP_LOG(TS_OFF, VLEVEL_H, "|%3d ", bg_distance[i]/10);
}
for (uint8_t i = 0; i < 64; i++) for (uint8_t i = 0; i < 64; i++)
{ {
if ((Result.ZoneResult[i].NumberOfTargets > 0) && (fhmos_bg.maskoff[i]==0)) //if ((Result.ZoneResult[i].NumberOfTargets > 0) && (fhmos_gesture.maskoff[i]==0))
{ {
range_distance = (uint32_t)Result.ZoneResult[i].Distance[0]; // range_distance = (uint32_t)Result.ZoneResult[i].Distance[0];
range_distance = (uint32_t)bg_distance[i];
if (range_distance < sts_fall_head_position) if (range_distance < sts_fall_head_position)
{ {
sts_fall_head_position = range_distance; // simply find out the head level sts_fall_head_position = range_distance; // simply find out the head level
@ -382,14 +428,15 @@ void sts_generate_fall_gesture_map(void)
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)/20;
//fhmos_gesture.maskoff[i] = ((2*fhmos_gesture.h2cm[i])< fhmos_cfg.th_gesture_mask_off_height_cm)? 0:1; //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.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)(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 // debug
// if (i%8==0) printf("\r\n"); // if (i%8==0) printf("\r\n");
// printf("|%4ld %4d ", range_distance, fhmos_gesture.h2cm[i]); // printf("|%4ld %4d ", range_distance, fhmos_gesture.h2cm[i]);
} }
else { //else {
fhmos_gesture.h2cm[i] = 0; //fhmos_gesture.h2cm[i] = 0;
} //}
if (i%8==0) APP_LOG(TS_OFF, VLEVEL_L, "\r\n"); if (i%8==0) APP_LOG(TS_OFF, VLEVEL_L, "\r\n");
APP_LOG(TS_OFF, VLEVEL_L, "|%4d ", fhmos_gesture.h2cm[i]); APP_LOG(TS_OFF, VLEVEL_L, "|%4d ", fhmos_gesture.h2cm[i]);
} }
@ -397,7 +444,7 @@ void sts_generate_fall_gesture_map(void)
for (i = 0; i < 64; i++) for (i = 0; i < 64; i++)
{ {
if (i%8==0) APP_LOG(TS_OFF, VLEVEL_L, "\r\n"); if (i%8==0) APP_LOG(TS_OFF, VLEVEL_L, "\r\n");
APP_LOG(TS_OFF, VLEVEL_L, "|%d ", fhmos_gesture.maskoff[i]); APP_LOG(TS_OFF, VLEVEL_M, "|%d ", fhmos_gesture.maskoff[i]);
} }
for (i=0; i<8; i++) for (i=0; i<8; i++)
@ -410,10 +457,11 @@ void sts_generate_fall_gesture_map(void)
if (i%4 == 0) APP_LOG(TS_OFF, VLEVEL_L, "\r\n"); if (i%4 == 0) APP_LOG(TS_OFF, VLEVEL_L, "\r\n");
//h4 = MIN(fhmos_gesture.h2cm[2*i+0],sts_gesture_mask_cap_height_2cm)/8; //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; //l4 = MIN(fhmos_gesture.h2cm[2*i+1],sts_gesture_mask_cap_height_2cm)/8;
h4 = fhmos_gesture.h2cm[2*i+0]; h4 = 0xff&(fhmos_gesture.h2cm[2*i+0]);
l4 = fhmos_gesture.h2cm[2*i+1]; l4 = 0xff&(fhmos_gesture.h2cm[2*i+1]);
fhmos_gesture.cube[i] = ((h4&0x0f)<<4)|(l4&0x0f); fhmos_gesture.cube[i] = ((h4&0x0f)<<4)|(l4&0x0f);
APP_LOG(TS_OFF, VLEVEL_L, " [%2d_%2d]", (fhmos_gesture.cube[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_level = 0xff&((sts_sensor_install_height - sts_fall_head_position)/10); // head level from floor in CM
@ -502,18 +550,18 @@ void STS_TOF_L8_Init(void)
{ {
MX_53L8A1_ThresholdDetection_Init(); MX_53L8A1_ThresholdDetection_Init();
MX_53L8A1_ThresholdDetection_Process(); MX_53L8A1_ThresholdDetection_Process();
} }
void STS_TOF_L8_Process(void) void STS_TOF_L8_Process(void)
{ {
//status = VL53L8A1_RANGING_SENSOR_Start(VL53L8A1_DEV_CENTER, RS_MODE_ASYNC_CONTINUOUS);
//while (1) //while (1)
{ {
/* interrupt mode */ /* interrupt mode */
if (ToF_EventDetected != 0) if (ToF_EventDetected != 0)
//while(ToF_EventDetected==0){};
{ {
ToF_EventDetected = 0; ToF_EventDetected = 0;
#if 1 #if 1
@ -614,7 +662,7 @@ static void MX_53L8A1_ThresholdDetection_Process(void)
//ITConfig.HighThreshold = HIGH_THRESHOLD; /* mm */ //ITConfig.HighThreshold = HIGH_THRESHOLD; /* mm */
sts_high_threshold = sts_sensor_install_height+ 400; sts_high_threshold = sts_sensor_install_height+ 400;
sts_low_threshold = sts_high_threshold - 1400; sts_low_threshold = sts_high_threshold - 1550;
APP_LOG(TS_OFF, VLEVEL_H, "\r\n Threshold High=%4d, Low=%4d \r\n", sts_high_threshold, sts_low_threshold); APP_LOG(TS_OFF, VLEVEL_H, "\r\n Threshold High=%4d, Low=%4d \r\n", sts_high_threshold, sts_low_threshold);
ITConfig.LowThreshold = sts_low_threshold; ITConfig.LowThreshold = sts_low_threshold;
@ -685,11 +733,11 @@ static void print_result(RANGING_SENSOR_Result_t *Result)
{ {
int8_t i; int8_t i;
uint16_t head_distance_from_ceiling=8000; uint16_t head_distance_from_ceiling_mm=8000;
//printf("%c[2H", 27); /* clear screen */ //printf("%c[2H", 27); /* clear screen */
uint16_t factor1_floor_level_from_ceiling = sts_sensor_install_height; // 50mm min body height uint16_t factor1_floor_level_from_ceiling_mm = sts_sensor_install_height; // 50mm min body height
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 factor2_head_level_from_floor_mm = (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; uint16_t head_height_level_from_floor_mm=0;
sts_head_level_low = 0; sts_head_level_low = 0;
@ -697,28 +745,37 @@ static void print_result(RANGING_SENSOR_Result_t *Result)
//APP_LOG(TS_OFF, VLEVEL_M, "\r\n Eliminate edge \r\n"); //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}; //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) #if defined(NARROW_CUBICLE)
for (i=0; i<36; i++) for (i=0; i<RIO_EDGE_SIZE; i++)
fhmos_bg.maskoff[rio_edge[i]] = 1; fhmos_bg.maskoff[rio_edge[i]] = 1;
#endif #endif
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++) 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 (0 == fhmos_bg.maskoff[i]) // only within the non-mask-off blocks
{ // Result.ZoneResult[i].NumberOfTargets > 0) { // Result.ZoneResult[i].NumberOfTargets > 0)
if ((Result->ZoneResult[i].Distance[0] < head_distance_from_ceiling)&& (Result->ZoneResult[i].NumberOfTargets > 0)) if ((Result->ZoneResult[i].Distance[0] < head_distance_from_ceiling_mm)
head_distance_from_ceiling = Result->ZoneResult[i].Distance[0]; && (Result->ZoneResult[i].NumberOfTargets > 0))
head_distance_from_ceiling_mm = Result->ZoneResult[i].Distance[0];
// find out the min_distance or the highest position level // find out the min_distance or the highest position level
} }
} }
//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);
head_height_level_from_floor = MIN(1400, abs(sts_sensor_install_height - head_distance_from_ceiling)); //head_height_level_from_floor_mm = MIN(1400, abs(sts_sensor_install_height - head_distance_from_ceiling_mm));
APP_LOG(TS_OFF, VLEVEL_L, "\r\n Updown distance=%d cm, Floor Up head =%d cm\r\n", head_distance_from_ceiling/10, head_height_level_from_floor/10); 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);
/* state tree */ /* state tree */
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 ))) if ((head_distance_from_ceiling_mm <= factor1_floor_level_from_ceiling_mm)
&& ((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 ++; to_confirm ++;
if (to_confirm >=3 ) if (to_confirm >=3 )
@ -727,7 +784,7 @@ static void print_result(RANGING_SENSOR_Result_t *Result)
// to_confirm = 0; // to_confirm = 0;
} }
} else if ((head_distance_from_ceiling < (factor2_head_level_from_floor - 150))) // TODO XXX 50mm gap to avoid flapping back and forth } else if ((head_distance_from_ceiling_mm < (factor2_head_level_from_floor_mm - 150))) // TODO XXX 50mm gap to avoid flapping back and forth
{ {
sts_head_level_low = 0; sts_head_level_low = 0;
sts_fhmos_bitmap_pending = FHMOS_BITMAP_BLANK; sts_fhmos_bitmap_pending = FHMOS_BITMAP_BLANK;

1
STS/TOF/App/app_tof.h
View File

@ -58,6 +58,7 @@ typedef enum {
typedef enum { typedef enum {
FHMOS_BITMAP_BLANK=0, FHMOS_BITMAP_BLANK=0,
FHMOS_BITMAP_GENERATED, FHMOS_BITMAP_GENERATED,
FHMOS_BITMAP_2D_FINISHED,
FHMOS_BITMAP_FINISHED FHMOS_BITMAP_FINISHED
} fhmos_bitmap_state; } fhmos_bitmap_state;