sundp
/
STS_RR_R125
1
0
Fork 0
Code Issues Pull Requests 1 Packages Projects Releases Wiki Activity

revise tof sensor detection for max of 3 sensors instead of single one.

This commit is contained in:
Yunhorn 2023-07-07 11:35:00 +08:00
parent be33595898
commit 7e90cb476c
4 changed files with 171 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
STM32CubeIDE/.settings/language.settings.xml
View File

@ -5,7 +5,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="686453894977928153" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="900329934260214400" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>
@ -16,7 +16,7 @@
<provider copy-of="extension" id="org.eclipse.cdt.ui.UserLanguageSettingsProvider"/>
<provider-reference id="org.eclipse.cdt.core.ReferencedProjectsLanguageSettingsProvider" ref="shared-provider"/>
<provider-reference id="org.eclipse.cdt.managedbuilder.core.MBSLanguageSettingsProvider" ref="shared-provider"/>
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="686453894977928153" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<provider class="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" console="false" env-hash="900329934260214400" id="com.st.stm32cube.ide.mcu.toolchain.armnone.setup.CrossBuiltinSpecsDetector" keep-relative-paths="false" name="MCU ARM GCC Built-in Compiler Settings" parameter="${COMMAND} ${FLAGS} -E -P -v -dD &quot;${INPUTS}&quot;" prefer-non-shared="true">
<language-scope id="org.eclipse.cdt.core.gcc"/>
<language-scope id="org.eclipse.cdt.core.g++"/>
</provider>

8
TOF/App/X-WL55_WLE5_53L0X.h
View File

@ -20,6 +20,7 @@
/** @ingroup VL53L0X_config
* @{*/
#define MAX_TOF_COUNT 3
#ifndef VL53L0X_HAVE_UART
/**
@ -185,9 +186,9 @@ extern void XNUCLEO53L1A1_USART2_UART_Init(void);
* @note Most functions are using a device selector as input. ASCII 'c', 'l' or 'r' are also accepted.
*/
enum XNUCLEO53L1A1_dev_e{
XNUCLEO53L1A1_DEV_LEFT = 0, //!< left satellite device P21 header : 'l'
XNUCLEO53L1A1_DEV_CENTER = 1, //!< center (built-in) vl053 device : 'c"
XNUCLEO53L1A1_DEV_RIGHT= 2 //!< Right satellite device P22 header : 'r'
XNUCLEO53L0X_LEFT = 0, //!< left satellite device P21 header : 'l'
XNUCLEO53L0X_CENTER = 1, //!< center (built-in) vl053 device : 'c"
XNUCLEO53L0X_RIGHT= 2 //!< Right satellite device P22 header : 'r'
};
/**
@ -251,5 +252,6 @@ int XNUCLEO53L1A1_ResetId(int DevNo, int state );
* >0 interrupt configured but with potential sharing on EXTI groups see @ref VL53L0X_GPIO1_MAP
*/
int XNUCLEO53L1A1_SetIntrStateId(int EnableIntr, int DevNo);
int sts_tof_vl53l0x_DetectSensors(void);
#endif /* _X_WL55_WLE5_53L0X_H_ */

9
TOF/App/app_tof.h
View File

@ -30,6 +30,10 @@
#include "yunhorn_sts_prd_conf.h"
#include "yunhorn_sts_sensors.h"
#include "vl53l0x_def.h"
/* Exported defines ----------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
@ -45,11 +49,10 @@ void sts_vl53lx_ranging(vl53lx_model, range_mode, i_distance_threshold_mm, i_int
void STS_R0_SENSOR_Read(STS_R0_SensorDataTypeDef *r0_data);
int sts_tof_vl53l0x_DetectSensors(void);
#ifdef STS_USE_TOF_VL53L0X
void STS_TOF_VL53L0X_Range_Process(void);
void STS_R0_SENSOR_Read(STS_R0_SensorDataTypeDef *r0_data);
#endif

150
TOF/App/app_tof_vl53l0x_range.c
View File

@ -75,22 +75,29 @@ VL53L0X_RangingMeasurementData_t RangingMeasurementData;
* r(n) = averaged_r(n-1)*leaky +r(n)(1-leaky)
*
* */
int LeakyFactorFix8 = (int)( 0.6 *256);
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;
volatile int sts_tof_distance_data;
int nSensorPresent;
volatile int sts_tof_distance_data[MAX_TOF_COUNT];
extern volatile uint8_t sensor_data_ready;
VL53L0X_Dev_t VL53L0XDevs={
.I2cHandle=&X_WL55_WLE5_53L0X_hi2c,
.I2cDevAddr=0x52
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 */
@ -117,26 +124,33 @@ void HandleError(int err){
*/
int sts_tof_vl53l0x_DetectSensors(void)
{
int i;
uint16_t Id;
int status;
int FinalAddress;
nDevPresent = 0;
/* detect all sensors (even on-board)*/
for (i=0; i < MAX_TOF_COUNT; i++)
{
VL53L0X_Dev_t *pDev;
pDev = &VL53L0XDevs;
pDev = &VL53L0XDevs[i];
pDev->I2cDevAddr = 0x52;
pDev->Present = 0;
//status = XNUCLEO53L0A1_ResetId(pDev->Id, 1);
//HAL_Delay(2);
FinalAddress=0x52;
FinalAddress=0x52 +(i+1)*2;
APP_LOG(TS_OFF, VLEVEL_L, "\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);
//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_L, "#%u read id = %04x \r\n",i, Id);
if (status) {
APP_LOG(TS_OFF, VLEVEL_L, "# Read id fail\n");
break;
@ -159,30 +173,27 @@ int sts_tof_vl53l0x_DetectSensors(void)
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_DataInit %d fail\n");
break;
}
//APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X Present and initiated to final 0x%x\n", pDev->I2cDevAddr);
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X %d Present and initiated to final 0x%x\n", pDev->Id, pDev->I2cDevAddr);
nDevPresent++;
nDevMask |= 1 << i;
pDev->Present = 1;
}
else {
APP_LOG(TS_OFF, VLEVEL_L, "# unknown ID %x\n", Id);
status = 1;
}
/* if fail r can't use for any reason then put the device back to reset */
//if (status) {
//XNUCLEO53L0A1_ResetId(i, 0);
//}
} while(0);
/* Display detected sensor(s) */
}
return Id;
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;
@ -194,31 +205,33 @@ void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig)
uint8_t preRangeVcselPeriod = 14;
uint8_t finalRangeVcselPeriod = 10;
// uart_printf("\r\n######### start setup for single shot \r\n");
for( i=0; i<3; i++){
if( VL53L0XDevs[i].Present){
status=VL53L0X_StaticInit(&VL53L0XDevs);
status=VL53L0X_StaticInit(&VL53L0XDevs[i]);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_StaticInit failed\n");
}
status = VL53L0X_PerformRefCalibration(&VL53L0XDevs, &VhvSettings, &PhaseCal);
status = VL53L0X_PerformRefCalibration(&VL53L0XDevs[i], &VhvSettings, &PhaseCal);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefCalibration failed\n");
}
status = VL53L0X_PerformRefSpadManagement(&VL53L0XDevs, &refSpadCount, &isApertureSpads);
status = VL53L0X_PerformRefSpadManagement(&VL53L0XDevs[i], &refSpadCount, &isApertureSpads);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefSpadManagement failed\n");
}
status = VL53L0X_SetDeviceMode(&VL53L0XDevs, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
status = VL53L0X_SetDeviceMode(&VL53L0XDevs[i], VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetDeviceMode failed\n");
}
status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); // Enable Sigma limit
status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); // Enable Sigma limit
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckEnable failed\n");
}
status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); // Enable Signa limit
status = VL53L0X_SetLimitCheckEnable(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); // Enable Signa limit
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckEnable failed\n");
}
@ -250,43 +263,47 @@ void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig)
break;
}
status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, signalLimit);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckValue failed\n");
}
status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
status = VL53L0X_SetLimitCheckValue(&VL53L0XDevs[i], VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, sigmaLimit);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetLimitCheckValue failed\n");
}
status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&VL53L0XDevs, timingBudget);
status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(&VL53L0XDevs[i], timingBudget);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetMeasurementTimingBudgetMicroSeconds failed\n");
}
status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs, VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs[i], VL53L0X_VCSEL_PERIOD_PRE_RANGE, preRangeVcselPeriod);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetVcselPulsePeriod failed\n");
}
status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs, VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
status = VL53L0X_SetVcselPulsePeriod(&VL53L0XDevs[i], VL53L0X_VCSEL_PERIOD_FINAL_RANGE, finalRangeVcselPeriod);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_SetVcselPulsePeriod failed\n");
}
status = VL53L0X_PerformRefCalibration(&VL53L0XDevs, &VhvSettings, &PhaseCal);
status = VL53L0X_PerformRefCalibration(&VL53L0XDevs[i], &VhvSettings, &PhaseCal);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_PerformRefCalibration failed\n");
}
VL53L0XDevs.LeakyFirst =1;
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){
void sts_tof_vl53l0x_Sensor_SetNewRange(VL53L0X_Dev_t *pDev, VL53L0X_RangingMeasurementData_t *pRange)
{
if( pRange->RangeStatus == 0 ){
if( pDev->LeakyFirst ){
pDev->LeakyFirst = 0;
@ -301,56 +318,61 @@ void sts_tof_vl53l0x_Sensor_SetNewRange(VL53L0X_Dev_t *pDev, VL53L0X_RangingMeas
}
}
VL53L0X_Error sts_tof_vl53l0x_range(VL53L0X_Dev_t *dev,VL53L0X_RangingMeasurementData_t *pdata, RangingConfig_e rangingConfig)
{
//int over=0;
VL53L0X_Error status = VL53L0X_ERROR_NONE;
/* Setup all sensors in Single Shot mode */
sts_tof_vl53l0x_SetupSingleShot(rangingConfig);
/* Call All-In-One blocking API function */
status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs, &RangingMeasurementData);
if( status ==0 ){
sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs,&RangingMeasurementData);
/* Display distance in mm */
if( RangingMeasurementData.RangeStatus == 0 ){
sts_tof_distance_data = (int)VL53L0XDevs.LeakyRange;
//uart_printf("## MeasureData Distance = %4d mm \r\n", (int)Distance_data,(int) VL53L0XDevs.LeakyRange);
}
}
else{
HandleError(ERR_DEMO_RANGE_ONE);
}
return status;
}
/* USER CODE END 0 */
void STS_TOF_VL53L0X_Range_Process(void)
{
int Status=0;
int status=0, i;
RangingConfig_e RangingConfig = HIGH_ACCURACY; //HIGH_ACCURACY; //LONG_RANGE;
XWL55_WLE5_53L0X_Init();
Status = sts_tof_vl53l0x_DetectSensors(); // confirm sensor online
//if (Status ==1 )
//APP_LOG(TS_OFF, VLEVEL_M, "sensor online \r\n");
nSensorPresent = sts_tof_vl53l0x_DetectSensors(); // confirm sensor online
APP_LOG(TS_OFF, VLEVEL_M, "sensor online \r\n");
nDevMask = 0;
if (nSensorPresent > 0)
{
// RangingConfig == HIGH ACCURACY, LONG RANGE, HIGH SPEED
sts_tof_vl53l0x_SetupSingleShot(RangingConfig);
Status = sts_tof_vl53l0x_range(&VL53L0XDevs,&RangingMeasurementData, RangingConfig);
for (i=0; i< MAX_TOF_COUNT; i++)
{
if (VL53L0XDevs[i].Present ==1)
{
status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs[i], &RangingMeasurementData);
if( status ==0 )
{
sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs[i],&RangingMeasurementData);
/* Display distance in mm */
if( RangingMeasurementData.RangeStatus == 0 )
{
sts_tof_distance_data[i] = (int)VL53L0XDevs[i].LeakyRange;
//uart_printf("## MeasureData Distance = %4d mm \r\n", (int)Distance_data,(int) VL53L0XDevs.LeakyRange);
}
} else
{
HandleError(ERR_DEMO_RANGE_ONE);
}
// ########## two conditions
// ########## 1) return status ==0,
// ########## 2) and ranging status for valid ranging value !!!!!!!!!!!!!!!!!
if ((Status == 0) && (RangingMeasurementData.RangeStatus == 0 ))
if ((status == 0) && (RangingMeasurementData.RangeStatus == 0 ))
{
//sts_tof_distance_data = (int) VL53L0XDevs.LeakyRange;
APP_LOG(TS_OFF, VLEVEL_M, "## Measured Range = %4d mm\r\n", (int)sts_tof_distance_data);
sensor_data_ready = 1;
APP_LOG(TS_OFF, VLEVEL_M, "## Measured Range TOF #0 = %4d mm, TOF #1 = %4d mm\r\n",
(int)sts_tof_distance_data[0],(int)sts_tof_distance_data[1]);
nDevMask |= (1 << i);
sensor_data_ready |= 1;
}
}
} // for i < MAX_TOF_COUNT
} // nSensorPresent >0
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */