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

revised RFAC and set TOF range to high accuracy and max range to 1000 mm

This commit is contained in:
Yunhorn 2023-11-09 14:14:17 +08:00
parent b8b4e6a144
commit e2dadadf4e
2 changed files with 46 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Core/Src/sts_cmox_hmac_sha.c
View File

@ -41,11 +41,7 @@ uint32_t sts_hmac_verify(void)
GetUniqueId(uid);
ret = sts_hmac_sha1((const uint8_t *) mKey, sizeof(mKey), (const uint8_t*)(uid+4), 4, &hmac_result);
/*
for (i=0;i<sizeof(Computed_Tag_SHA1); i++) {
APP_LOG(TS_OFF,VLEVEL_M,"0x%02x ", hmac_result.hmac_tag[i]);
}
*/
ret = memcmp(hmac_result.hmac_tag, (void *)sts_ac_code, sizeof(sts_ac_code));
hmac_result.ac_pass = (ret == 0x0)?1U:0U;

78
TOF/App/app_tof_vl53l0x_range.c
View File

@ -44,7 +44,7 @@
* @defgroup ErrCode Errors code shown on display
* @{
*/
#define STS_MAX_L0_RANGE ((uint32_t) 2000)
#define STS_MAX_L0_RANGE ((uint32_t) 1000)
#define ERR_DETECT -1
#define ERR_DEMO_RANGE_ONE 1
#define ERR_DEMO_RANGE_MULTI 2
@ -154,7 +154,7 @@ int sts_tof_vl53l0x_DetectSensors(void)
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);
@ -173,21 +173,26 @@ int sts_tof_vl53l0x_DetectSensors(void)
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);
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, "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);
@ -368,9 +373,9 @@ void STS_TOF_VL53L0X_Range_Process(void)
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] = STS_MAX_L0_RANGE;
sts_tof_distance_data[1] = STS_MAX_L0_RANGE;
sts_tof_distance_data[2] = STS_MAX_L0_RANGE;
sts_tof_distance_data[0] = 0;
sts_tof_distance_data[1] = 0;
sts_tof_distance_data[2] = 0;
if (nSensorPresent > 0)
{
@ -382,33 +387,40 @@ void STS_TOF_VL53L0X_Range_Process(void)
{
if (VL53L0XDevs[i].Present ==1)
{
status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs[i], &RangingMeasurementData);
if( status ==0 )
{
sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs[i],&RangingMeasurementData);
if( RangingMeasurementData.RangeStatus == 0 )
uint8_t rep=0;
do {
rep++;
status = VL53L0X_PerformSingleRangingMeasurement(&VL53L0XDevs[i], &RangingMeasurementData);
if (status ==0)
{
sts_tof_distance_data[i] = (int)VL53L0XDevs[i].LeakyRange;
if (sts_tof_distance_data[i] ==0) sts_tof_distance_data[i]+= STS_MAX_L0_RANGE;
nDevMask |= (1 << i);
sensor_data_ready |= 1;
} else
{
//HandleError(ERR_DEMO_RANGE_ONE);
APP_LOG(TS_OFF, VLEVEL_H,"\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 !!!!!!!!!!!!!!!!!
}
}
sts_tof_vl53l0x_Sensor_SetNewRange(&VL53L0XDevs[i],&RangingMeasurementData);
if( RangingMeasurementData.RangeStatus == 0 )
{
sts_tof_distance_data[i] = (int)VL53L0XDevs[i].LeakyRange;
HAL_Delay(30);
} // for i < MAX_TOF_COUNT
if (sensor_data_ready != 0) {
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",
nDevMask |= (1 << i);
sensor_data_ready |= 1;
} else
{
//HandleError(ERR_DEMO_RANGE_ONE);
APP_LOG(TS_OFF, VLEVEL_H,"\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:sts_tof_distance_data[i];
}
HAL_Delay(5);
} // for i < MAX_TOF_COUNT
if (sensor_data_ready != 0) {
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]);
}
}
} // nSensorPresent >0