769 lines
30 KiB
C
769 lines
30 KiB
C
|
|
/* Includes ------------------------------------------------------------------*/
|
|
#include "main.h"
|
|
#include "sys_app.h"
|
|
/* USER CODE BEGIN Includes */
|
|
#include "app_tof_peoplecount.h"
|
|
#ifdef STS_P2
|
|
#include "VL53L1X_API.h"
|
|
#include "VL53l1X_calibration.h"
|
|
#include "X-NUCLEO-53L1A1.h"
|
|
#endif
|
|
#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,1};
|
|
extern volatile uint32_t STS_TOFScanPeriod_sec, STS_TxPeriod_sec, STS_HeartBeatTimerPeriod_sec;
|
|
volatile uint8_t sts_tof_presence_state=0, sts_tof_presence_state_changed=0;
|
|
|
|
/* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */
|
|
/* DISTANCE_THRESHOLD = MAX - ASSUME_CHILD_HEIGHT(800mm) */
|
|
volatile sts_ppc_cfg_type_t ppc_cfg[6] = {
|
|
{DOOR_JAM_2000, 16, 2000,0, 1200,16, tbms_33,DISTANCE_MODE_LONG,199,199},
|
|
{DOOR_JAM_2400, 10, 2400,0, 1600,8, tbms_50,DISTANCE_MODE_LONG,175,231},
|
|
{DOOR_JAM_3000, 10, 3000,0, 2200,8,tbms_100,DISTANCE_MODE_LONG,175,231},
|
|
{DOOR_JAM_3500, 10, 3500,0, 2700,8,tbms_100,DISTANCE_MODE_LONG,175,231},
|
|
{DOOR_JAM_4000, 10, 4000,0, 3200,8,tbms_200,DISTANCE_MODE_LONG,175,231},
|
|
{DOOR_SIDE_1600, 8, 1600,0, 300,8, tbms_33,DISTANCE_MODE_SHORT,175,231},
|
|
};
|
|
volatile sts_zone_center_by_rows_of_spads_t zone_center[4]={
|
|
{4, 151, 247},
|
|
{6, 159, 239},
|
|
{8, 175, 231},
|
|
{16, 191,191},
|
|
};
|
|
#if (defined(STS_P2))
|
|
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 = 0; //sts_door_jam_profile; //DOOR_JAM_3000;
|
|
#endif
|
|
#if (defined(STS_P2))
|
|
volatile uint8_t sts_people_count_number_changed = 0;
|
|
static uint32_t cnt_today=0, cnt_week=0;
|
|
static uint8_t last_year=0, this_year=0,last_month=0, this_month=0,last_week=0, this_week=0,last_day=0, this_day=0;
|
|
#endif
|
|
uint16_t dev=0x52;
|
|
extern uint16_t sensor_id;
|
|
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 ProcessPresenceDetectionData(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;
|
|
}
|
|
|
|
//return CurrentZoneStatus;
|
|
#if 1
|
|
// 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 1
|
|
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
|
|
}
|
|
// if SOMEONE IN no matter LEFT or RIGHT or MIDDLE
|
|
return(AnEventHasOccured);
|
|
#endif
|
|
}
|
|
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_tof_presence_detection_sensor_Read(sts_tof_presence_detection_sensor_data_t *sts_t6_sensor_data)
|
|
{
|
|
sts_t6_sensor_data->tof_range_presence_state = sts_tof_presence_state;
|
|
}
|
|
|
|
void STS_people_count_sensor_Read(sts_people_count_sensor_data_t *sts_p2_sensor_data)
|
|
{
|
|
|
|
sts_p2_sensor_data->Count_Valid = sts_people_count_sensor_data.Count_Valid;
|
|
|
|
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;
|
|
|
|
|
|
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 of counting period
|
|
cnt_today ++;
|
|
if (cnt_today*STS_TxPeriod_sec > 86400)
|
|
{
|
|
cnt_today = 0;
|
|
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;
|
|
}
|
|
|
|
if (cnt_today ==0)
|
|
{ // new day, then check week/month/year, not check every time
|
|
// reset counter at end of day TODO XXX
|
|
if (last_day != this_day) {
|
|
cnt_today = 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;
|
|
}
|
|
if (cnt_week)
|
|
// reset counter at end of week TODO XXX
|
|
if (last_week != this_week) {
|
|
|
|
|
|
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;
|
|
}
|
|
|
|
// reset counter at end of month TODO XXX
|
|
if (last_month != this_month) {
|
|
|
|
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;
|
|
}
|
|
|
|
if (last_year != this_year) {
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
// 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,"\n\n# YUNHORN STS-P2 PEOPLE COUNTING PROCESS #\r\n");
|
|
sts_tof_vl53lx_peoplecount_init();
|
|
|
|
}
|
|
|
|
void STS_TOF_VL53LX_PeopleCounting_Process_Start(void)
|
|
{
|
|
|
|
//APP_LOG(TS_OFF, VLEVEL_M,"############### TOF VL53LX_ PEOPLE COUNTING SUB-PROCESS \r\n");
|
|
{
|
|
status = sts_tof_vl53lx_peoplecount_start();
|
|
sts_people_count_sensor_data.Count_Valid = (status ==0)? 1:0;
|
|
//APP_LOG(TS_OFF, VLEVEL_M,"############### SUB-PROCESS running .... \r\n");
|
|
}
|
|
|
|
}
|
|
|
|
void STS_TOF_VL53LX_PresenceDetection_Process_Start(void)
|
|
{
|
|
|
|
//APP_LOG(TS_OFF, VLEVEL_M,"############### TOF VL53LX_ PRESENCE DETECTION SUB-PROCESS \r\n");
|
|
{
|
|
status = sts_tof_vl53lx_presence_detection_start();
|
|
sts_people_count_sensor_data.Count_Valid = (status ==0)? 1:0;
|
|
//APP_LOG(TS_OFF, VLEVEL_M,"############### SUB-PROCESS running .... \r\n");
|
|
}
|
|
|
|
}
|
|
int sts_tof_vl53lx_presence_detection_start(void)
|
|
{
|
|
//uint8_t byteData, sensorState=0;
|
|
uint16_t wordData=0;
|
|
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);
|
|
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_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
|
|
|
|
|
|
if (Distance < ppc_cfg[sts_distance_mode].dist_threshold)
|
|
{
|
|
PresenceState = 1;
|
|
LED1_ON;
|
|
} else {
|
|
PresenceState = 0;
|
|
LED1_OFF;
|
|
}
|
|
//PresenceState = ProcessPresenceDetectionData(Distance, Zone, RangeStatus);
|
|
//printf("\nPresenceState =%d \n\r", PresenceState);
|
|
if (PresenceState == PrevPresenceState)
|
|
{
|
|
sts_tof_presence_state_changed = 0;
|
|
return 0;
|
|
} else {
|
|
sts_tof_presence_state = PresenceState;
|
|
PrevPresenceState = PresenceState;
|
|
sts_tof_presence_state_changed = 1;
|
|
}
|
|
//APP_LOG(TS_OFF, VLEVEL_M,"%d,%d,%d\n", Zone, Distance, Signal);
|
|
Zone++;
|
|
Zone = Zone%2;
|
|
|
|
return 0;
|
|
|
|
}
|
|
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_H,"XNUCLEO53L1A1_Init Start .......... \r\n");
|
|
status = XNUCLEO53L1A1_Init();
|
|
//APP_LOG(TS_OFF, VLEVEL_H,"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_H,"VL53L1X Model_ID: %X\n", byteData);
|
|
status = VL53L1_RdByte(dev, 0x0110, &byteData);
|
|
APP_LOG(TS_OFF, VLEVEL_M,"VL53L1X Module_Type: %X\n", byteData);
|
|
status = VL53L1_RdWord(dev, 0x010F, &wordData);
|
|
APP_LOG(TS_OFF, VLEVEL_M,"VL53L1X: %X\n", wordData);
|
|
sensor_id = wordData;
|
|
wordData = 0;
|
|
while (sensorState == 0) {
|
|
status = VL53L1X_BootState(dev, &sensorState);
|
|
HAL_Delay(2);
|
|
wordData ++;
|
|
if (wordData > 8000) {
|
|
APP_LOG(TS_OFF, VLEVEL_M,"\n\n ***Failed to boot Chip***\n\n\n");
|
|
return status;
|
|
}
|
|
}
|
|
APP_LOG(TS_OFF, VLEVEL_M,"\nChip booted\n\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_H,"Initialization or configuration of the device\n");
|
|
return (-1);
|
|
}
|
|
//APP_LOG(TS_OFF, VLEVEL_H,"\n\nStart counting people with profile : %s...\n\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_H,"Error in start ranging\n");
|
|
return (-1);
|
|
}
|
|
return 0;
|
|
}
|
|
int sts_tof_vl53lx_peoplecount_start(void)
|
|
{
|
|
//uint8_t byteData, sensorState=0;
|
|
uint16_t wordData=0;
|
|
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);
|
|
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_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;
|
|
}
|