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

1078 lines
42 KiB
C
Raw Blame History

/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "sys_app.h"
/* USER CODE BEGIN Includes */
#include "yunhorn_sts_sensors.h"
#include "app_tof_peoplecount.h"
#include "flash_if.h"
#include "stm32_mem.h"
#if defined(STS_P2)||defined(STS_T6)
#include "VL53L1X_API.h"
#include "VL53l1X_calibration.h"
#include "X-NUCLEO-53L1A1.h"
#include "sts_lamp_bar.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;
extern volatile sts_cfg_nvm_t sts_cfg_nvm;
extern volatile uint8_t nvm_store_value[];
volatile uint8_t sts_tof_presence_state=0, sts_tof_presence_state_changed=0, sts_tof_presence_distance_dm=0, sts_pir_state=0;
extern volatile uint8_t sts_lamp_bar_color, sts_color_occupy_vacant;
#define STS_COLOR_DEFAULT_VACANT (sts_color_occupy_vacant&0x0f)
#define STS_COLOR_DEFAULT_OCCUPY ((sts_color_occupy_vacant>>4)&0x0f)
/* 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[10] = {
{DOOR_SIDE_1300,10, 1300,0, 1200,8,33, DISTANCE_MODE_SHORT,175,231},
{DOOR_JAM_1600, 10, 1600,0, 1300,8,33, DISTANCE_MODE_SHORT,175,231},
{DOOR_JAM_2400, 10, 2400,400, 1900,8,50, DISTANCE_MODE_LONG,175,231},
{DOOR_JAM_3000, 10, 3000,1200, 2500,8,100,DISTANCE_MODE_LONG,175,231},
{DOOR_JAM_3500, 10, 3500,1700, 3000,8,100,DISTANCE_MODE_LONG,175,231},
{DOOR_JAM_4000, 10, 4000,2200, 3500,8,200,DISTANCE_MODE_LONG,175,231},
{DOOR_JAM_NOW, 10, 2200, 400, 1800,8,100,DISTANCE_MODE_LONG,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)||defined(STS_T6))
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; //DOOR_JAM_2400; //sts_door_jam_profile; //DOOR_JAM_3000;
#endif
#if (defined(STS_T6)||defined(STS_P2))
volatile uint8_t sts_people_count_number_changed = 0;
#endif
#if defined(STS_P2)
static uint32_t cnt_today=0, cnt_day=0, cnt_week=0, cnt_month=0, cnt_year=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};
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};
#if defined(STS_P2)
static int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatus);
#endif
/* 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 */
#if (defined(STS_T6))
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];
}
}
//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
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
}
#endif
#if (defined(STS_P2))
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_door_jam_profile].distance_array_size) {
Distances[zone][DistancesTableSize[zone]] = Distance;
DistancesTableSize[zone] ++;
}
else {
for (i=1; i<ppc_cfg[sts_door_jam_profile].distance_array_size; i++)
Distances[zone][i-1] = Distances[zone][i];
Distances[zone][ppc_cfg[sts_door_jam_profile].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];
}
}
// APP_LOG(TS_OFF, VLEVEL_M, "\r\n Distance=%d Min. & threshold ---%d %d", Distance, MinDistance,ppc_cfg[sts_distance_mode].dist_threshold);
if (MinDistance < ppc_cfg[sts_door_jam_profile].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) {
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)) {
if ((PathTrack[1] == 1) && (PathTrack[2] == 3)) { // 2025-JAN-90 aggressive mode
// 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 Head/Min =%d Range=%d\n", PeopleCount, MinDistance, Distance);
sts_people_count_sensor_data.Walk_In_People_Count ++;
sts_people_count_number_changed = 1;
LED_RED_OFF;
LED_GREEN_ON;
HAL_Delay(50);
LED_GREEN_OFF;
// 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)) {
} else if ((PathTrack[1] == 2) && (PathTrack[2] == 3)) { // 2025-JAN-90 aggressive mode
// 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 Head/Min =%d Range=%d\n", PeopleCount, MinDistance, Distance);
sts_people_count_sensor_data.Walk_Out_People_Count ++;
//LED1_ON; //TODO XXX, CHANGE TO (LED_IN_OFF && LED_OUT_ON);
//LED1_TOGGLE;
LED_GREEN_OFF;
LED_RED_ON;
HAL_Delay(50);
LED_RED_OFF;
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_L,"Walk Around ie. Wrong path, Head Distance=%d \n", MinDistance);
sts_people_count_sensor_data.Walk_Around_People_Count ++;
LED_RED_ON;
LED_GREEN_ON;
HAL_Delay(30);
LED_GREEN_OFF;
LED_RED_OFF;
HAL_Delay(30);
LED_RED_ON;
LED_GREEN_ON;
HAL_Delay(30);
LED_GREEN_OFF;
LED_RED_OFF;
}
}
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 0
#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);
}
#endif
#if defined(STS_T6)
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;
sts_t6_sensor_data->tof_presence_distance_dm = sts_tof_presence_distance_dm;
sts_t6_sensor_data->pir_motion_sensor_state = sts_pir_state;
sts_t6_sensor_data->lamp_bar_color = sts_lamp_bar_color;
}
#endif
#if defined(STS_P2)
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_cfg_nvm.periodicity; //sts_people_count_sensor_data.Count_Period;
sts_p2_sensor_data->Count_Period_Unit = sts_cfg_nvm.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
cnt_day ++;
if (cnt_day >=7) {
cnt_day = 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;
cnt_week++;
if (cnt_week >= 4 ) {
cnt_week =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;
cnt_month++;
if (cnt_month >=12) {
cnt_month =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;
cnt_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;
}
}
}
uint16_t sts_p2_data_to_nvm[32] ={0x0}, pp =0;
// OnRestoreSTSLOGContextRequest((void*)sts_p2_data_to_nvm);
// TODO XXX, better read the data in flash and then add up today's data
// TODO XXX
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Week_Walk_In_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Week_Walk_Out_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Week_Walk_Around_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Month_Walk_In_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Month_Walk_Out_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Month_Walk_Around_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Year_Walk_In_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Year_Walk_Out_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_Year_Walk_Around_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_LifeCycle_Walk_In_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_LifeCycle_Walk_Out_People_Count;
sts_p2_data_to_nvm[pp++] = sts_people_count_sensor_data.Sum_LifeCycle_Walk_Around_People_Count;
/*
APP_LOG(TS_OFF, VLEVEL_M, "\n-----------------------------------------------\n");
APP_LOG(TS_OFF, VLEVEL_M, "\n00 01 02 03 04 05 06 07 08 09 10 11 12 13 14 15\n");
for (uint8_t i=0; i<YUNHORN_STS_MAX_NVM_CFG_SIZE; i++)
{
if (i%8==0) APP_LOG(TS_OFF, VLEVEL_M, "\n");
APP_LOG(TS_OFF, VLEVEL_M, "%04X ", sts_p2_data_to_nvm[i]);
}
APP_LOG(TS_OFF, VLEVEL_M, "\n\r");
APP_LOG(TS_OFF, VLEVEL_M, "\n###############################################\n\r");
*/
if (FLASH_IF_Erase(STS_SENSOR_DATA_NVM_BASE_ADDRESS, FLASH_PAGE_SIZE) == FLASH_IF_OK)
{
FLASH_IF_Write(STS_SENSOR_DATA_NVM_BASE_ADDRESS, (const void *)sts_p2_data_to_nvm, 64);
}
}
// reset counter at end of year TODO XXX
// reset counter at end of year TODO XXX
// write to NVM store TODO XXX
}
#endif
#if (defined(STS_P2))
void STS_TOF_VL53LX_PeopleCounting_Process_Init(void)
{
APP_LOG(TS_OFF, VLEVEL_L,"\n\n# YUNHORN STS-P2 PEOPLE COUNTING PROCESS #\r\n");
sts_tof_vl53lx_peoplecount_init();
sts_people_count_sensor_data.Count_Period = sts_cfg_nvm.periodicity;
sts_people_count_sensor_data.Count_Period_Unit = sts_cfg_nvm.unit;
uint16_t log_in_nvm[32];
OnRestoreSTSLOGContextRequest(log_in_nvm);
sts_people_count_sensor_data.Sum_Day_Walk_In_People_Count = (uint16_t)log_in_nvm[0];
sts_people_count_sensor_data.Sum_Day_Walk_Out_People_Count = (uint16_t)log_in_nvm[1];
sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count = (uint16_t)log_in_nvm[2];
sts_people_count_sensor_data.Sum_Week_Walk_In_People_Count = (uint16_t)log_in_nvm[3];
sts_people_count_sensor_data.Sum_Week_Walk_Out_People_Count = (uint16_t)log_in_nvm[4];
sts_people_count_sensor_data.Sum_Week_Walk_Around_People_Count = (uint16_t)log_in_nvm[5];
sts_people_count_sensor_data.Sum_Month_Walk_In_People_Count = (uint16_t)log_in_nvm[6];
sts_people_count_sensor_data.Sum_Month_Walk_Out_People_Count = (uint16_t)log_in_nvm[7];
sts_people_count_sensor_data.Sum_Month_Walk_Around_People_Count = (uint16_t)log_in_nvm[8];
sts_people_count_sensor_data.Sum_Year_Walk_In_People_Count = (uint16_t)log_in_nvm[9];
sts_people_count_sensor_data.Sum_Year_Walk_Out_People_Count = (uint16_t)log_in_nvm[10];
sts_people_count_sensor_data.Sum_Year_Walk_Around_People_Count = (uint16_t)log_in_nvm[11];
sts_people_count_sensor_data.Sum_LifeCycle_Walk_In_People_Count = (uint16_t)log_in_nvm[12];
sts_people_count_sensor_data.Sum_LifeCycle_Walk_Out_People_Count = (uint16_t)log_in_nvm[13];
sts_people_count_sensor_data.Sum_LifeCycle_Walk_Around_People_Count = (uint16_t)log_in_nvm[14];
}
void STS_TOF_VL53LX_PeopleCounting_Process_Start(void)
{
//APP_LOG(TS_OFF, VLEVEL_M,"############### TOF VL53LX_ PEOPLE COUNTING SUB-PROCESS \r\n");
{
int status = sts_tof_vl53lx_peoplecount_start();
if (status == -1)
{
sts_tof_vl53lx_peoplecount_init();
return ;
} else if (status !=0)
{
sts_people_count_sensor_data.Count_Valid = 0;
//APP_LOG(TS_OFF, VLEVEL_M,"# Counting time out... \r\n");
}
}
}
#endif
#if (defined(STS_T6))
void STS_TOF_VL53LX_PresenceDetection_Process_Start(void)
{
//APP_LOG(TS_OFF, VLEVEL_M,"############### TOF VL53LX_ PRESENCE DETECTION SUB-PROCESS \r\n");
{
int status = sts_tof_vl53lx_presence_detection_start();
if (status == -1)
{
sts_tof_vl53lx_presence_detection_init();
return ;
}
if (sts_tof_presence_state ==1)
{
LED_GREEN_OFF;
LED_RED_ON;
sts_lamp_bar_color = (sts_cfg_nvm.color_occupy_vacant>>4)&0x0f; //STS_COLOR_DEFAULT_OCCUPY; //(sts_cfg_nvm.color_occupy_vacant>>4)&0x0f; //STS_RED;
} else if (sts_tof_presence_state ==0)
{
LED_RED_OFF;
LED_GREEN_ON;
sts_lamp_bar_color = (sts_cfg_nvm.color_occupy_vacant)&0x0f; //STS_COLOR_DEFAULT_VACANT; //(sts_cfg_nvm.color_occupy_vacant)&0x0f; //STS_GREEN;
}
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_Init(void)
{
APP_LOG(TS_OFF, VLEVEL_M,"\r\n############### %s Init ############### \r\n", YUNHORN_STS_PRD_STRING);
{
uint8_t status = sts_tof_vl53lx_presence_detection_init();
sts_people_count_sensor_data.Count_Valid = (status ==0)? 1:0;
}
}
#endif
#if (defined(STS_T6))
int sts_tof_vl53lx_presence_detection_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_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);
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_L,"\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);
sts_distance_mode = (sts_door_jam_profile ==0)?DISTANCE_MODE_SHORT:DISTANCE_MODE_LONG;
APP_LOG(TS_OFF, VLEVEL_M, "\r\nDistance[ Max=%d, Min=%d Threshold=%d ] [Timing=%d ] [ Mode=%d ]\r\n",
ppc_cfg[sts_door_jam_profile].max_distance,
ppc_cfg[sts_door_jam_profile].min_distance,
ppc_cfg[sts_door_jam_profile].dist_threshold,
ppc_cfg[sts_door_jam_profile].timing_budget,
ppc_cfg[sts_door_jam_profile].distance_mode);
status += VL53L1X_SetDistanceMode(dev, ppc_cfg[sts_door_jam_profile].distance_mode); /* 1=short, 2=long, DISTANCE_MODE */
status += VL53L1X_SetTimingBudgetInMs(dev, ppc_cfg[sts_door_jam_profile].timing_budget); /* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */
status += VL53L1X_SetInterMeasurementInMs(dev, ppc_cfg[sts_door_jam_profile].timing_budget);
status += VL53L1X_SetROI(dev, ppc_cfg[sts_door_jam_profile].rows_of_SPADS, 16); /* minimum ROI 4,4 */
status += VL53L1X_SetDistanceThreshold(dev,ppc_cfg[sts_door_jam_profile].min_distance,ppc_cfg[sts_door_jam_profile].max_distance, 3, 1); // 3= in window, 1= int on target
center[0]=ppc_cfg[sts_door_jam_profile].front_zone_center;
center[1]=ppc_cfg[sts_door_jam_profile].back_zone_center;
if (status != 0) {
APP_LOG(TS_OFF, VLEVEL_M,"Initialization or configuration of the device\n");
return (-1);
}
APP_LOG(TS_OFF, VLEVEL_M,"\n\nStart detection with profile : %d \r\n\n\n\n", sts_door_jam_profile);
status = VL53L1X_StartRanging(dev); /* This function has to be called to enable the ranging */
if (status != 0) {
APP_LOG(TS_OFF, VLEVEL_M,"Error in start ranging\n");
return (-1);
}
return 0;
}
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 > 500) return -1; // 50 ms timer, so make this 60% to fail back
}
status = 0;
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, status =%X \n", status);
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 0
if ((RangeStatus == 0) || (RangeStatus == 4) || (RangeStatus == 7)) {
if (Distance <= ppc_cfg[sts_door_jam_profile].min_distance) //MIN_DISTANCE) // wraparound case see the explanation at the constants definition place
Distance = ppc_cfg[sts_door_jam_profile].max_distance + ppc_cfg[sts_door_jam_profile].min_distance; //MAX_DISTANCE + MIN_DISTANCE;
}
else // severe error cases
Distance = ppc_cfg[sts_door_jam_profile].max_distance; //MAX_DISTANCE;
// inject the new ranged distance in the people counting algorithm
#endif
//if (Distance < ppc_cfg[sts_door_jam_profile].dist_threshold)
#if 1
if ((Distance < ppc_cfg[sts_door_jam_profile].dist_threshold) && ((Distance > ppc_cfg[sts_door_jam_profile].min_distance)))
{
PresenceState = 1;
} else {
PresenceState = 0;
}
#endif
//uint8_t PresenceState2 = ProcessPresenceDetectionData(Distance, Zone, RangeStatus);
//printf("\nPresenceState2 =%d \n\r", PresenceState2);
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;
sts_tof_presence_distance_dm = (uint8_t)(Distance/100); // change to dm=10cm=100mm
APP_LOG(TS_OFF, VLEVEL_M,"\r\n Presence Detected =%d\r\n", sts_tof_presence_state);
}
APP_LOG(TS_OFF, VLEVEL_M,"%d,%d,%d\n", Zone, Distance, Signal);
Zone++;
Zone = Zone%2;
return 1;
}
#endif
#if (defined(STS_P2))
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_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);
sensor_id = wordData;
wordData = 0;
while (sensorState == 0) {
status = VL53L1X_BootState(dev, &sensorState);
HAL_Delay(1);
wordData ++;
if (wordData > 8000) {
APP_LOG(TS_OFF, VLEVEL_L,"\n\n ***Failed to boot Chip***\n\n\n");
return status;
}
}
APP_LOG(TS_OFF, VLEVEL_L,"\nChip booted\n\n");
/* Initialize and configure the device according to people counting need */
status = VL53L1X_SensorInit(dev);
sts_distance_mode = (sts_door_jam_profile ==0)?DISTANCE_MODE_SHORT:DISTANCE_MODE_LONG;
APP_LOG(TS_OFF, VLEVEL_L, "\r\n MaxDistance=%d, \nMinDistance=%d \ndist_threshold=%d \n timing_budget=%d \n distance_mode=%d \r\n",
ppc_cfg[sts_door_jam_profile].max_distance,
ppc_cfg[sts_door_jam_profile].min_distance,
ppc_cfg[sts_door_jam_profile].dist_threshold,
ppc_cfg[sts_door_jam_profile].timing_budget,
ppc_cfg[sts_door_jam_profile].distance_mode);
status += VL53L1X_SetDistanceMode(dev, ppc_cfg[sts_door_jam_profile].distance_mode); /* 1=short, 2=long, DISTANCE_MODE */
status += VL53L1X_SetTimingBudgetInMs(dev, ppc_cfg[sts_door_jam_profile].timing_budget); /* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */
status += VL53L1X_SetInterMeasurementInMs(dev, ppc_cfg[sts_door_jam_profile].timing_budget);
status += VL53L1X_SetROI(dev, ppc_cfg[sts_door_jam_profile].rows_of_SPADS, 16); /* minimum ROI 4,4 */
// status += VL53L1X_SetDistanceThreshold(dev,ppc_cfg[sts_door_jam_profile].min_distance,ppc_cfg[sts_door_jam_profile].max_distance, 3, 1); // 3= in window, 1= int on target
center[0]=ppc_cfg[sts_door_jam_profile].front_zone_center;
center[1]=ppc_cfg[sts_door_jam_profile].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_L,"\n\nInit finished...\r\nStart counting people with profile : %d \r\n\n\n\n", sts_door_jam_profile);
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;
}
#endif
#if (defined(STS_P2))
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;
uint8_t status =0;
//char DisplayStr[5];
/* read and display data */
while (dataReady == 0) {
status = VL53L1X_CheckForDataReady(dev, &dataReady);
HAL_Delay(1);
wordData++;
if (wordData > 500) return -1; // 50 ms timer, so make this 60% to fail back
}
dataReady = 0;
status = 0; wordData =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 changing 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 0
if ((RangeStatus == 0) || (RangeStatus == 4) || (RangeStatus == 7)) {
if (Distance <= ppc_cfg[sts_door_jam_profile].min_distance) //MIN_DISTANCE) // wraparound case see the explanation at the constants definition place
Distance = ppc_cfg[sts_door_jam_profile].max_distance + ppc_cfg[sts_door_jam_profile].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_door_jam_profile].max_distance; //MAX_DISTANCE;
#endif
// inject the new ranged distance in the people counting algorithm
//APP_LOG(TS_OFF, VLEVEL_L,"%d,%d,%d\n", Zone, Distance, Signal);
PplCounter = ProcessPeopleCountingData(Distance, Zone, RangeStatus);
//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,"Zone, Distance, Signal = %d,%d,%d\n", Zone, Distance, Signal);
Zone++;
Zone = Zone%2;
return 0;
}
#endif
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);
} else if (Distance != 0 )
{
APP_LOG(TS_OFF, VLEVEL_L,"\r\nVL53L1X RANGE DISTANCE =%d \r\n", Distance);
*range_distance = Distance;
}
return status;
}
Reference in New Issue View Git Blame Copy Permalink