Merge pull request 'P2B' (#51) from P2B into master
Reviewed-on: https://gitea.yunhorn.com/sundp/WLE5CC_NODE_STS/pulls/51
This commit is contained in:
commit
874955126f
Binary file not shown.
|
|
@ -369,7 +369,9 @@ enum sts_lamp_color {
|
|||
*/
|
||||
|
||||
#define STS_CONFIG_NVM_BASE_ADDRESS FLASH_USER_START_ADDR
|
||||
#define STS_CONFIG_NVM_CONFIG_SIZE FLASH_USER_CONFIG_SIZE
|
||||
#define STS_CONFIG_NVM_CONFIG_SIZE FLASH_USER_CONFIG_SIZE
|
||||
#define STS_SENSOR_DATA_NVM_BASE_ADDRESS ((void *) 0x0803F000U) // Last 4kB of flash
|
||||
#define STS_SENSOR_DATA_NVM_SIZE ((void *) 0x000007FFU) //0x400=1KB=1024
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* LoRaWAN NVM configuration */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
|
|
|||
|
|
@ -238,7 +238,9 @@ void OnStoreSTSCFGContextRequest(void);
|
|||
/**
|
||||
* @brief Read config from flash
|
||||
*/
|
||||
void OnRestoreSTSCFGContextRequest(uint8_t *cfg_in_nvm);
|
||||
void OnRestoreSTSCFGContextRequest(void *cfg_in_nvm);
|
||||
|
||||
void OnRestoreSTSLOGContextRequest(void *log_in_nvm);
|
||||
|
||||
void STS_REBOOT_CONFIG_Init(void);
|
||||
|
||||
|
|
|
|||
|
|
@ -47,8 +47,10 @@
|
|||
#if defined(STS_P2)||defined(STS_T6)
|
||||
#include "app_tof.h"
|
||||
#include "app_tof_peoplecount.h"
|
||||
extern volatile sts_people_count_sensor_data_t *sts_people_count_sensor_data;
|
||||
extern volatile uint8_t sts_door_jam_profile;
|
||||
extern volatile uint8_t sts_tof_presence_state_changed;
|
||||
extern volatile sts_ppc_cfg_type_t ppc_cfg[];
|
||||
#elif defined(L8)
|
||||
#include "app_tof.h"
|
||||
#endif
|
||||
|
|
@ -71,6 +73,11 @@ volatile sts_cfg_nvm_t sts_cfg_nvm = {
|
|||
0x00, //sts_ioc_mask
|
||||
0x20, //32 bytes, below start of p[0] 20 BYTES AND 12 BYTES FALL DOWN CFG
|
||||
{ // below 20 bytes
|
||||
#ifdef STS_P2
|
||||
0x0,0x8,0x10,0x0,0x3,0x8,0x21,0x1,0xaf,0xe7, //current work config profile short
|
||||
0x1,0x8,0x10,0x0,0x3,0x8,0x21,0x1,0xaf,0xe7, //current work config profile long, after measured distance
|
||||
// change the max distance, min distance, SPAD, front center and back center, etc.
|
||||
#else
|
||||
0x08, //start_m [8]*0.1 meter =0.8
|
||||
0x19, //lenght_m 0x19=[25]*0.1=2.5f meter
|
||||
0x0F, //threshold 0X0F=[15]*0.1=1.5f
|
||||
|
|
@ -91,6 +98,7 @@ volatile sts_cfg_nvm_t sts_cfg_nvm = {
|
|||
0x00, //reserve --P[17]
|
||||
0x00, //reserve --P[18]
|
||||
0x00, //reserve --P[19]
|
||||
#endif
|
||||
}, // above 20 bytes
|
||||
0x00, //reserve2
|
||||
0x00, //reserve3
|
||||
|
|
@ -137,7 +145,7 @@ extern volatile int sts_tof_distance_data[MAX_TOF_COUNT];
|
|||
volatile sts_tof_range_data_t sts_range_sensor_data={0x0,0x0,0x0};
|
||||
#endif
|
||||
#if defined(STS_P2)||defined(STS_T6)
|
||||
volatile uint8_t sts_door_jam_profile = 0;
|
||||
volatile uint8_t sts_door_jam_profile = DOOR_JAM_2400;
|
||||
#endif
|
||||
#ifdef STS_R4
|
||||
volatile uint8_t sts_soap_level_state=0;
|
||||
|
|
@ -647,6 +655,22 @@ void USER_APP_Parse_CMD_P(uint8_t *parse_buffer, uint8_t parse_buffer_size)
|
|||
break;
|
||||
case 12:
|
||||
break;
|
||||
case 13:
|
||||
for (uint8_t i=0; i<10; i++) {
|
||||
sts_cfg_nvm.p[i] = (uint8_t)parse_buffer[CFG_CMD4+i];
|
||||
}
|
||||
|
||||
ppc_cfg[parse_buffer[CFG_CMD4]].distance_array_size = (uint8_t)parse_buffer[CFG_CMD4+1];
|
||||
ppc_cfg[parse_buffer[CFG_CMD4]].max_distance = (uint8_t)parse_buffer[CFG_CMD4+2];
|
||||
ppc_cfg[parse_buffer[CFG_CMD4]].min_distance = (uint8_t)parse_buffer[CFG_CMD4+3];
|
||||
ppc_cfg[parse_buffer[CFG_CMD4]].dist_threshold = (uint8_t)parse_buffer[CFG_CMD4+4];
|
||||
ppc_cfg[parse_buffer[CFG_CMD4]].rows_of_SPADS = (uint8_t)parse_buffer[CFG_CMD4+5];
|
||||
ppc_cfg[parse_buffer[CFG_CMD4]].timing_budget = (uint8_t)parse_buffer[CFG_CMD4+6];
|
||||
ppc_cfg[parse_buffer[CFG_CMD4]].distance_mode = (uint8_t)parse_buffer[CFG_CMD4+7];
|
||||
ppc_cfg[parse_buffer[CFG_CMD4]].front_zone_center = (uint8_t)parse_buffer[CFG_CMD4+8];
|
||||
ppc_cfg[parse_buffer[CFG_CMD4]].back_zone_center = (uint8_t)parse_buffer[CFG_CMD4+9];
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
@ -1041,6 +1065,7 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s
|
|||
//OnSystemReset();
|
||||
break;
|
||||
case 'S': /* "YZS": Self Function Testing */
|
||||
|
||||
STS_SENSOR_Function_Test_Process();
|
||||
|
||||
break;
|
||||
|
|
@ -1067,20 +1092,23 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s
|
|||
outbuf[i++] = (uint8_t)sts_version;
|
||||
outbuf[i++] = (uint8_t)sts_hardware_ver;
|
||||
outbuf[i++] = (uint8_t)(99*((GetBatteryLevel()/254)&0xff));
|
||||
outbuf[i++] = (uint8_t)(4); //four bytes for distance
|
||||
outbuf[i++] = (uint8_t)(2); //two bytes for distance
|
||||
#if defined(VL53LX)||defined(VL53L0)
|
||||
#if 0
|
||||
outbuf[i++] = (uint8_t)(sts_sensor_install_height/1000+0x30)&0xff;
|
||||
outbuf[i++] = (uint8_t)((sts_sensor_install_height/100)%10+0x30)&0xff;
|
||||
outbuf[i++] = (uint8_t)((sts_sensor_install_height/10)%10+0x30)&0xff;
|
||||
outbuf[i++] = (uint8_t)((sts_sensor_install_height/1)%10+0x30)&0xff;
|
||||
//outbuf[i++] = (uint8_t)(sts_sensor_install_height>>8)&0xFF;
|
||||
//outbuf[i++] = (uint8_t)(sts_sensor_install_height)&0xFF;
|
||||
#endif
|
||||
outbuf[i++] = (uint8_t)(sts_sensor_install_height>>8)&0xFF;
|
||||
outbuf[i++] = (uint8_t)(sts_sensor_install_height)&0xFF;
|
||||
#endif
|
||||
|
||||
STS_SENSOR_Upload_Message(YUNHORN_STS_USER_APP_CTRL_REPLY_PORT, i, (uint8_t *)outbuf);
|
||||
break;
|
||||
case 'M': /* "YZM": Mask level */
|
||||
i = 0;
|
||||
outbuf[i++] = (uint8_t)'Y';
|
||||
outbuf[i++] = (uint8_t)'M';
|
||||
outbuf[i++] = (uint8_t)sts_service_mask;
|
||||
outbuf[i++] = (uint8_t)sts_work_mode;
|
||||
|
|
@ -1166,6 +1194,17 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s
|
|||
STS_SENSOR_Upload_Message(YUNHORN_STS_USER_APP_CTRL_REPLY_PORT, i, (uint8_t *)outbuf);
|
||||
|
||||
break;
|
||||
case 'M':
|
||||
uint8_t log_in_nvm[YUNHORN_STS_MAX_NVM_CFG_SIZE]={0x0};
|
||||
OnRestoreSTSLOGContextRequest((void*)&log_in_nvm);
|
||||
i=0;
|
||||
outbuf[0] = 0x4D; // 'M' for MEM IN FLASH
|
||||
UTIL_MEM_cpy_8(outbuf+1, log_in_nvm,32);
|
||||
i=33;
|
||||
STS_SENSOR_Upload_Message(YUNHORN_STS_USER_APP_CTRL_REPLY_PORT, i, (uint8_t *)outbuf);
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
|
@ -1249,8 +1288,12 @@ void USER_APP_AUTO_RESPONDER_Parse(uint8_t *parse_buffer, uint8_t parse_buffer_s
|
|||
APP_LOG(TS_OFF,VLEVEL_M, "\nSTS Tx Periodicty changed to %u sec\n\r", STS_TxPeriod_sec);
|
||||
#ifdef STS_R4
|
||||
OnYunhornSTSTxPeriodicityChanged(STS_TxPeriod_sec*1000);
|
||||
#elif defined(STS_R1D)||defined(STS_R1)||defined(STS_R5)||defined(STS_P2)||defined(STS_O6T)||defined(STS_R2)||defined(STS_R3)||defined(STS_R6)||defined(STS_O5)||defined(STS_M1)
|
||||
#elif defined(STS_R1D)||defined(STS_R1)||defined(STS_R5)||defined(STS_O6T)||defined(STS_R2)||defined(STS_R3)||defined(STS_R6)||defined(STS_O5)||defined(STS_M1)
|
||||
OnYunhornSTSTxPeriodicityChanged(STS_TxPeriod_sec*1000);
|
||||
#elif defined(STS_P2)
|
||||
OnYunhornSTSTxPeriodicityChanged(STS_TxPeriod_sec*1000);
|
||||
sts_people_count_sensor_data->Count_Period = (uint8_t)((tlv_buf[CFG_CMD3]-0x30)*10+ (tlv_buf[CFG_CMD4]-0x30));
|
||||
sts_people_count_sensor_data->Count_Period_Unit = (uint8_t)tlv_buf[CFG_CMD5];
|
||||
#elif defined(STS_O6)||defined(STS_O7)
|
||||
OnYunhornSTSHeartBeatPeriodicityChanged(periodicity_length);
|
||||
#endif
|
||||
|
|
@ -1548,7 +1591,7 @@ void OnStoreSTSCFGContextRequest(void)
|
|||
/* USER CODE END OnStoreContextRequest_Last */
|
||||
}
|
||||
|
||||
void OnRestoreSTSCFGContextRequest(uint8_t *cfg_in_nvm)
|
||||
void OnRestoreSTSCFGContextRequest(void *cfg_in_nvm)
|
||||
{
|
||||
/* USER CODE BEGIN OnRestoreSTSCFGContextRequest_1 */
|
||||
|
||||
|
|
@ -1559,6 +1602,16 @@ void OnRestoreSTSCFGContextRequest(uint8_t *cfg_in_nvm)
|
|||
/* USER CODE END OnRestoreSTSCFGContextRequest_Last */
|
||||
}
|
||||
|
||||
void OnRestoreSTSLOGContextRequest(void *log_in_nvm)
|
||||
{
|
||||
/* USER CODE BEGIN OnRestoreSTSCFGContextRequest_1 */
|
||||
|
||||
/* USER CODE END OnRestoreSTSCFGContextRequest_1 */
|
||||
UTIL_MEM_cpy_8((void*)log_in_nvm, (void *)STS_SENSOR_DATA_NVM_BASE_ADDRESS, YUNHORN_STS_MAX_NVM_CFG_SIZE);
|
||||
/* USER CODE BEGIN OnRestoreSTSCFGContextRequest_Last */
|
||||
|
||||
/* USER CODE END OnRestoreSTSCFGContextRequest_Last */
|
||||
}
|
||||
|
||||
void STS_REBOOT_CONFIG_Init(void)
|
||||
{
|
||||
|
|
@ -1667,7 +1720,10 @@ void OnRestoreSTSCFGContextProcess(void)
|
|||
//sampling_heartbeat_periodicity = (sampling_heartbeat_periodicity > 0)? sampling_heartbeat_periodicity : 1; // in seconds unit
|
||||
//sampling_heartbeat_periodicity = sampling_heartbeat_periodicity*1000;
|
||||
#ifdef STS_P2
|
||||
STS_TxPeriod_sec = periodicity;
|
||||
STS_HeartBeatTimerPeriod_sec = sampling_heartbeat_periodicity;
|
||||
sts_people_count_sensor_data->Count_Period = sts_cfg_nvm.periodicity;
|
||||
sts_people_count_sensor_data->Count_Period_Unit = sts_cfg_nvm.unit;
|
||||
#endif
|
||||
|
||||
|
||||
|
|
@ -1732,8 +1788,12 @@ void STS_SENSOR_Distance_Test_Process(void)
|
|||
#if defined(VL53LX)||defined(L8)
|
||||
//MX_TOF_Init();
|
||||
//MX_TOF_Process();
|
||||
sts_sensor_install_height = (uint16_t)MX_TOF_Ranging_Process();
|
||||
int status=0;
|
||||
uint16_t range_distance =0;
|
||||
status = sts_tof_vl53lx_range_distance(&range_distance);
|
||||
sts_sensor_install_height = range_distance;
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\n STS SENSOR INSTALLATION HEIGHT =%d mm\n\r", (uint16_t)sts_sensor_install_height);
|
||||
|
||||
#endif
|
||||
#if defined(VL53L0)
|
||||
STS_TOF_VL53L0X_Range_Process();
|
||||
|
|
@ -1762,14 +1822,20 @@ void STS_SENSOR_Function_Test_Process(void)
|
|||
tstbuf[i++] = (uint8_t) (99*GetBatteryLevel()/254)&0xff;
|
||||
|
||||
#if (defined(STS_P2)||defined(STS_T6))
|
||||
STS_SENSOR_MEMS_Get_ID(&sensor_id);
|
||||
//STS_SENSOR_MEMS_Get_ID(&sensor_id);
|
||||
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\nSensor_id=%04x\r\n", sensor_id);
|
||||
|
||||
if (((sensor_id & 0xff)!= 0xCC) && (((sensor_id >>8) & 0xFF)!=0xEA)) // no VL53L1X found
|
||||
{
|
||||
tstbuf[i++] = (uint8_t) 'X'; // Slave MEMS Not Avaliable
|
||||
} else
|
||||
{
|
||||
STS_SENSOR_Distance_Test_Process();
|
||||
//STS_SENSOR_Distance_Test_Process();
|
||||
int status=0;
|
||||
uint16_t range_distance =0;
|
||||
status = sts_tof_vl53lx_range_distance(&range_distance);
|
||||
sts_sensor_install_height = range_distance;
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\nSensor Install Height =%4d mm\n", sts_sensor_install_height);
|
||||
tstbuf[i++] = (uint8_t) (2)&0xff; //length of following data
|
||||
tstbuf[i++] = (uint8_t) (sts_sensor_install_height>>8)&0xff; // MSB of sensor height
|
||||
|
|
@ -1781,17 +1847,22 @@ void STS_SENSOR_Function_Test_Process(void)
|
|||
|
||||
if ((sts_sensor_install_height >= 1500)&&(sts_sensor_install_height<=4000))
|
||||
{
|
||||
sts_door_jam_profile = DOOR_JAM_2000; // DOOR_JAM_2000: assume door_jame less 2000mm
|
||||
sts_door_jam_profile = DOOR_JAM_1600; // assume door_jame less 1600mm profile =1
|
||||
|
||||
if (sts_sensor_install_height >= 1600)
|
||||
sts_door_jam_profile ++; //DOOR_JAM_2400, profile =2
|
||||
|
||||
if (sts_sensor_install_height >= 2000)
|
||||
sts_door_jam_profile ++; //DOOR_JAM_2400
|
||||
if (sts_sensor_install_height >= 2400)
|
||||
sts_door_jam_profile ++; //DOOR_JAM_3000
|
||||
sts_door_jam_profile ++; //DOOR_JAM_3000, profile =3
|
||||
|
||||
if (sts_sensor_install_height >= 3000)
|
||||
sts_door_jam_profile ++; //DOOR_JAM_3500
|
||||
sts_door_jam_profile ++; //DOOR_JAM_3500, profile = 4
|
||||
|
||||
if (sts_sensor_install_height >= 3500)
|
||||
sts_door_jam_profile ++; //DOOR_JAM_4000
|
||||
sts_door_jam_profile ++; //DOOR_JAM_4000, profile = 5
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\r\nDOOR JAM PROFILE=%d \r\n", sts_door_jam_profile);
|
||||
}
|
||||
|
||||
#endif
|
||||
#ifdef YUNHORN_STS_O6_ENABLED
|
||||
tstbuf[i++] = (uint8_t)20; //length of following data
|
||||
|
|
@ -1817,8 +1888,8 @@ void STS_SENSOR_Function_Test_Process(void)
|
|||
#endif
|
||||
//MX_TOF_Process();
|
||||
#ifdef STS_P2
|
||||
MX_TOF_Init();
|
||||
MX_TOF_Ranging_Process();
|
||||
//MX_TOF_Init();
|
||||
//MX_TOF_Ranging_Process();
|
||||
#endif
|
||||
#ifdef VL53L0
|
||||
PME_ON;
|
||||
|
|
|
|||
|
|
@ -134,7 +134,7 @@ uint16_t MX_TOF_Ranging_Process(void)
|
|||
|
||||
#if (defined(STS_P2)||defined(STS_T6))
|
||||
uint16_t range_distance=0;
|
||||
uint8_t range_mode = 2; //STS_TOF_LONG_RANGE;
|
||||
uint8_t range_mode = STS_TOF_LONG_RANGE; //STS_TOF_LONG_RANGE;
|
||||
STS_TOF_VL53LX_Range_Process(range_mode, &range_distance);
|
||||
APP_LOG(TS_OFF, VLEVEL_M, "\n VL53L1 Range distance =%u mm \n\r", (uint16_t)range_distance);
|
||||
|
||||
|
|
@ -483,10 +483,13 @@ void BSP_PB_Callback(Button_TypeDef Button)
|
|||
uint8_t sts_vl53lx_ranging(uint16_t *ranged_distance, uint8_t range_mode, uint16_t distance_threshold_mm, uint16_t inter_measurement_ms, uint16_t macro_timing,
|
||||
uint16_t roi_width, uint16_t sigma_mm, uint16_t signal_kcps)
|
||||
{
|
||||
uint8_t status=0, dev=0x52;
|
||||
uint16_t estimated_distance_mm=0;
|
||||
#if 0
|
||||
uint8_t status=0, dev=0x52;
|
||||
uint16_t estimated_distance_mm=0;
|
||||
status = VL53L1X_SensorInit(dev);
|
||||
status += VL53L1X_SetDistanceMode(dev, 2); /* 1=short, 2=long, DISTANCE_MODE */
|
||||
status += VL53L1X_SetDistanceMode(dev, range_mode); /* 1=short, 2=long, DISTANCE_MODE */
|
||||
status += VL53L1X_SetTimingBudgetInMs(dev, 100); /* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */
|
||||
status += VL53L1X_SetInterMeasurementInMs(dev, 50);
|
||||
status += VL53L1X_SetROI(dev, 8, 8); /* minimum ROI 4,4 */
|
||||
|
|
@ -499,8 +502,9 @@ uint16_t roi_width, uint16_t sigma_mm, uint16_t signal_kcps)
|
|||
APP_LOG(TS_OFF, VLEVEL_L,"Start Range failed\n");
|
||||
return (-1);
|
||||
}
|
||||
|
||||
#endif
|
||||
status = VL53L1X_GetDistance(dev, &estimated_distance_mm);
|
||||
status = VL53L1X_ClearInterrupt(dev); /* clear interrupt has to be called to enable next interrupt*/
|
||||
APP_LOG(TS_OFF,VLEVEL_M,"Target detected! Distance =%d mm \r\n", estimated_distance_mm );
|
||||
*ranged_distance = estimated_distance_mm;
|
||||
status = VL53L1X_StopRanging(dev);
|
||||
|
|
|
|||
|
|
@ -3,7 +3,10 @@
|
|||
#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"
|
||||
#ifdef STS_P2
|
||||
#include "VL53L1X_API.h"
|
||||
#include "VL53l1X_calibration.h"
|
||||
|
|
@ -17,17 +20,20 @@
|
|||
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;
|
||||
|
||||
/* TIMING_BUDGET, in ms possible values [15, 20, 50, 100, 200, 500] */
|
||||
/* DISTANCE_THRESHOLD = MAX - ASSUME_CHILD_HEIGHT(800mm) */
|
||||
// door jam profile
|
||||
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},
|
||||
{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,0, 1600,8,33, DISTANCE_MODE_LONG,175,231},
|
||||
{DOOR_JAM_3000, 10, 3000,0, 2200,8,100,DISTANCE_MODE_LONG,175,231},
|
||||
{DOOR_JAM_3500, 10, 3500,0, 2700,8,100,DISTANCE_MODE_LONG,175,231},
|
||||
{DOOR_JAM_4000, 10, 4000,0, 3200,8,200,DISTANCE_MODE_LONG,175,231},
|
||||
};
|
||||
volatile sts_zone_center_by_rows_of_spads_t zone_center[4]={
|
||||
{4, 151, 247},
|
||||
|
|
@ -39,12 +45,12 @@ volatile sts_zone_center_by_rows_of_spads_t zone_center[4]={
|
|||
extern volatile uint8_t sts_door_jam_profile;
|
||||
volatile uint8_t sts_presence_state=0;
|
||||
static int PresenceState=0, PrevPresenceState=0;
|
||||
volatile uint8_t sts_distance_mode = DOOR_JAM_2400; //sts_door_jam_profile; //DOOR_JAM_3000;
|
||||
volatile uint8_t sts_distance_mode = DOOR_JAM_2400; //DOOR_JAM_2400; //sts_door_jam_profile; //DOOR_JAM_3000;
|
||||
#endif
|
||||
#if (defined(STS_P2))
|
||||
volatile uint8_t sts_people_count_number_changed = 0;
|
||||
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;
|
||||
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;
|
||||
|
|
@ -53,7 +59,8 @@ 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 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
|
||||
|
|
@ -117,12 +124,13 @@ int ProcessPresenceDetectionData(int16_t Distance, uint8_t zone, uint8_t RangeSt
|
|||
MinDistance = Distances[zone][i];
|
||||
}
|
||||
}
|
||||
|
||||
//APP_LOG(TS_OFF, VLEVEL_M, "\r\nMin Distance =%d ", MinDistance);
|
||||
if (MinDistance < ppc_cfg[sts_distance_mode].dist_threshold) {
|
||||
// Someone is in !
|
||||
CurrentZoneStatus = SOMEONE;
|
||||
//APP_LOG(TS_OFF, VLEVEL_M, "---> Someone is in");
|
||||
}
|
||||
|
||||
//APP_LOG(TS_OFF, VLEVEL_M, "\r\n");
|
||||
//return CurrentZoneStatus;
|
||||
#if 1
|
||||
// left zone
|
||||
|
|
@ -257,14 +265,14 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu
|
|||
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) {
|
||||
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_distance_mode].distance_array_size; i++)
|
||||
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_distance_mode].distance_array_size-1] = Distance;
|
||||
Distances[zone][ppc_cfg[sts_door_jam_profile].distance_array_size-1] = Distance;
|
||||
}
|
||||
|
||||
// pick up the min distance
|
||||
|
|
@ -275,11 +283,13 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu
|
|||
MinDistance = Distances[zone][i];
|
||||
}
|
||||
}
|
||||
|
||||
if (MinDistance < ppc_cfg[sts_distance_mode].dist_threshold) {
|
||||
//APP_LOG(TS_OFF, VLEVEL_M, "\r\n MinDistance & dist_threshold ---%d %d", MinDistance,ppc_cfg[sts_distance_mode].dist_threshold);
|
||||
if (MinDistance < ppc_cfg[sts_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) {
|
||||
|
|
@ -352,6 +362,9 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu
|
|||
APP_LOG(TS_OFF, VLEVEL_M,"Walk In, People Count=%d\n", PeopleCount);
|
||||
sts_people_count_sensor_data.Walk_In_People_Count ++;
|
||||
sts_people_count_number_changed = 1;
|
||||
//LED1_ON; //TODO XXX, CHANGE TO (LED_IN_ON && LED_OUT_OFF);
|
||||
LED1_TOGGLE;
|
||||
|
||||
} else if ((PathTrack[1] == 2) && (PathTrack[2] == 3) && (PathTrack[3] == 1)) {
|
||||
// This an exit
|
||||
PeopleCount --;
|
||||
|
|
@ -360,11 +373,14 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu
|
|||
DistancesTableSize[1] = 0;
|
||||
APP_LOG(TS_OFF, VLEVEL_M,"Walk Out, People Count=%d\n", PeopleCount);
|
||||
sts_people_count_sensor_data.Walk_Out_People_Count ++;
|
||||
//LED1_ON; //TODO XXX, CHANGE TO (LED_IN_OFF && LED_OUT_ON);
|
||||
LED1_TOGGLE;
|
||||
sts_people_count_number_changed = 1;
|
||||
} else {
|
||||
// reset the table filling size also in case of unexpected path
|
||||
DistancesTableSize[0] = 0;
|
||||
DistancesTableSize[1] = 0;
|
||||
|
||||
APP_LOG(TS_OFF, VLEVEL_M,"Walk Around ie. Wrong path\n");
|
||||
sts_people_count_sensor_data.Walk_Around_People_Count ++;
|
||||
}
|
||||
|
|
@ -394,6 +410,7 @@ int ProcessPeopleCountingData(int16_t Distance, uint8_t zone, uint8_t RangeStatu
|
|||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
//printf ("Inside PeopleCount = %d , returned\n", PeopleCount);
|
||||
// output debug data to main host machine
|
||||
return(PeopleCount);
|
||||
|
|
@ -412,8 +429,8 @@ void STS_people_count_sensor_Read(sts_people_count_sensor_data_t *sts_p2_sensor_
|
|||
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;
|
||||
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 ---
|
||||
|
|
@ -489,40 +506,80 @@ void STS_people_count_sensor_Read(sts_people_count_sensor_data_t *sts_p2_sensor_
|
|||
sts_people_count_sensor_data.Walk_Around_People_Count = 0;
|
||||
}
|
||||
|
||||
if (cnt_today ==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;
|
||||
|
||||
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;
|
||||
}
|
||||
if (cnt_week)
|
||||
// reset counter at end of week TODO XXX
|
||||
if (last_week != this_week) {
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// reset counter at end of month TODO XXX
|
||||
if (last_month != this_month) {
|
||||
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_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;
|
||||
}
|
||||
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;
|
||||
|
||||
if (last_year != this_year) {
|
||||
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);
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -534,8 +591,31 @@ void STS_people_count_sensor_Read(sts_people_count_sensor_data_t *sts_p2_sensor_
|
|||
|
||||
void STS_TOF_VL53LX_PeopleCounting_Process_Init(void)
|
||||
{
|
||||
APP_LOG(TS_OFF, VLEVEL_M,"\n\n# YUNHORN STS-P2 PEOPLE COUNTING PROCESS #\r\n");
|
||||
APP_LOG(TS_OFF, VLEVEL_H,"\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[2];
|
||||
sts_people_count_sensor_data.Sum_Day_Walk_Around_People_Count = (uint16_t)log_in_nvm[4];
|
||||
|
||||
sts_people_count_sensor_data.Sum_Week_Walk_In_People_Count = (uint16_t)log_in_nvm[6];
|
||||
sts_people_count_sensor_data.Sum_Week_Walk_Out_People_Count = (uint16_t)log_in_nvm[8];
|
||||
sts_people_count_sensor_data.Sum_Week_Walk_Around_People_Count = (uint16_t)log_in_nvm[10];
|
||||
|
||||
sts_people_count_sensor_data.Sum_Month_Walk_In_People_Count = (uint16_t)log_in_nvm[12];
|
||||
sts_people_count_sensor_data.Sum_Month_Walk_Out_People_Count = (uint16_t)log_in_nvm[14];
|
||||
sts_people_count_sensor_data.Sum_Month_Walk_Around_People_Count = (uint16_t)log_in_nvm[16];
|
||||
|
||||
sts_people_count_sensor_data.Sum_Year_Walk_In_People_Count = (uint16_t)log_in_nvm[18];
|
||||
sts_people_count_sensor_data.Sum_Year_Walk_Out_People_Count = (uint16_t)log_in_nvm[20];
|
||||
sts_people_count_sensor_data.Sum_Year_Walk_Around_People_Count = (uint16_t)log_in_nvm[22];
|
||||
|
||||
sts_people_count_sensor_data.Sum_LifeCycle_Walk_In_People_Count = (uint16_t)log_in_nvm[24];
|
||||
sts_people_count_sensor_data.Sum_LifeCycle_Walk_Out_People_Count = (uint16_t)log_in_nvm[26];
|
||||
sts_people_count_sensor_data.Sum_LifeCycle_Walk_Around_People_Count = (uint16_t)log_in_nvm[28];
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -692,17 +772,25 @@ int sts_tof_vl53lx_peoplecount_init(void)
|
|||
|
||||
/* 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;
|
||||
|
||||
sts_distance_mode = (sts_door_jam_profile ==0)?DISTANCE_MODE_SHORT:DISTANCE_MODE_LONG;
|
||||
APP_LOG(TS_OFF, VLEVEL_H, "\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 */
|
||||
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_H,"\n\nInit finished...\r\nStart counting people with profile : %d \r\n\n\n\n", sts_distance_mode);
|
||||
APP_LOG(TS_OFF, VLEVEL_H,"\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) {
|
||||
|
|
@ -754,19 +842,47 @@ int sts_tof_vl53lx_peoplecount_start(void)
|
|||
// 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;
|
||||
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_distance_mode].max_distance; //MAX_DISTANCE;
|
||||
Distance = ppc_cfg[sts_door_jam_profile].max_distance; //MAX_DISTANCE;
|
||||
// inject the new ranged distance in the people counting algorithm
|
||||
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,"%d,%d,%d\n", Zone, Distance, Signal);
|
||||
//APP_LOG(TS_OFF, VLEVEL_M,"Zone, Distance, Signal = %d,%d,%d\n", Zone, Distance, Signal);
|
||||
Zone++;
|
||||
Zone = Zone%2;
|
||||
|
||||
return 0;
|
||||
}
|
||||
int sts_tof_vl53lx_range_distance(uint16_t *range_distance)
|
||||
{
|
||||
|
||||
uint16_t wordData=0, Distance=0;
|
||||
//uint8_t RangeStatus;
|
||||
uint8_t dataReady=0, status=0, dev=0x52;
|
||||
|
||||
/* read and display data */
|
||||
|
||||
while (dataReady == 0) {
|
||||
status = VL53L1X_CheckForDataReady(dev, &dataReady);
|
||||
HAL_Delay(1);
|
||||
wordData++;
|
||||
if (wordData > 30) return -1; // 50 ms timer, so make this 60% to fail back
|
||||
}
|
||||
dataReady = 0;
|
||||
//status += VL53L1X_GetRangeStatus(dev, &RangeStatus);
|
||||
status += VL53L1X_GetDistance(dev, &Distance);
|
||||
status += VL53L1X_ClearInterrupt(dev); /* clear interrupt has to be called to enable next interrupt*/
|
||||
if (status != 0) {
|
||||
APP_LOG(TS_OFF, VLEVEL_L,"Error in operating the device\n");
|
||||
return (-1);
|
||||
}
|
||||
APP_LOG(TS_OFF, VLEVEL_M,"\r\nVL53L1X RANGE DISTANCE =%d \r\n", Distance);
|
||||
*range_distance = Distance;
|
||||
return status;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -37,12 +37,13 @@
|
|||
#define LEFT 0
|
||||
#define RIGHT 1
|
||||
|
||||
#define DOOR_JAM_2000 0
|
||||
#define DOOR_JAM_2400 1
|
||||
#define DOOR_JAM_3000 2
|
||||
#define DOOR_JAM_3500 3
|
||||
#define DOOR_JAM_4000 4
|
||||
#define DOOR_SIDE_1600 5
|
||||
#define DOOR_SIDE_1300 0
|
||||
#define DOOR_JAM_1600 1
|
||||
#define DOOR_JAM_2400 2
|
||||
#define DOOR_JAM_3000 3
|
||||
#define DOOR_JAM_3500 4
|
||||
#define DOOR_JAM_4000 5
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -78,7 +79,7 @@
|
|||
#define MAX_DISTANCE 2000 // mm was 2400
|
||||
#define MIN_DISTANCE 0 // mm
|
||||
#define DIST_THRESHOLD 1600 // mm
|
||||
#define ROWS_OF_SPADS 16 // 8x16 SPADs ROI
|
||||
#define ROWS_OF_SPADS 8 // 8x16 SPADs ROI
|
||||
#define TIMING_BUDGET 33 //33 // was 20 ms, I found 33 ms has better succes rate with lower reflectance target
|
||||
#define DISTANCE_MODE DISTANCE_MODE_LONG
|
||||
#endif
|
||||
|
|
@ -92,7 +93,7 @@
|
|||
#elif ROWS_OF_SPADS == 8
|
||||
#define FRONT_ZONE_CENTER 175 // was 167, see UM2555 on st.com, centre = 175 has better return signal rate for the ROI #1
|
||||
#define BACK_ZONE_CENTER 231
|
||||
#elif ROWS_OF_SPADS == 16
|
||||
#elif ROWS_OF_SPADS == 10
|
||||
#define FRONT_ZONE_CENTER 199
|
||||
#define BACK_ZONE_CENTER 199
|
||||
#endif
|
||||
|
|
@ -172,6 +173,7 @@ int sts_tof_vl53lx_presence_detection_start(void);
|
|||
|
||||
int sts_tof_vl53lx_peoplecount_init(void);
|
||||
int sts_tof_vl53lx_peoplecount_start(void);
|
||||
int sts_tof_vl53lx_range_distance(uint16_t *range_distance);
|
||||
|
||||
//int sts_tof_vl53lx_peoplecount(void);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue