sundp
/
O7
1
0
Fork 0
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity
O7/TOF/App/app_tof_peoplecount.c

443 lines
18 KiB
C
Raw Permalink Blame History

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "sys_app.h"
/* USER CODE BEGIN Includes */
#include "app_tof_peoplecount.h"
#include "VL53L1X_API.h"
#include "VL53l1X_calibration.h"
#include "X-NUCLEO-53L1A1.h"
#include "app_tof.h"
/* USER CODE END Includes */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* Private variables ---------------------------------------------------------*/
extern I2C_HandleTypeDef hi2c2;
volatile sts_people_count_sensor_data_t sts_people_count_sensor_data={0,0,0,2,'M',0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
volatile sts_ppc_cfg_type_t ppc_cfg[3] = {
{DOOR_JAM_2000, 8, 2000,0,1600,8,33,DISTANCE_MODE_SHORT,175,247},
{DOOR_JAM_2000, 8, 2000,0,1600,8,33,DISTANCE_MODE_SHORT,175,247},
{DOOR_JAM_2400, 10, 2400,0,1600,8,33,DISTANCE_MODE_LONG,175,231},
};
volatile sts_zone_center_by_rows_of_spads_t zone_center[3]={
{4, 151, 247},
{6, 159, 239},
{8, 175, 231},
};
volatile uint8_t sts_distance_mode = DISTANCE_MODE_LONG;
volatile uint8_t sts_people_count_number_changed = 0;
uint16_t dev=0x52;
int status = 0;
volatile int IntCount;
#define isInterrupt 1 /* If isInterrupt = 1 then device working in interrupt mode, else device working in polling mode */
/* USER CODE END PV */
static int PplCounter;
static int center[2] = {FRONT_ZONE_CENTER,BACK_ZONE_CENTER}; //{ppc_cfg[sts_distance_mode].front_zone_center, ppc_cfg[sts_distance_mode].back_zone_center}; /* these are the spad center of the 2 4*16 zones */
static int Zone = 0;
static int PathTrack[] = {0,0,0,0};
static int PathTrackFillingSize = 1; // init this to 1 as we start from state where nobody is any of the zones
static int LeftPreviousStatus = NOBODY;
static int RightPreviousStatus = NOBODY;
static int PeopleCount = 0;
static uint16_t Distances[2][DISTANCES_ARRAY_SIZE];
static uint8_t DistancesTableSize[2] = {0,0};
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE END PFP */
//void sts_tof_vl53lx_peoplecount(void);
//int sts_tof_vl53lx_peoplecount_subprocess(void);
/* USER CODE BEGIN 0 */
#define VL53L1X_INT_Pin (GPIO_PIN_3) // WL55JC GPIO_PIN_10, F401xE ==>(GPIO_PIN_4)
#if 0
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
if (GPIO_Pin==VL53L1X_INT_Pin)
{
IntCount++;
}
}
#endif
/* USER CODE END 0 */
int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatus) {
uint16_t MinDistance;
uint8_t i;
#ifdef TRACE_PPC
#define TIMES_WITH_NO_EVENT 10// was 40
static uint32_t trace_count = TIMES_WITH_NO_EVENT; // replace by 0 if you want to trace the first TIMES_WITH_NO_EVENT values
#endif
int CurrentZoneStatus = NOBODY;
int AllZonesCurrentStatus = 0;
int AnEventHasOccured = 0;
// Add just picked distance to the table of the corresponding zone
if (DistancesTableSize[zone] < ppc_cfg[sts_distance_mode].distance_array_size) {
Distances[zone][DistancesTableSize[zone]] = Distance;
DistancesTableSize[zone] ++;
}
else {
for (i=1; i<ppc_cfg[sts_distance_mode].distance_array_size; i++)
Distances[zone][i-1] = Distances[zone][i];
Distances[zone][ppc_cfg[sts_distance_mode].distance_array_size-1] = Distance;
}
// pick up the min distance
MinDistance = Distances[zone][0];
if (DistancesTableSize[zone] >= 2) {
for (i=1; i<DistancesTableSize[zone]; i++) {
if (Distances[zone][i] < MinDistance)
MinDistance = Distances[zone][i];
}
}
if (MinDistance < ppc_cfg[sts_distance_mode].dist_threshold) {
// Someone is in !
CurrentZoneStatus = SOMEONE;
}
// left zone
if (zone == LEFT) {
if (CurrentZoneStatus != LeftPreviousStatus) {
// event in left zone has occured
AnEventHasOccured = 1;
if (CurrentZoneStatus == SOMEONE) {
AllZonesCurrentStatus += 1;
}
// need to check right zone as well ...
if (RightPreviousStatus == SOMEONE) {
// event in left zone has occured
AllZonesCurrentStatus += 2;
}
// remember for next time
LeftPreviousStatus = CurrentZoneStatus;
}
}
// right zone
else {
if (CurrentZoneStatus != RightPreviousStatus) {
// event in left zone has occured
AnEventHasOccured = 1;
if (CurrentZoneStatus == SOMEONE) {
AllZonesCurrentStatus += 2;
}
// need to left right zone as well ...
if (LeftPreviousStatus == SOMEONE) {
// event in left zone has occured
AllZonesCurrentStatus += 1;
}
// remember for next time
RightPreviousStatus = CurrentZoneStatus;
}
}
#ifdef TRACE_PPC
// print debug data only when someone is within the field of view
trace_count++;
if ((CurrentZoneStatus == SOMEONE) || (LeftPreviousStatus == SOMEONE) || (RightPreviousStatus == SOMEONE))
trace_count = 0;
#if 0
if (trace_count < TIMES_WITH_NO_EVENT)
printf ("%d,%d,%d,%d,%d\n", zone, Distance, MinDistance, RangeStatus, PeopleCount);
#endif
#endif
// if an event has occured
if (AnEventHasOccured) {
if (PathTrackFillingSize < 4) {
PathTrackFillingSize ++;
}
// if nobody anywhere lets check if an exit or entry has happened
if ((LeftPreviousStatus == NOBODY) && (RightPreviousStatus == NOBODY)) {
// check exit or entry only if PathTrackFillingSize is 4 (for example 0 1 3 2) and last event is 0 (nobobdy anywhere)
if (PathTrackFillingSize == 4) {
// check exit or entry. no need to check PathTrack[0] == 0 , it is always the case
if ((PathTrack[1] == 1) && (PathTrack[2] == 3) && (PathTrack[3] == 2)) {
// This an entry
PeopleCount ++;
// reset the table filling size in case an entry or exit just found
DistancesTableSize[0] = 0;
DistancesTableSize[1] = 0;
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;
} else if ((PathTrack[1] == 2) && (PathTrack[2] == 3) && (PathTrack[3] == 1)) {
// This an exit
PeopleCount --;
// reset the table filling size in case an entry or exit just found
DistancesTableSize[0] = 0;
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 ++;
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 ++;
}
}
PathTrackFillingSize = 1;
}
else {
// update PathTrack
// example of PathTrack update
// 0
// 0 1
// 0 1 3
// 0 1 3 1
// 0 1 3 3
// 0 1 3 2 ==> if next is 0 : check if exit
PathTrack[PathTrackFillingSize-1] = AllZonesCurrentStatus;
}
#if 1
#ifdef TRACE_PPC
if (AnEventHasOccured) {
for (int j=0; j<PathTrackFillingSize; j++)
printf ("%d ", PathTrack[j]);
}
printf("\n");
#endif
#endif
}
//printf ("Inside PeopleCount = %d , returned\n", PeopleCount);
// output debug data to main host machine
return(PeopleCount);
}
void STS_people_count_sensor_Read(sts_people_count_sensor_data_t *sts_p2_sensor_data)
{
sts_p2_sensor_data->Walk_In_People_Count = sts_people_count_sensor_data.Walk_In_People_Count;
sts_p2_sensor_data->Walk_Out_People_Count = sts_people_count_sensor_data.Walk_Out_People_Count;
sts_p2_sensor_data->Walk_Around_People_Count = sts_people_count_sensor_data.Walk_Around_People_Count;
sts_p2_sensor_data->Count_Period = sts_people_count_sensor_data.Count_Period;
sts_p2_sensor_data->Count_Period_Unit = sts_people_count_sensor_data.Count_Period_Unit;
// Sum Day counting
// if not end of day ---
sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count += sts_people_count_sensor_data.Walk_In_People_Count;
sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count += sts_people_count_sensor_data.Walk_Out_People_Count;
sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count += sts_people_count_sensor_data.Walk_Around_People_Count;
sts_p2_sensor_data->Sum_Day_Walk_In_People_Count = sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count;
sts_p2_sensor_data->Sum_Day_Walk_Out_People_Count = sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count;
sts_p2_sensor_data->Sum_Day_Walk_Around_People_Count = sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count;
// else reset day counter
#if 0
sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count = 0;
sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count = 0;
sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count = 0;
#endif
// Sum Week counting
// if not end of week ---
sts_people_count_sensor_data.Sum_Week_Walk_In_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count;
sts_people_count_sensor_data.Sum_Week_Walk_Out_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count;
sts_people_count_sensor_data.Sum_Week_Walk_Around_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count;
// else reset week counter
#if 0
sts_people_count_sensor_data.Sum_Week_Walk_In_People_Count = 0;
sts_people_count_sensor_data.Sum_Week_Walk_Out_People_Count = 0;
sts_people_count_sensor_data.Sum_Week_Walk_Around_People_Count = 0;
#endif
// Sum Month counting
// if not end of month ---
sts_people_count_sensor_data.Sum_Month_Walk_In_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count;
sts_people_count_sensor_data.Sum_Month_Walk_Out_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count;
sts_people_count_sensor_data.Sum_Month_Walk_Around_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count;
// else reset month counter
#if 0
sts_people_count_sensor_data.Sum_Month_Walk_In_People_Count = 0;
sts_people_count_sensor_data.Sum_Month_Walk_Out_People_Count = 0;
sts_people_count_sensor_data.Sum_Month_Walk_Around_People_Count = 0;
#endif
// Sum Year counting
// if not end of year ---
sts_people_count_sensor_data.Sum_Year_Walk_In_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count;
sts_people_count_sensor_data.Sum_Year_Walk_Out_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count;
sts_people_count_sensor_data.Sum_Year_Walk_Around_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count;
// else reset year counter
#if 0
sts_people_count_sensor_data.Sum_Year_Walk_In_People_Count = 0;
sts_people_count_sensor_data.Sum_Year_Walk_Out_People_Count = 0;
sts_people_count_sensor_data.Sum_Year_Walk_Around_People_Count = 0;
#endif
// Sum LifeCycle counting
sts_people_count_sensor_data.Sum_LifeCycle_Walk_In_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count;
sts_people_count_sensor_data.Sum_LifeCycle_Walk_Out_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count;
sts_people_count_sensor_data.Sum_LifeCycle_Walk_Around_People_Count += sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count;
//reset counter of counting period
sts_people_count_sensor_data.Walk_In_People_Count = 0;
sts_people_count_sensor_data.Walk_Out_People_Count = 0;
sts_people_count_sensor_data.Walk_Around_People_Count = 0;
// reset counter at end of day TODO XXX
// reset counter at end of week TODO XXX
// reset counter at end of month TODO XXX
// reset counter at end of year TODO XXX
// reset counter at end of year TODO XXX
// write to NVM store TODO XXX
}
void STS_TOF_VL53LX_PeopleCounting_Process_Init(void)
{
APP_LOG(TS_OFF, VLEVEL_M,"############### TOF VL53LX_ PEOPLE COUNTING PROCESS INITIALIZATION\r\n");
sts_tof_vl53lx_peoplecount_init();
}
void STS_TOF_VL53LX_PeopleCounting_Process_Start(void* context)
{
//APP_LOG(TS_OFF, VLEVEL_M,"############### TOF VL53LX_ PEOPLE COUNTING SUB-PROCESS \r\n");
{
status = sts_tof_vl53lx_peoplecount_start();
//APP_LOG(TS_OFF, VLEVEL_M,"############### SUB-PROCESS running .... \r\n");
}
}
int sts_tof_vl53lx_peoplecount_init(void)
{
//int8_t error;
uint8_t byteData, sensorState=0;
uint16_t wordData;
/* int PplCounter = 0;*/
/* MCU Configuration----------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
/* Configure the system clock */
/* Initialize all configured peripherals */
APP_LOG(TS_OFF, VLEVEL_L,"XNUCLEO53L1A1_Init Start .......... \r\n");
status = XNUCLEO53L1A1_Init();
APP_LOG(TS_OFF, VLEVEL_L,"XNUCLEO53L1A1_Init Status : %X\n", status);
status = XNUCLEO53L1A1_ResetId(XNUCLEO53L1A1_DEV_CENTER, 0); // Reset ToF sensor
HAL_Delay(2);
status = XNUCLEO53L1A1_ResetId(XNUCLEO53L1A1_DEV_CENTER, 1); // Reset ToF sensor
// Those basic I2C read functions can be used to check your own I2C functions */
status = VL53L1_RdByte(dev, 0x010F, &byteData);
APP_LOG(TS_OFF, VLEVEL_L,"VL53L1X Model_ID: %X\n", byteData);
status = VL53L1_RdByte(dev, 0x0110, &byteData);
APP_LOG(TS_OFF, VLEVEL_L,"VL53L1X Module_Type: %X\n", byteData);
status = VL53L1_RdWord(dev, 0x010F, &wordData);
APP_LOG(TS_OFF, VLEVEL_L,"VL53L1X: %X\n", wordData);
while (sensorState == 0) {
status = VL53L1X_BootState(dev, &sensorState);
HAL_Delay(2);
}
APP_LOG(TS_OFF, VLEVEL_L,"Chip booted\n");
/* Initialize and configure the device according to people counting need */
status = VL53L1X_SensorInit(dev);
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);
status += VL53L1X_SetROI(dev, ppc_cfg[sts_distance_mode].rows_of_SPADS, 16); /* minimum ROI 4,4 */
center[0]=ppc_cfg[sts_distance_mode].front_zone_center;
center[1]=ppc_cfg[sts_distance_mode].back_zone_center;
if (status != 0) {
APP_LOG(TS_OFF, VLEVEL_L,"Initialization or configuration of the device\n");
return (-1);
}
APP_LOG(TS_OFF, VLEVEL_L,"Start counting people with profile : %s...\n", PROFILE_STRING);
status = VL53L1X_StartRanging(dev); /* This function has to be called to enable the ranging */
if (status != 0) {
APP_LOG(TS_OFF, VLEVEL_L,"Error in start ranging\n");
return (-1);
}
return 0;
}
int sts_tof_vl53lx_peoplecount_start(void)
{
//uint8_t byteData, sensorState=0;
//uint16_t wordData;
uint16_t Distance, Signal;
uint8_t RangeStatus;
uint8_t dataReady;
//char DisplayStr[5];
/* read and display data */
while (dataReady == 0) {
status = VL53L1X_CheckForDataReady(dev, &dataReady);
HAL_Delay(1);
}
dataReady = 0;
status += VL53L1X_GetRangeStatus(dev, &RangeStatus);
status += VL53L1X_GetDistance(dev, &Distance);
status += VL53L1X_GetSignalPerSpad(dev, &Signal);
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);
}
//HAL_Delay(WAIT_BEFORE_PROGRAMMING_OTHER_ZONE_CENTER); // 10, 8, 7, 6 tested OK
status = VL53L1X_SetROICenter(dev, center[Zone]);
if (status != 0) {
APP_LOG(TS_OFF, VLEVEL_L,"Error in chaning the center of the ROI\n");
return (-1);
}
// check the status of the ranging. In case of error, lets assume the distance is the max of the use case
// Value RangeStatus string Comment
// 0 VL53L1_RANGESTATUS_RANGE_VALID Ranging measurement is valid
// 1 VL53L1_RANGESTATUS_SIGMA_FAIL Raised if sigma estimator check is above the internal defined threshold
// 2 VL53L1_RANGESTATUS_SIGNAL_FAIL Raised if signal value is below the internal defined threshold
// 4 VL53L1_RANGESTATUS_OUTOFBOUNDS_ FAIL Raised when phase is out of bounds
// 5 VL53L1_RANGESTATUS_HARDWARE_FAIL Raised in case of HW or VCSEL failure
// 7 VL53L1_RANGESTATUS_WRAP_TARGET_ FAIL Wrapped target, not matching phases
// 8 VL53L1_RANGESTATUS_PROCESSING_ FAIL Internal algorithm underflow or overflow
// 14 VL53L1_RANGESTATUS_RANGE_INVALID The reported range is invalid
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;
}
else // severe error cases
Distance = ppc_cfg[sts_distance_mode].max_distance; //MAX_DISTANCE;
// inject the new ranged distance in the people counting algorithm
PplCounter = ProcessPeopleCountingData(Distance, Zone, RangeStatus);
//printf("\PplCounter =%d \n\r", PplCounter);
//sprintf(DisplayStr, "%4d", PplCounter); // only use for special EVK with display
//XNUCLEO53L1A1_SetDisplayString(DisplayStr);
//APP_LOG(TS_OFF, VLEVEL_H,"%d,%d,%d\n", Zone, Distance, Signal);
Zone++;
Zone = Zone%2;
return 0;
}
Reference in New Issue View Git Blame Copy Permalink