/* 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) 2500)
#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;
}
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] = STS_MAX_L0_RANGE;
sts_tof_distance_data[1] = STS_MAX_L0_RANGE;
sts_tof_distance_data[2] = STS_MAX_L0_RANGE;
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 = LONG_RANGE; //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] = (int)VL53L0XDevs[i].LeakyRange ==0?STS_MAX_L0_RANGE:(int)VL53L0XDevs[i].LeakyRange;
}
HAL_Delay(5);
} // 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****/