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

revised GPIO, XSHUT fully support 3 TOF sensors

This commit is contained in:
Yunhorn 2023-07-13 16:34:21 +08:00
parent 1c393d1043
commit 0f665e497d
2 changed files with 18 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
TOF/App/X-WL55_WLE5_53L0X.c
View File

@ -205,15 +205,16 @@ void XWL55_WLE5_53L0X_ResetId(uint8_t DevNo, int state)
break;
case XNUCLEO53L0X_CENTER:
HAL_GPIO_WritePin(TOF_C_XSHUT_GPIO_Port, TOF_C_XSHUT_Pin, ((state == 1)?GPIO_PIN_SET:GPIO_PIN_RESET)); break;
HAL_GPIO_WritePin(TOF_C_XSHUT_GPIO_Port, TOF_C_XSHUT_Pin, ((state == 1)?GPIO_PIN_SET:GPIO_PIN_RESET));
break;
case XNUCLEO53L0X_RIGHT:
HAL_GPIO_WritePin(TOF_R_XSHUT_GPIO_Port, TOF_R_XSHUT_Pin, ((state == 1)?GPIO_PIN_SET:GPIO_PIN_RESET)); break;
default:
HAL_GPIO_WritePin(TOF_R_XSHUT_GPIO_Port, TOF_R_XSHUT_Pin, ((state == 1)?GPIO_PIN_SET:GPIO_PIN_RESET));
break;
default:
break;
}
}
/**

32
TOF/App/app_tof_vl53l0x_range.c
View File

@ -111,8 +111,8 @@ void sts_tof_vl53l0x_SetupSingleShot(RangingConfig_e rangingConfig);
void HandleError(int err){
//char msg[16];
//sprintf(msg,"Er%d", err);
APP_LOG(TS_OFF, VLEVEL_L,"Er%d\r\n",err);
while(1){};
APP_LOG(TS_OFF, VLEVEL_L,"Er %04u\r\n",err);
//while(1){};
}
@ -135,7 +135,7 @@ int sts_tof_vl53l0x_DetectSensors(void)
XWL55_WLE5_53L0X_ResetId(i,0);
}
//XWL55_WLE5_53L0X_SetIntrStateId(0,0);
XWL55_WLE5_53L0X_SetIntrStateId(0,0);
/* detect all sensors (even on-board)*/
for (i=0; i < MAX_TOF_COUNT; i++)
@ -145,7 +145,7 @@ int sts_tof_vl53l0x_DetectSensors(void)
pDev->I2cDevAddr = 0x52;
pDev->Present = 0;
XWL55_WLE5_53L0X_ResetId(pDev->Id, 1);
HAL_Delay(2);
HAL_Delay(30);
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);
@ -218,9 +218,12 @@ 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++){
for( i=0; i< MAX_TOF_COUNT; i++){
if( VL53L0XDevs[i].Present){
APP_LOG(TS_OFF, VLEVEL_L, "\r\n ###### Starting Range #%u sensor \r\n",i);
status=VL53L0X_StaticInit(&VL53L0XDevs[i]);
if( status ){
APP_LOG(TS_OFF, VLEVEL_L, "VL53L0X_StaticInit failed\n");
@ -353,35 +356,26 @@ void STS_TOF_VL53L0X_Range_Process(void)
{
if (VL53L0XDevs[i].Present ==1)
{
APP_LOG(TS_OFF, VLEVEL_L, "\r\nStart Ranging #%u sensor, position mask 0x%02x \r\n", i, nDevMask);
status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs[i], &RangingMeasurementData);
APP_LOG(TS_OFF, VLEVEL_L, "\r\nRanging status =0x%04x \r\n", status);
if( status ==0 )
{
sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs[i],&RangingMeasurementData);
/* Display distance in mm */
APP_LOG(TS_OFF, VLEVEL_L, "\r\nSet New Range status =%u \r\n", RangingMeasurementData.RangeStatus);
if( RangingMeasurementData.RangeStatus == 0 )
{
sts_tof_distance_data[i] = (int)VL53L0XDevs[i].LeakyRange;
nDevMask |= (1 << i);
sensor_data_ready |= 1;
//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 !!!!!!!!!!!!!!!!!
// ########## two conditions
// ########## 1) return status ==0,
// ########## 2) and ranging status for valid ranging value !!!!!!!!!!!!!!!!!
}
}
HAL_Delay(30);
} // for i < MAX_TOF_COUNT
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",