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

309 lines
11 KiB
C
Raw 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"
/* USER CODE END Includes */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* Private variables ---------------------------------------------------------*/
extern I2C_HandleTypeDef hi2c2;
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 */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE END PFP */
int sts_tof_vl53lx_peoplecount(void);
/* USER CODE BEGIN 0 */
#define VL53L1X_INT_Pin (GPIO_PIN_10) // WL55JC GPIO_PIN_10, F401xE ==>(GPIO_PIN_4)
/*
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
if (GPIO_Pin==VL53L1X_INT_Pin)
{
IntCount++;
}
}
*/
/* USER CODE END 0 */
int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatus) {
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};
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] < DISTANCES_ARRAY_SIZE) {
Distances[zone][DistancesTableSize[zone]] = Distance;
DistancesTableSize[zone] ++;
}
else {
for (i=1; i<DISTANCES_ARRAY_SIZE; i++)
Distances[zone][i-1] = Distances[zone][i];
Distances[zone][DISTANCES_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 < 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 (trace_count < TIMES_WITH_NO_EVENT)
// printf ("%d,%d,%d,%d,%d\n", zone, Distance, MinDistance, RangeStatus, PeopleCount);
#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_L,"Walk In, People Count=%d\n", PeopleCount);
} 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_L,"Walk Out, People Count=%d\n", PeopleCount);
} else {
// reset the table filling size also in case of unexpected path
DistancesTableSize[0] = 0;
DistancesTableSize[1] = 0;
APP_LOG(TS_OFF, VLEVEL_L,"Wrong path\n");
}
}
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;
}
//#ifdef TRACE_PPC
// if (AnEventHasOccured) {
// for (int j=0; j<PathTrackFillingSize; j++)
// printf ("%d ", PathTrack[j]);
// }
// printf("\n");
//#endif
}
// output debug data to main host machine
return(PeopleCount);
}
void STS_TOF_VL53LX_PeopleCounting_Process(void)
{
APP_LOG(TS_OFF, VLEVEL_L,"############### TOF VL53LX_ PEOPLE COUNTING PROCESS \r\n");
sts_tof_vl53lx_peoplecount();
}
int sts_tof_vl53lx_peoplecount(void)
{
//int8_t error;
uint8_t byteData, sensorState=0;
uint16_t wordData;
uint16_t Distance, Signal;
uint8_t RangeStatus;
uint8_t dataReady;
char DisplayStr[5];
int PplCounter;
int center[2] = {FRONT_ZONE_CENTER, BACK_ZONE_CENTER}; /* these are the spad center of the 2 4*16 zones */
int Zone = 0;
/* int PplCounter = 0;*/
/* MCU Configuration----------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
/* Configure the system clock */
/* Initialize all configured peripherals */
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, DISTANCE_MODE); /* 1=short, 2=long */
status += VL53L1X_SetTimingBudgetInMs(dev, TIMING_BUDGET); /* in ms possible values [15, 20, 50, 100, 200, 500] */
status += VL53L1X_SetInterMeasurementInMs(dev, TIMING_BUDGET);
status += VL53L1X_SetROI(dev, ROWS_OF_SPADS, 16); /* minimum ROI 4,4 */
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 */
while(1) { /* 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 <= MIN_DISTANCE) // wraparound case see the explanation at the constants definition place
Distance = MAX_DISTANCE + MIN_DISTANCE;
}
else // severe error cases
Distance = MAX_DISTANCE;
// inject the new ranged distance in the people counting algorithm
PplCounter = ProcessPeopleCountingData(Distance, Zone, RangeStatus);
//sprintf(DisplayStr, "%4d", PplCounter); // only use for special EVK with display
//XNUCLEO53L1A1_SetDisplayString(DisplayStr);
APP_LOG(TS_OFF, VLEVEL_L,"%d,%d,%d\n", Zone, Distance, Signal);
Zone++;
Zone = Zone%2;
}
}
Reference in New Issue View Git Blame Copy Permalink