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

wip, not good

This commit is contained in:
Yunhorn 2024-11-02 21:05:55 +08:00
parent 13d880a049
commit 5aed732764
5 changed files with 80 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
STM32CubeIDE/Release/WLE5CC_NODE_STS.elf
View File

Binary file not shown.

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

@ -1041,6 +1041,7 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s
//OnSystemReset();
break;
case 'S': /* "YZS": Self Function Testing */
STS_SENSOR_Function_Test_Process();
break;
@ -1067,14 +1068,16 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s
outbuf[i++] = (uint8_t)sts_version;
outbuf[i++] = (uint8_t)sts_hardware_ver;
outbuf[i++] = (uint8_t)(99*((GetBatteryLevel()/254)&0xff));
outbuf[i++] = (uint8_t)(4); //four bytes for distance
outbuf[i++] = (uint8_t)(2); //two bytes for distance
#if defined(VL53LX)||defined(VL53L0)
#if 0
outbuf[i++] = (uint8_t)(sts_sensor_install_height/1000+0x30)&0xff;
outbuf[i++] = (uint8_t)((sts_sensor_install_height/100)%10+0x30)&0xff;
outbuf[i++] = (uint8_t)((sts_sensor_install_height/10)%10+0x30)&0xff;
outbuf[i++] = (uint8_t)((sts_sensor_install_height/1)%10+0x30)&0xff;
//outbuf[i++] = (uint8_t)(sts_sensor_install_height>>8)&0xFF;
//outbuf[i++] = (uint8_t)(sts_sensor_install_height)&0xFF;
#endif
outbuf[i++] = (uint8_t)(sts_sensor_install_height>>8)&0xFF;
outbuf[i++] = (uint8_t)(sts_sensor_install_height)&0xFF;
#endif
STS_SENSOR_Upload_Message(YUNHORN_STS_USER_APP_CTRL_REPLY_PORT, i, (uint8_t *)outbuf);
@ -1732,8 +1735,12 @@ void STS_SENSOR_Distance_Test_Process(void)
#if defined(VL53LX)||defined(L8)
//MX_TOF_Init();
//MX_TOF_Process();
sts_sensor_install_height = (uint16_t)MX_TOF_Ranging_Process();
int status=0;
uint16_t range_distance =0;
status = sts_tof_vl53lx_range_distance(&range_distance);
sts_sensor_install_height = range_distance;
APP_LOG(TS_OFF, VLEVEL_M, "\n STS SENSOR INSTALLATION HEIGHT =%d mm\n\r", (uint16_t)sts_sensor_install_height);
#endif
#if defined(VL53L0)
STS_TOF_VL53L0X_Range_Process();
@ -1762,14 +1769,20 @@ void STS_SENSOR_Function_Test_Process(void)
tstbuf[i++] = (uint8_t) (99*GetBatteryLevel()/254)&0xff;
#if (defined(STS_P2)||defined(STS_T6))
STS_SENSOR_MEMS_Get_ID(&sensor_id);
//STS_SENSOR_MEMS_Get_ID(&sensor_id);
APP_LOG(TS_OFF, VLEVEL_M, "\r\nSensor_id=%04x\r\n", sensor_id);
if (((sensor_id & 0xff)!= 0xCC) && (((sensor_id >>8) & 0xFF)!=0xEA)) // no VL53L1X found
{
tstbuf[i++] = (uint8_t) 'X'; // Slave MEMS Not Avaliable
} else
{
STS_SENSOR_Distance_Test_Process();
//STS_SENSOR_Distance_Test_Process();
int status=0;
uint16_t range_distance =0;
status = sts_tof_vl53lx_range_distance(&range_distance);
sts_sensor_install_height = range_distance;
APP_LOG(TS_OFF, VLEVEL_M, "\nSensor Install Height =%4d mm\n", sts_sensor_install_height);
tstbuf[i++] = (uint8_t) (2)&0xff; //length of following data
tstbuf[i++] = (uint8_t) (sts_sensor_install_height>>8)&0xff; // MSB of sensor height
@ -1785,12 +1798,16 @@ void STS_SENSOR_Function_Test_Process(void)
if (sts_sensor_install_height >= 2000)
sts_door_jam_profile ++; //DOOR_JAM_2400
if (sts_sensor_install_height >= 2400)
sts_door_jam_profile ++; //DOOR_JAM_3000
if (sts_sensor_install_height >= 3000)
sts_door_jam_profile ++; //DOOR_JAM_3500
if (sts_sensor_install_height >= 3500)
sts_door_jam_profile ++; //DOOR_JAM_4000
APP_LOG(TS_OFF, VLEVEL_M, "\r\nDOOR JAM PROFILE=%d \r\n", sts_door_jam_profile);
}
#endif
#ifdef YUNHORN_STS_O6_ENABLED
@ -1817,8 +1834,8 @@ void STS_SENSOR_Function_Test_Process(void)
#endif
//MX_TOF_Process();
#ifdef STS_P2
MX_TOF_Init();
MX_TOF_Ranging_Process();
//MX_TOF_Init();
//MX_TOF_Ranging_Process();
#endif
#ifdef VL53L0
PME_ON;

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

@ -134,7 +134,7 @@ uint16_t MX_TOF_Ranging_Process(void)
#if (defined(STS_P2)||defined(STS_T6))
uint16_t range_distance=0;
uint8_t range_mode = 2; //STS_TOF_LONG_RANGE;
uint8_t range_mode = STS_TOF_LONG_RANGE; //STS_TOF_LONG_RANGE;
STS_TOF_VL53LX_Range_Process(range_mode, &range_distance);
APP_LOG(TS_OFF, VLEVEL_M, "\n VL53L1 Range distance =%u mm \n\r", (uint16_t)range_distance);
@ -483,10 +483,13 @@ void BSP_PB_Callback(Button_TypeDef Button)
uint8_t sts_vl53lx_ranging(uint16_t *ranged_distance, uint8_t range_mode, uint16_t distance_threshold_mm, uint16_t inter_measurement_ms, uint16_t macro_timing,
uint16_t roi_width, uint16_t sigma_mm, uint16_t signal_kcps)
{
uint8_t status=0, dev=0x52;
uint16_t estimated_distance_mm=0;
#if 0
uint8_t status=0, dev=0x52;
uint16_t estimated_distance_mm=0;
status = VL53L1X_SensorInit(dev);
status += VL53L1X_SetDistanceMode(dev, 2); /* 1=short, 2=long, DISTANCE_MODE */
status += VL53L1X_SetDistanceMode(dev, range_mode); /* 1=short, 2=long, DISTANCE_MODE */
status += VL53L1X_SetTimingBudgetInMs(dev, 100); /* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */
status += VL53L1X_SetInterMeasurementInMs(dev, 50);
status += VL53L1X_SetROI(dev, 8, 8); /* minimum ROI 4,4 */
@ -499,8 +502,9 @@ uint16_t roi_width, uint16_t sigma_mm, uint16_t signal_kcps)
APP_LOG(TS_OFF, VLEVEL_L,"Start Range failed\n");
return (-1);
}
#endif
status = VL53L1X_GetDistance(dev, &estimated_distance_mm);
status = VL53L1X_ClearInterrupt(dev); /* clear interrupt has to be called to enable next interrupt*/
APP_LOG(TS_OFF,VLEVEL_M,"Target detected! Distance =%d mm \r\n", estimated_distance_mm );
*ranged_distance = estimated_distance_mm;
status = VL53L1X_StopRanging(dev);

52
STS/TOF/App/app_tof_peoplecount.c
View File

@ -39,7 +39,7 @@ volatile sts_zone_center_by_rows_of_spads_t zone_center[4]={
extern volatile uint8_t sts_door_jam_profile;
volatile uint8_t sts_presence_state=0;
static int PresenceState=0, PrevPresenceState=0;
volatile uint8_t sts_distance_mode = DOOR_JAM_2400; //sts_door_jam_profile; //DOOR_JAM_3000;
volatile uint8_t sts_distance_mode = DOOR_JAM_2400; //DOOR_JAM_2400; //sts_door_jam_profile; //DOOR_JAM_3000;
#endif
#if (defined(STS_P2))
volatile uint8_t sts_people_count_number_changed = 0;
@ -117,12 +117,13 @@ int ProcessPresenceDetectionData(int16_t Distance, uint8_t zone, uint8_t RangeSt
MinDistance = Distances[zone][i];
}
}
APP_LOG(TS_OFF, VLEVEL_M, "\r\nMin Distance =%d ", MinDistance);
if (MinDistance < ppc_cfg[sts_distance_mode].dist_threshold) {
// Someone is in !
CurrentZoneStatus = SOMEONE;
APP_LOG(TS_OFF, VLEVEL_M, "---> Someone is in");
}
APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
//return CurrentZoneStatus;
#if 1
// left zone
@ -275,11 +276,13 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu
MinDistance = Distances[zone][i];
}
}
APP_LOG(TS_OFF, VLEVEL_M, "\r\n MinDistance & dist_threshold ---%d %d", MinDistance,ppc_cfg[sts_distance_mode].dist_threshold);
if (MinDistance < ppc_cfg[sts_distance_mode].dist_threshold) {
// Someone is in !
CurrentZoneStatus = SOMEONE;
APP_LOG(TS_OFF, VLEVEL_M, ">SOMEONE IN");
}
APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
// left zone
if (zone == LEFT) {
@ -352,6 +355,9 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu
APP_LOG(TS_OFF, VLEVEL_M,"Walk In, People Count=%d\n", PeopleCount);
sts_people_count_sensor_data.Walk_In_People_Count ++;
sts_people_count_number_changed = 1;
//LED1_ON; //TODO XXX, CHANGE TO (LED_IN_ON && LED_OUT_OFF);
LED1_TOGGLE;
} else if ((PathTrack[1] == 2) && (PathTrack[2] == 3) && (PathTrack[3] == 1)) {
// This an exit
PeopleCount --;
@ -360,11 +366,14 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu
DistancesTableSize[1] = 0;
APP_LOG(TS_OFF, VLEVEL_M,"Walk Out, People Count=%d\n", PeopleCount);
sts_people_count_sensor_data.Walk_Out_People_Count ++;
//LED1_ON; //TODO XXX, CHANGE TO (LED_IN_OFF && LED_OUT_ON);
LED1_TOGGLE;
sts_people_count_number_changed = 1;
} else {
// reset the table filling size also in case of unexpected path
DistancesTableSize[0] = 0;
DistancesTableSize[1] = 0;
APP_LOG(TS_OFF, VLEVEL_M,"Walk Around ie. Wrong path\n");
sts_people_count_sensor_data.Walk_Around_People_Count ++;
}
@ -394,6 +403,7 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu
#endif
#endif
}
//printf ("Inside PeopleCount = %d , returned\n", PeopleCount);
// output debug data to main host machine
return(PeopleCount);
@ -692,6 +702,10 @@ int sts_tof_vl53lx_peoplecount_init(void)
/* Initialize and configure the device according to people counting need */
status = VL53L1X_SensorInit(dev);
sts_distance_mode = sts_door_jam_profile;
APP_LOG(TS_OFF, VLEVEL_M, "\r\n Door Jam Profile=%d, distance Mode =%d \r\n", sts_door_jam_profile, sts_distance_mode);
APP_LOG(TS_OFF, VLEVEL_M, "\r\n MaxDistance=%d, \nMinDistance=%d \ndist_threshold=%d \n timing_budget=%d \n distance_mode=%d \r\n",
ppc_cfg[sts_distance_mode].max_distance, ppc_cfg[sts_distance_mode].min_distance, ppc_cfg[sts_distance_mode].dist_threshold,ppc_cfg[sts_distance_mode].timing_budget,ppc_cfg[sts_distance_mode].distance_mode);
status += VL53L1X_SetDistanceMode(dev, ppc_cfg[sts_distance_mode].distance_mode); /* 1=short, 2=long, DISTANCE_MODE */
status += VL53L1X_SetTimingBudgetInMs(dev, ppc_cfg[sts_distance_mode].timing_budget); /* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */
status += VL53L1X_SetInterMeasurementInMs(dev, ppc_cfg[sts_distance_mode].timing_budget);
@ -756,6 +770,7 @@ int sts_tof_vl53lx_peoplecount_start(void)
if ((RangeStatus == 0) || (RangeStatus == 4) || (RangeStatus == 7)) {
if (Distance <= ppc_cfg[sts_distance_mode].min_distance) //MIN_DISTANCE) // wraparound case see the explanation at the constants definition place
Distance = ppc_cfg[sts_distance_mode].max_distance + ppc_cfg[sts_distance_mode].min_distance; //MAX_DISTANCE + MIN_DISTANCE;
APP_LOG(TS_OFF, VLEVEL_M, "\r\nRangeStatus=%d Distance=%d \r\n", RangeStatus, Distance);
}
else // severe error cases
Distance = ppc_cfg[sts_distance_mode].max_distance; //MAX_DISTANCE;
@ -764,9 +779,36 @@ int sts_tof_vl53lx_peoplecount_start(void)
//printf("\nPplCounter =%d \n\r", PplCounter);
//sprintf(DisplayStr, "%4d", PplCounter); // only use for special EVK with display
//XNUCLEO53L1A1_SetDisplayString(DisplayStr);
//APP_LOG(TS_OFF, VLEVEL_M,"%d,%d,%d\n", Zone, Distance, Signal);
//APP_LOG(TS_OFF, VLEVEL_M,"Zone, Distance, Signal = %d,%d,%d\n", Zone, Distance, Signal);
Zone++;
Zone = Zone%2;
return 0;
}
int sts_tof_vl53lx_range_distance(uint16_t *range_distance)
{
uint16_t wordData=0, Distance=0;
uint8_t RangeStatus;
uint8_t dataReady=0, status=0, dev=0x52;
/* read and display data */
while (dataReady == 0) {
status = VL53L1X_CheckForDataReady(dev, &dataReady);
HAL_Delay(1);
wordData++;
if (wordData > 30) return -1; // 50 ms timer, so make this 60% to fail back
}
dataReady = 0;
//status += VL53L1X_GetRangeStatus(dev, &RangeStatus);
status += VL53L1X_GetDistance(dev, &Distance);
status += VL53L1X_ClearInterrupt(dev); /* clear interrupt has to be called to enable next interrupt*/
if (status != 0) {
APP_LOG(TS_OFF, VLEVEL_L,"Error in operating the device\n");
return (-1);
}
APP_LOG(TS_OFF, VLEVEL_M,"\r\nVL53L1X RANGE DISTANCE =%d \r\n", Distance);
*range_distance = Distance;
return status;
}

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

@ -172,6 +172,7 @@ int sts_tof_vl53lx_presence_detection_start(void);
int sts_tof_vl53lx_peoplecount_init(void);
int sts_tof_vl53lx_peoplecount_start(void);
int sts_tof_vl53lx_range_distance(uint16_t *range_distance);
//int sts_tof_vl53lx_peoplecount(void);