448 lines
14 KiB
C
448 lines
14 KiB
C
/* USER CODE BEGIN Header */
|
|
/**
|
|
******************************************************************************
|
|
* @file app_tof_vl53l0x_range.c
|
|
* @author Yunhorn (r) Technology Limited Application Team
|
|
* @brief Application of the LRWAN Middleware
|
|
******************************************************************************
|
|
* @attention
|
|
*
|
|
* Copyright (c) 2023 Yunhorn Technology Limited.
|
|
* Copyright (c) 2023 Shenzhen Yunhorn Technology Co., Ltd.
|
|
* 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.
|
|
*
|
|
******************************************************************************
|
|
*/
|
|
|
|
/* Includes ------------------------------------------------------------------*/
|
|
#include "stm32xxx_hal.h"
|
|
|
|
/* USER CODE BEGIN Includes */
|
|
#include <string.h>
|
|
#include "X-WL55_WLE5_53L0X.h"
|
|
#include "main.h"
|
|
#include "vl53l0x_api.h"
|
|
#include <limits.h>
|
|
#include "sys_app.h"
|
|
|
|
/**
|
|
* @defgroup Configuration Static configuration
|
|
* @{
|
|
*/
|
|
|
|
/** @} */ /* config group */
|
|
|
|
#ifndef MIN
|
|
# define MIN(a,b) ((a) < (b) ? (a) : (b))
|
|
#endif
|
|
|
|
/**
|
|
* @defgroup ErrCode Errors code shown on display
|
|
* @{
|
|
*/
|
|
#define STS_MAX_L0_RANGE ((uint32_t) 1000)
|
|
#define ERR_DETECT -1
|
|
#define ERR_DEMO_RANGE_ONE 1
|
|
#define ERR_DEMO_RANGE_MULTI 2
|
|
extern I2C_HandleTypeDef hi2c2;
|
|
#define X_WL55_WLE5_53L0X_hi2c hi2c2
|
|
|
|
/** }@} */ /* defgroup ErrCode */
|
|
|
|
|
|
/* USER CODE END Includes */
|
|
|
|
/* Private variables ---------------------------------------------------------*/
|
|
|
|
/* USER CODE BEGIN PV */
|
|
/* Private variables ---------------------------------------------------------*/
|
|
|
|
typedef enum {
|
|
LONG_RANGE = 0, /*!< Long range mode */
|
|
HIGH_SPEED = 1, /*!< High speed mode */
|
|
HIGH_ACCURACY = 2, /*!< High accuracy mode */
|
|
} RangingConfig_e;
|
|
|
|
/**
|
|
* Global ranging struct
|
|
*/
|
|
VL53L0X_RangingMeasurementData_t RangingMeasurementData;
|
|
|
|
/** leaky factor for filtered range
|
|
*
|
|
* r(n) = averaged_r(n-1)*leaky +r(n)(1-leaky)
|
|
*
|
|
* */
|
|
int LeakyFactorFix8 = (int)( 0.6 *256); // 0.6 for ranging, 0.0 for gesture detect
|
|
/** How many device detect set by @a DetectSensors()*/
|
|
int nDevPresent=0;
|
|
/** bit is index in VL53L0XDevs that is not necessary the dev id of the BSP */
|
|
int nDevMask=0;
|
|
|
|
int nSensorPresent;
|
|
|
|
volatile int sts_tof_distance_data[MAX_TOF_COUNT]={0,0,0};
|
|
extern volatile uint8_t sensor_data_ready;
|
|
|
|
VL53L0X_Dev_t VL53L0XDevs[]={
|
|
{.Id=XNUCLEO53L0X_LEFT, .DevLetter='l',.I2cHandle=&X_WL55_WLE5_53L0X_hi2c, .I2cDevAddr=0x52},
|
|
{.Id=XNUCLEO53L0X_CENTER,.DevLetter='c', .I2cHandle=&X_WL55_WLE5_53L0X_hi2c, .I2cDevAddr=0x52},
|
|
{.Id=XNUCLEO53L0X_RIGHT, .DevLetter='r',.I2cHandle=&X_WL55_WLE5_53L0X_hi2c, .I2cDevAddr=0x52},
|
|
};
|
|
|
|
/* USER CODE BEGIN PFP */
|
|
/* Private function prototypes -----------------------------------------------*/
|
|
//void ResetAndDetectSensor(int SetDisplay);
|
|
|
|
void sts_tof_vl53l0x_Sensor_SetNewRange(VL53L0X_Dev_t *pDev, VL53L0X_RangingMeasurementData_t *pRange);
|
|
void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig);
|
|
/* USER CODE END PFP */
|
|
|
|
/* USER CODE BEGIN 0 */
|
|
|
|
/**
|
|
* Handle Error
|
|
*
|
|
* Set err on display and loop forever
|
|
* @param err Error case code
|
|
*/
|
|
void HandleError(int err){
|
|
//char msg[16];
|
|
//sprintf(msg,"Er%d", err);
|
|
APP_LOG(TS_OFF, VLEVEL_L,"Er %04u\r\n",err);
|
|
//while(1){};
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* Reset all sensor then goes true presence detection
|
|
*
|
|
* All present devices are data initiated and assigned with their final address
|
|
* @return
|
|
*/
|
|
int sts_tof_vl53l0x_DetectSensors(void)
|
|
{
|
|
int i;
|
|
uint16_t Id;
|
|
int status;
|
|
int FinalAddress;
|
|
nDevPresent = 0;
|
|
|
|
for (i=0; i < MAX_TOF_COUNT; i++) {
|
|
XWL55_WLE5_53L0X_ResetId(i,0);
|
|
//XWL55_WLE5_53L0X_SetIntrStateId(0,i);
|
|
}
|
|
|
|
/* detect all sensors (even on-board)*/
|
|
for (i=0; i < MAX_TOF_COUNT; i++)
|
|
{
|
|
VL53L0X_Dev_t *pDev;
|
|
pDev = &VL53L0XDevs[i];
|
|
pDev->I2cDevAddr = 0x52;
|
|
pDev->Present = 0;
|
|
XWL55_WLE5_53L0X_ResetId(pDev->Id, 1);
|
|
HAL_Delay(3);
|
|
FinalAddress = 0x52+(i+1)*2;
|
|
|
|
//APP_LOG(TS_OFF, VLEVEL_H, "\r\n Detect TOF sensors #%u with I2CDevAddr=0x%02x \r\n", i, pDev->I2cDevAddr);
|
|
|
|
do {
|
|
/* Set I2C standard mode (400 KHz) before doing the first register access */
|
|
//if (status == VL53L0X_ERROR_NONE)
|
|
status = VL53L0X_WrByte(pDev, 0x88, 0x00);
|
|
|
|
/* Try to read one register using default 0x52 address */
|
|
status = VL53L0X_RdWord(pDev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
|
|
//APP_LOG(TS_OFF, VLEVEL_H, "#%u read id = %04x I2C ADDR=0x%2X\r\n",i, Id, pDev->I2cDevAddr);
|
|
if (status) {
|
|
APP_LOG(TS_OFF, VLEVEL_H, "# Read id fail \r\n");
|
|
break;
|
|
}
|
|
if (Id == 0xEEAA) {
|
|
/* Sensor is found => Change its I2C address to final one */
|
|
status = VL53L0X_SetDeviceAddress(pDev,FinalAddress);
|
|
if (status != 0) {
|
|
APP_LOG(TS_OFF, VLEVEL_H, "#i VL53L0X_SetDeviceAddress fail\r\n");
|
|
break;
|
|
}
|
|
pDev->I2cDevAddr = FinalAddress;
|
|
/* Check all is OK with the new I2C address and initialize the sensor */
|
|
status = VL53L0X_RdWord(pDev, VL53L0X_REG_IDENTIFICATION_MODEL_ID, &Id);
|
|
if (status !=0)
|
|
{
|
|
APP_LOG(TS_OFF, VLEVEL_H, "#i VL53L0X_RdWord fail\r\n");
|
|
break;
|
|
}
|
|
status = VL53L0X_DataInit(pDev);
|
|
if( status == 0 ){
|
|
pDev->Present = 1;
|
|
//APP_LOG(TS_OFF, VLEVEL_H, "#%u VL53L0X_SetDeviceAddress to 0x%02x\r\n",i, pDev->I2cDevAddr);
|
|
|
|
|
|
}
|
|
else{
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_DataInit %d fail\r\n", Id);
|
|
break;
|
|
}
|
|
nDevPresent++;
|
|
nDevMask |= 1 << i;
|
|
pDev->Present = 1;
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X %d Present and initiated to final 0x%2x, Position Mask=0x%02x\r\n", pDev->Id, pDev->I2cDevAddr, nDevMask);
|
|
|
|
} else {
|
|
APP_LOG(TS_OFF, VLEVEL_H, "# unknown ID %x\r\n", Id);
|
|
status = 1;
|
|
}
|
|
} while(0);
|
|
/* Display detected sensor(s) */
|
|
if (status) {
|
|
XWL55_WLE5_53L0X_ResetId(i,0);
|
|
}
|
|
}
|
|
|
|
return nDevPresent;
|
|
}
|
|
|
|
/**
|
|
* Setup all detected sensors for single shot mode and setup ranging configuration
|
|
*/
|
|
void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig)
|
|
{ int i;
|
|
int status=VL53L0X_ERROR_NONE;
|
|
uint8_t VhvSettings;
|
|
uint8_t PhaseCal;
|
|
uint32_t refSpadCount;
|
|
uint8_t isApertureSpads;
|
|
FixPoint1616_t signalLimit = (FixPoint1616_t)(0.25*65536);
|
|
FixPoint1616_t sigmaLimit = (FixPoint1616_t)(18*65536);
|
|
uint32_t timingBudget = 33000;
|
|
uint8_t preRangeVcselPeriod = 14;
|
|
uint8_t finalRangeVcselPeriod = 10;
|
|
// uart_printf("\r\n######### start setup for single shot \r\n");
|
|
for( i=0; i< MAX_TOF_COUNT; i++){
|
|
|
|
if( VL53L0XDevs[i].Present){
|
|
|
|
APP_LOG(TS_OFF, VLEVEL_H, "\r\n ###### Starting Range #%u sensor \r\n",i);
|
|
|
|
status=VL53L0X_StaticInit(&VL53L0XDevs[i]);
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_StaticInit failed\n");
|
|
}
|
|
|
|
status = VL53L0X_PerformRefCalibration(&VL53L0XDevs[i], &VhvSettings, &PhaseCal);
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_PerformRefCalibration failed\n");
|
|
}
|
|
|
|
status = VL53L0X_PerformRefSpadManagement(&VL53L0XDevs[i], &refSpadCount, &isApertureSpads);
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_PerformRefSpadManagement failed\n");
|
|
}
|
|
|
|
status = VL53L0X_SetDeviceMode(&VL53L0XDevs[i], VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_SetDeviceMode failed\n");
|
|
}
|
|
status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); // Enable Sigma limit
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_SetLimitCheckEnable failed\n");
|
|
}
|
|
status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); // Enable Signa limit
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_SetLimitCheckEnable failed\n");
|
|
}
|
|
/* Ranging configuration */
|
|
switch(rangingConfig) {
|
|
case LONG_RANGE:
|
|
signalLimit = (FixPoint1616_t)(0.1*65536);
|
|
sigmaLimit = (FixPoint1616_t)(60*65536);
|
|
timingBudget = 33000;
|
|
preRangeVcselPeriod = 18;
|
|
finalRangeVcselPeriod = 14;
|
|
break;
|
|
case HIGH_ACCURACY:
|
|
signalLimit = (FixPoint1616_t)(0.25*65536);
|
|
sigmaLimit = (FixPoint1616_t)(18*65536);
|
|
timingBudget = 200000;
|
|
preRangeVcselPeriod = 14;
|
|
finalRangeVcselPeriod = 10;
|
|
break;
|
|
case HIGH_SPEED:
|
|
signalLimit = (FixPoint1616_t)(0.25*65536);
|
|
sigmaLimit = (FixPoint1616_t)(32*65536);
|
|
timingBudget = 20000;
|
|
preRangeVcselPeriod = 14;
|
|
finalRangeVcselPeriod = 10;
|
|
break;
|
|
default:
|
|
APP_LOG(TS_OFF, VLEVEL_H, "Not Supported");
|
|
break;
|
|
}
|
|
|
|
status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_SetLimitCheckValue failed\n");
|
|
}
|
|
|
|
status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_SetLimitCheckValue failed\n");
|
|
}
|
|
|
|
status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&VL53L0XDevs[i], timingBudget);
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_SetMeasurementTimingBudgetMicroSeconds failed\n");
|
|
}
|
|
|
|
status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs[i], VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_SetVcselPulsePeriod failed\n");
|
|
}
|
|
|
|
status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs[i], VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_SetVcselPulsePeriod failed\n");
|
|
}
|
|
|
|
status = VL53L0X_PerformRefCalibration(&VL53L0XDevs[i], &VhvSettings, &PhaseCal);
|
|
if( status ){
|
|
APP_LOG(TS_OFF, VLEVEL_H, "VL53L0X_PerformRefCalibration failed\n");
|
|
}
|
|
|
|
VL53L0XDevs[i].LeakyFirst =1;
|
|
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
/* Store new ranging data into the device structure, apply leaky integrator if needed */
|
|
void sts_tof_vl53l0x_Sensor_SetNewRange(VL53L0X_Dev_t *pDev, VL53L0X_RangingMeasurementData_t *pRange)
|
|
{
|
|
if( pRange->RangeStatus == 0 ){
|
|
if( pDev->LeakyFirst ){
|
|
pDev->LeakyFirst = 0;
|
|
pDev->LeakyRange = pRange->RangeMilliMeter;
|
|
}
|
|
else{
|
|
pDev->LeakyRange = (pDev->LeakyRange*LeakyFactorFix8 + (256-LeakyFactorFix8)*pRange->RangeMilliMeter)>>8;
|
|
}
|
|
}
|
|
else{
|
|
pDev->LeakyFirst = 1;
|
|
}
|
|
}
|
|
|
|
/* USER CODE END 0 */
|
|
void STS_TOF250_Range_Process(void)
|
|
{
|
|
#define I2C_TIME_OUT_BASE 10
|
|
#define I2C_TIME_OUT_BYTE 1
|
|
int status;
|
|
uint8_t pdata[2]={0x0,0x0}; uint32_t count=2;
|
|
int i2c_time_out = I2C_TIME_OUT_BASE + count*I2C_TIME_OUT_BYTE;
|
|
|
|
sensor_data_ready = 0 ;
|
|
sts_tof_distance_data[0] = 0;
|
|
sts_tof_distance_data[1] = 0;
|
|
sts_tof_distance_data[2] = 0;
|
|
|
|
status = HAL_I2C_Master_Receive(&hi2c2, TOF250_I2C_ADDR, pdata, count, i2c_time_out);
|
|
if (status == HAL_OK) {
|
|
APP_LOG(TS_OFF, VLEVEL_H, "\r\n## TOF250 ranging value %02x %02x %4d ",pdata[0],pdata[1], *pdata);
|
|
sts_tof_distance_data[2] = pdata[0]*255+pdata[1];
|
|
sensor_data_ready = 1;
|
|
APP_LOG(TS_OFF, VLEVEL_H, "\r\n## Measured Range: \r\nTOF #0 = %4u mm, \r\nTOF #1 = %4u mm, \r\nTOF #2 = %4u mm\r\n",
|
|
(int)sts_tof_distance_data[0],(int)sts_tof_distance_data[1],(int)sts_tof_distance_data[2]);
|
|
}
|
|
}
|
|
void STS_TOF_VL53L0X_Range_Process(void)
|
|
{
|
|
int status=0, i;
|
|
RangingConfig_e RangingConfig = HIGH_ACCURACY; //HIGH_ACCURACY; //LONG_RANGE;
|
|
XWL55_WLE5_53L0X_Init();
|
|
if ((nDevMask ==0) || (nSensorPresent ==0))
|
|
{
|
|
nSensorPresent = sts_tof_vl53l0x_DetectSensors(); // confirm sensor online
|
|
APP_LOG(TS_OFF, VLEVEL_H, "\r\n %u pcs sensor(s) online \r\n", nSensorPresent);
|
|
}
|
|
sts_tof_distance_data[0] = 0;
|
|
sts_tof_distance_data[1] = 0;
|
|
sts_tof_distance_data[2] = 0;
|
|
|
|
if (nSensorPresent > 0)
|
|
{
|
|
// RangingConfig == HIGH ACCURACY, LONG RANGE, HIGH SPEED
|
|
sts_tof_vl53l0x_SetupSingleShot(RangingConfig);
|
|
|
|
sensor_data_ready = 0 ;
|
|
for (i=0; i< MAX_TOF_COUNT; i++)
|
|
{
|
|
if (VL53L0XDevs[i].Present ==1)
|
|
{
|
|
uint8_t rep=0;
|
|
do {
|
|
rep++;
|
|
status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs[i], &RangingMeasurementData);
|
|
if (status ==0)
|
|
{
|
|
sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs[i],&RangingMeasurementData);
|
|
if( RangingMeasurementData.RangeStatus == 0 )
|
|
{
|
|
sts_tof_distance_data[i] = (int)VL53L0XDevs[i].LeakyRange;
|
|
|
|
nDevMask |= (1 << i);
|
|
sensor_data_ready |= 1;
|
|
} else
|
|
{
|
|
//HandleError(ERR_DEMO_RANGE_ONE);
|
|
//APP_LOG(TS_OFF, VLEVEL_M,"\r\n#%u sensor ERR code = %04u\r\n",i, ERR_DEMO_RANGE_ONE);
|
|
}
|
|
// ########## two conditions
|
|
// ########## 1) return status ==0,
|
|
// ########## 2) and ranging status for valid ranging value !!!!!!!!!!!!!!!!!
|
|
}
|
|
HAL_Delay(1);
|
|
} while ((RangingMeasurementData.RangeStatus != 0)&&(rep <10));
|
|
sts_tof_distance_data[i] = (RangingMeasurementData.RangeStatus!=0)?STS_MAX_L0_RANGE:VL53L0XDevs[i].LeakyRange;
|
|
}
|
|
|
|
HAL_Delay(1);
|
|
} // for i < MAX_TOF_COUNT
|
|
|
|
if (sensor_data_ready != 0) {
|
|
APP_LOG(TS_OFF, VLEVEL_L, "\r\n## Measured Range: \r\nTOF #0 = %4u mm, \r\nTOF #1 = %4u mm, \r\nTOF #2 = %4u mm\r\n",
|
|
(int)sts_tof_distance_data[0],(int)sts_tof_distance_data[1],(int)sts_tof_distance_data[2]);
|
|
}
|
|
|
|
} // nSensorPresent >0
|
|
|
|
// reset for next ranging
|
|
//nDevMask = 0;
|
|
//nSensorPresent = 0;
|
|
}
|
|
|
|
|
|
|
|
/* USER CODE BEGIN 4 */
|
|
|
|
/* USER CODE END 4 */
|
|
|
|
|
|
/**
|
|
* @}
|
|
*/
|
|
|
|
/**
|
|
* @}
|
|
*/
|
|
|
|
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|