void Sample_XTalkCalibrate(void) {
VL6180xDev_t myDev;
VL6180x_RangeData_t Range;
int XTalkRate;
int i, status;
/* init device */
MyDev_Init(myDev);
status = Sample_Init(myDev);
if( status <0 ){
HandleError("Sample_Init fail");
}
/* run calibration */
XTalkRate = Sample_XTalkRunCalibration(myDev);
/*TODO when possible reset re-init device else set back required scaling/filter*/
VL6180x_FilterSetState(myDev, 1); // turn on wrap around filter again
VL6180x_UpscaleSetScaling(myDev, 3); // set scaling
/* apply cross talk */
status = VL6180x_SetXTalkCompensationRate(myDev, XTalkRate);
if( status<0 ){ /* ignore warning but not error */
HandleError("VL6180x_WrWord fail");
}
for( i=0; i< 10; i++){
status = VL6180x_RangePollMeasurement(myDev, &Range);
MyDev_ShowRange(myDev, Range.range_mm);
}
}
/**
* Implement Xtalk calibration as described in VL6180x Datasheet (DocID026171 Rev 6)
*
* Device must be initialized
*
* @note device scaling wrap filter and xtalk setting are scraped !
* It is safer to reset and re init device when done
* @warning follow strictly procedure described in the device manual
* @param myDev The device
* @return The cross talk (9.7 fix point as expected in register)
*/
int Sample_XTalkRunCalibration(VL6180xDev_t myDev)
{
VL6180x_RangeData_t Range[N_MEASURE_AVG];
int32_t RangeSum;
int32_t RateSum;
int status;
int i;
int XTalkInt;
int RealTargetDistance=100;
VL6180x_UpscaleSetScaling(myDev, 1); // set scaling x1
// we do not check status as this can fail only in config that do not have configurable upscale
#if VL6180x_WRAP_AROUND_FILTER_SUPPORT
VL6180x_FilterSetState(myDev, 0); // turn off wrap around filter
#endif
status = VL6180x_ClearAllInterrupt(myDev);
if( status ){
HandleError("VL6180x_ClearAllInterrupt fail");
}
/* Ask user to place a black target at know RealTargetDistance (100mm) */
MyDev_UserWait(myDev, "Calibrating : place black target at 100mm from glass");
status=VL6180x_WrWord(myDev, SYSRANGE_CROSSTALK_COMPENSATION_RATE, 0);
for( i=0; i<N_MEASURE_AVG; i++){
status = VL6180x_RangePollMeasurement(myDev, &Range[i]);
if( status ){
HandleError("VL6180x_RangePollMeasurement fail");
}
if( Range[i].errorStatus != 0 ){
HandleError("No target detect");
}
}
RangeSum=0;
RateSum=0;
for( i=0; i<N_MEASURE_AVG; i++){
RangeSum+= Range[i].range_mm;
RateSum+= Range[i].signalRate_mcps;
}
/* Rate sum is 9.7 fixpoint so does xtlak computed below
* The order of operation is important to have decent final precision without use of floating point
* using a higher real distance and number of point may lead to 32 bit integer overflow in formula below */
XTalkInt = RateSum*(N_MEASURE_AVG*RealTargetDistance-RangeSum)/N_MEASURE_AVG /(RealTargetDistance*N_MEASURE_AVG);
MyDev_Printf(myDev, "avg range %d rate %d comp=%d\n", RangeSum/N_MEASURE_AVG, RateSum/N_MEASURE_AVG, XTalkInt);
return XTalkInt;
}
void range_finder_update(int *range)
{
VL6180x_RangeData_t Range;
tca9545Channel_e channel;
channel = Channel_0;
do {
tca9545_set(channel);
VL6180x_RangePollMeasurement(myDev, &Range);
if (Range.errorStatus == 0 )
{
*range++ = Range.range_mm;
MyDev_ShowRange(myDev, Range.range_mm,0); // your code display range in mm
}
else
{
*range++ = 255;
MyDev_ShowErr(myDev, Range.errorStatus); // your code display error code
}
channel++;
} while (channel <= Channel_3); // your code to stop looping
}
int range_finder_get(enum range_finder_channel channel)
{
VL6180x_RangeData_t Range;
switch (channel)
{
case left_side:
tca9545_set(Channel_0);
break;
case right_side:
tca9545_set(Channel_3);
break;
case left_front:
tca9545_set(Channel_1);
break;
case right_front:
tca9545_set(Channel_2);
break;
}
VL6180x_RangePollMeasurement(myDev, &Range);
return Range.range_mm;
}
