298 lines
9.2 KiB
C
298 lines
9.2 KiB
C
/**
|
|
******************************************************************************
|
|
* @file : app_tof.c
|
|
* @author : IMG SW Application Team
|
|
* @brief : This file provides code for the configuration
|
|
* of the STMicroelectronics.X-CUBE-TOF1.3.2.0 instances.
|
|
******************************************************************************
|
|
*
|
|
* @attention
|
|
*
|
|
* Copyright (c) 2022 STMicroelectronics.
|
|
* All rights reserved.
|
|
*
|
|
* This software is licensed under terms that can be found in the LICENSE file
|
|
* in the root directory of this software component.
|
|
* If no LICENSE file comes with this software, it is provided AS-IS.
|
|
*
|
|
******************************************************************************
|
|
*/
|
|
|
|
#ifdef __cplusplus
|
|
extern "C" {
|
|
#endif
|
|
|
|
/* Includes ------------------------------------------------------------------*/
|
|
#include "app_tof.h"
|
|
#include "main.h"
|
|
#include "sys_app.h"
|
|
#include <stdio.h>
|
|
|
|
#include "VL53L1X_ULP_api.h"
|
|
//#include "53l1a2_ranging_sensor.h"
|
|
#include "stm32wlxx_nucleo.h"
|
|
|
|
/* Private typedef -----------------------------------------------------------*/
|
|
|
|
/* Private define ------------------------------------------------------------*/
|
|
#define TIMING_BUDGET (30U) /* 16 ms < TimingBudget < 500 ms */
|
|
#define POLLING_PERIOD (250U) /* refresh rate for polling mode (milliseconds, shall be consistent with TimingBudget value) */
|
|
|
|
/* Private variables ---------------------------------------------------------*/
|
|
//static RANGING_SENSOR_Capabilities_t Cap;
|
|
//static RANGING_SENSOR_ProfileConfig_t Profile;
|
|
//static int32_t status = 0;
|
|
|
|
volatile uint8_t ToF_EventDetected = 0;
|
|
|
|
|
|
/* Private function prototypes -----------------------------------------------*/
|
|
|
|
int sts_vl53lx_ranging(uint8_t vl_model, 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 IsInterruptDetected(uint16_t dev);
|
|
|
|
void MX_TOF_Init(void)
|
|
{
|
|
/* USER CODE BEGIN SV */
|
|
|
|
/* USER CODE END SV */
|
|
|
|
/* USER CODE BEGIN TOF_Init_PreTreatment */
|
|
|
|
/* USER CODE END TOF_Init_PreTreatment */
|
|
|
|
/* Initialize the peripherals and the TOF components */
|
|
APP_LOG(TS_OFF,VLEVEL_L,"\r\n###################### MX TOF Init... \r\n");
|
|
|
|
//MX_53L1A2_SimpleRanging_Init();
|
|
STS_TOF_VL53LX_PeopleCounting_Process();
|
|
/* USER CODE BEGIN TOF_Init_PostTreatment */
|
|
|
|
/* USER CODE END TOF_Init_PostTreatment */
|
|
}
|
|
|
|
/*
|
|
* LM background task
|
|
*/
|
|
void MX_TOF_Process(void)
|
|
{
|
|
/* USER CODE BEGIN TOF_Process_PreTreatment */
|
|
|
|
/* USER CODE END TOF_Process_PreTreatment */
|
|
|
|
|
|
//APP_LOG(TS_OFF,VLEVEL_L,"\r\n###################### MX TOF Process... \r\n");
|
|
//STS_TOF_VL53LX_Range_Process();
|
|
//STS_TOF_VL53LX_PeopleCounting_Process();
|
|
|
|
sts_tof_vl53lx_peoplecount_subprocess();
|
|
|
|
/* USER CODE BEGIN TOF_Process_PostTreatment */
|
|
|
|
/* USER CODE END TOF_Process_PostTreatment */
|
|
}
|
|
typedef enum {
|
|
STS_TOF_SHORT_RANGE=0,
|
|
STS_TOF_LONG_RANGE,
|
|
STS_TOF_LOW_POWER_RANGE,
|
|
STS_TOF_PP_RANGE
|
|
} range_mode_t;
|
|
typedef enum {
|
|
STS_TOF_VL53L0X=0,
|
|
STS_TOF_VL53L1X,
|
|
STS_TOF_VL53L2X,
|
|
STS_TOF_VL53L3X,
|
|
STS_TOF_VL53L4X,
|
|
STS_TOF_VL53L5X,
|
|
STS_TOF_VL53L6X,
|
|
STS_TOF_VL53L7X
|
|
} vl53lx_model;
|
|
|
|
uint8_t IsInterruptDetected(uint16_t dev)
|
|
{
|
|
// To be filled with customer HW. This function should
|
|
// return 1 when an interrupt is raised by the ToF on GPIO1 pin (pin7)
|
|
if (ToF_EventDetected )
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_L,"############### TOF EVENT DETECTED \r\n");
|
|
ToF_EventDetected =0;
|
|
return 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
void STS_TOF_VL53LX_Range_Process(void)
|
|
{
|
|
uint8_t vl53lx_model = STS_TOF_VL53L1X;
|
|
uint8_t range_mode = STS_TOF_SHORT_RANGE;
|
|
|
|
uint16_t i_distance_threshold_mm = 800;
|
|
uint16_t i_inter_measurement_ms, i_macro_timing;
|
|
uint16_t i_roi_width=16, i_sigma_mm=30, i_signal_kcps=2000;
|
|
while (1)
|
|
{
|
|
|
|
|
|
switch (range_mode)
|
|
{
|
|
case STS_TOF_SHORT_RANGE:
|
|
// STS ---002 for short distance
|
|
/* Example for robust and short distance measurements. Max distance reduced
|
|
* but very low number of false-positives */
|
|
//status |= VL53L1X_ULP_SetSigmaThreshold(dev, 30);
|
|
//status |= VL53L1X_ULP_SetSignalThreshold(dev, 2000);
|
|
i_sigma_mm = 30; //increase this for longer distance, reduce for shorter distance
|
|
i_signal_kcps = 2000; // 1000- 6000 kcps
|
|
i_distance_threshold_mm = 900;
|
|
i_inter_measurement_ms = 100; // 100 - 1000 ms
|
|
i_macro_timing = 100;
|
|
//i_roi_width = 16;
|
|
|
|
break;
|
|
|
|
case STS_TOF_LONG_RANGE:
|
|
// STS --- 003 for long range
|
|
/* Relax some limits. Be careful, it can create false-positives !*/
|
|
//status |= VL53L1X_ULP_SetSigmaThreshold(dev, 60);
|
|
//status |= VL53L1X_ULP_SetSignalThreshold(dev, 1200);
|
|
i_sigma_mm = 85; // increase this for longer distance, reduce for short distance
|
|
i_signal_kcps = 1000; // 1000- 6000 kcps
|
|
i_distance_threshold_mm = 4000; //4000;
|
|
i_inter_measurement_ms = 200; // 100 - 1000 ms
|
|
i_macro_timing = 30; // 1 - 100 ms
|
|
i_roi_width = 8;
|
|
|
|
|
|
break;
|
|
|
|
case STS_TOF_LOW_POWER_RANGE:
|
|
// STS---001 for ultra low power
|
|
/* Reduce the macro timing to minimum. This is equivalent as reducing the integration time */
|
|
//status = VL53L1X_ULP_SetMacroTiming(dev, 1);
|
|
i_distance_threshold_mm = 4000;
|
|
i_inter_measurement_ms = 100; // 100 - 1000 ms
|
|
i_macro_timing = 1;
|
|
/* Reduce at maximum the SPADS */
|
|
//status = VL53L1X_ULP_SetROI(dev, 4);
|
|
i_roi_width = 4;
|
|
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
sts_vl53lx_ranging(vl53lx_model, range_mode, i_distance_threshold_mm, i_inter_measurement_ms,i_macro_timing, i_roi_width, i_sigma_mm, i_signal_kcps);
|
|
|
|
range_mode ++;
|
|
range_mode %= 3 ;
|
|
}
|
|
}
|
|
|
|
void BSP_PB_Callback(Button_TypeDef Button)
|
|
{
|
|
//PushButtonDetected = 1;
|
|
}
|
|
|
|
|
|
int sts_vl53lx_ranging(uint8_t vl_model, 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)
|
|
{
|
|
/*********************************/
|
|
/* VL53L1X ranging variables */
|
|
/*********************************/
|
|
uint8_t status, loop;
|
|
uint8_t dev;
|
|
uint16_t sensor_id;
|
|
uint8_t measurement_status;
|
|
uint16_t estimated_distance_mm, r_signal_kcps, r_sigma_mm, r_ambient_kcps;
|
|
/*********************************/
|
|
/* Customer platform */
|
|
/*********************************/
|
|
/* Default VL53L1X Ultra Low Power I2C address */
|
|
dev = 0x52;
|
|
|
|
/* (Optional) Change I2C address */
|
|
// status = VL53L1X_ULP_SetI2CAddress(dev, 0x52);
|
|
// dev = 0x20;
|
|
|
|
/*********************************/
|
|
/* Power on sensor and init */
|
|
/*********************************/
|
|
APP_LOG(TS_OFF,VLEVEL_L,"Range Mode =%d \r\n",range_mode);
|
|
/* (Optional) Check if there is a VL53L1X sensor connected */
|
|
status = VL53L1X_ULP_GetSensorId(dev, &sensor_id);
|
|
APP_LOG(TS_OFF,VLEVEL_L,"VL53L1X address =%X\r\n",sensor_id );
|
|
if(status || (sensor_id != 0xEACC))
|
|
{
|
|
APP_LOG(TS_OFF,VLEVEL_L,"VL53L1X not detected at requested address\n");
|
|
return status;
|
|
}
|
|
/* (Mandatory) Init VL53L1X sensor */
|
|
status = VL53L1X_ULP_SensorInit(dev);
|
|
if(status)
|
|
{
|
|
APP_LOG(TS_OFF,VLEVEL_L,"VL53L1X ultra low power Loading failed\n");
|
|
//HAL_Delay(100);
|
|
return status;
|
|
}
|
|
APP_LOG(TS_OFF,VLEVEL_L,"VL53L1X ultra low power ready ! \r\n");
|
|
/*********************************/
|
|
/* Sensor configuration */
|
|
/*********************************/
|
|
/* (Optional) Program sensor to raise an interrupt ONLY below 300mm */
|
|
status = VL53L1X_ULP_SetInterruptConfiguration(dev, distance_threshold_mm, 1); //i_distance_threshold_mm
|
|
/* (Optional) Program a 10Hz ranging frequency */
|
|
status = VL53L1X_ULP_SetInterMeasurementInMs(dev, inter_measurement_ms); // range_interval_ms
|
|
/* Increase the macro timing. This is equivalent as increasing the integration time */
|
|
status = VL53L1X_ULP_SetMacroTiming(dev, macro_timing); // micro_timing_ms
|
|
/* Enable all the SPADS */
|
|
status = VL53L1X_ULP_SetROI(dev, roi_width); // SPADS { 1 -- 16 }
|
|
|
|
if(range_mode != STS_TOF_LOW_POWER_RANGE)
|
|
{
|
|
/* Example for robust and short distance measurements. Max distance reduced
|
|
* but very low number of false-positives */
|
|
status |= VL53L1X_ULP_SetSigmaThreshold(dev, sigma_mm);
|
|
status |= VL53L1X_ULP_SetSignalThreshold(dev, signal_kcps);
|
|
}
|
|
|
|
/*********************************/
|
|
/* Ranging loop */
|
|
/*********************************/
|
|
status = VL53L1X_ULP_StartRanging(dev);
|
|
if(status)
|
|
{
|
|
APP_LOG(TS_OFF,VLEVEL_L,"VL53L1X_ULP_StartRanging failed with status %u\n", status);
|
|
return status;
|
|
}
|
|
APP_LOG(TS_OFF,VLEVEL_L,"Ranging started. Put your hand close to the sensor to generate an interrupt...\n");
|
|
loop = 0;
|
|
while(loop < 20)
|
|
{
|
|
/* Use this external function to detect when a hardware interrupt is generated on PIN 7 (GPIO1). It means that a new measurement is ready.
|
|
*/
|
|
|
|
if(IsInterruptDetected(dev))
|
|
{
|
|
/* (Mandatory) Clear HW interrupt to restart measurements */
|
|
VL53L1X_ULP_ClearInterrupt(dev);
|
|
/* Dump debug data */
|
|
status = VL53L1X_ULP_DumpDebugData(dev, &measurement_status,
|
|
&estimated_distance_mm, &r_sigma_mm, &r_signal_kcps, &r_ambient_kcps);
|
|
|
|
APP_LOG(TS_OFF,VLEVEL_L,"Target detected! Interrupt raised by sensor, Distance =%d mm \r\n", estimated_distance_mm );
|
|
loop++;
|
|
}
|
|
}
|
|
|
|
status = VL53L1X_ULP_StopRanging(dev);
|
|
APP_LOG(TS_OFF,VLEVEL_L,"End of VL53L1X ultra low power demo\n");
|
|
return status;
|
|
}
|
|
|
|
|
|
#ifdef __cplusplus
|
|
}
|
|
#endif
|