Single Ranging example with High Accuracy for the VL53L0X Time-of-Flight sensor
Dependencies: mbed vl53l0x_api
Revision 2:98cd8c47c1ad, committed 2019-05-03
- Comitter:
- adonisduo
- Date:
- Fri May 03 00:05:37 2019 +0000
- Parent:
- 1:2e2199fe8ccc
- Commit message:
- Released by HANSL
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Aug 23 05:19:55 2016 +0000 +++ b/main.cpp Fri May 03 00:05:37 2019 +0000 @@ -80,6 +80,110 @@ return Status; } +VL53L0X_Error rangingTest(VL53L0X_Dev_t *pMyDevice) +{ + VL53L0X_Error Status = VL53L0X_ERROR_NONE; + VL53L0X_RangingMeasurementData_t RangingMeasurementData; + int i; + FixPoint1616_t LimitCheckCurrent; + uint32_t refSpadCount; + uint8_t isApertureSpads; + uint8_t VhvSettings; + uint8_t PhaseCal; + + if(Status == VL53L0X_ERROR_NONE) + { + printf ("Call of VL53L0X_StaticInit\n"); + Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization + print_pal_error(Status); + } + + if(Status == VL53L0X_ERROR_NONE) + { + printf ("Call of VL53L0X_PerformRefCalibration\n"); + Status = VL53L0X_PerformRefCalibration(pMyDevice, + &VhvSettings, &PhaseCal); // Device Initialization + print_pal_error(Status); + } + + if(Status == VL53L0X_ERROR_NONE) // needed if a coverglass is used and no calibration has been performed + { + printf ("Call of VL53L0X_PerformRefSpadManagement\n"); + Status = VL53L0X_PerformRefSpadManagement(pMyDevice, + &refSpadCount, &isApertureSpads); // Device Initialization + printf ("refSpadCount = %d, isApertureSpads = %d\n", refSpadCount, isApertureSpads); + print_pal_error(Status); + } + + if(Status == VL53L0X_ERROR_NONE) + { + + // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement + printf ("Call of VL53L0X_SetDeviceMode\n"); + Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode + print_pal_error(Status); + } + + // Enable/Disable Sigma and Signal check + if (Status == VL53L0X_ERROR_NONE) { + Status = VL53L0X_SetLimitCheckEnable(pMyDevice, + VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); + } + if (Status == VL53L0X_ERROR_NONE) { + Status = VL53L0X_SetLimitCheckEnable(pMyDevice, + VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); + } + + if (Status == VL53L0X_ERROR_NONE) { + Status = VL53L0X_SetLimitCheckValue(pMyDevice, + VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, + (FixPoint1616_t)(0.25*65536)); + } + if (Status == VL53L0X_ERROR_NONE) { + Status = VL53L0X_SetLimitCheckValue(pMyDevice, + VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, + (FixPoint1616_t)(18*65536)); + } + if (Status == VL53L0X_ERROR_NONE) { + Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(pMyDevice, + 200000); + } + + if(Status == VL53L0X_ERROR_NONE) + { + printf ("Call of VL53L0X_SetRangeFractionEnable\n"); + Status = VL53L0X_SetRangeFractionEnable(pMyDevice, 0x01); + } + /* + * Step 4 : Test ranging mode + */ + + if(Status == VL53L0X_ERROR_NONE) + { + for(i=0;i<100;i++){ + //printf ("Call of VL53L0X_PerformSingleRangingMeasurement\n"); + Status = VL53L0X_PerformSingleRangingMeasurement(pMyDevice, + &RangingMeasurementData); + + //print_pal_error(Status); + //print_range_status(&RangingMeasurementData); + + VL53L0X_GetLimitCheckCurrent(pMyDevice, + VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, &LimitCheckCurrent); + + printf("RANGE IGNORE THRESHOLD: %f\n", (float)LimitCheckCurrent/65536.0f); + + + if (Status != VL53L0X_ERROR_NONE) break; + + //printf("Measured distance: %i\n\n", RangingMeasurementData.RangeMilliMeter); + + + } + } + return Status; +} + int main() { @@ -93,6 +197,7 @@ int32_t status_int; + pc.baud(460800); pc.printf("VL53L0X API Simple Ranging Example\r\n"); // Initialize Comms @@ -110,20 +215,20 @@ } pc.printf("VL53L0X API Version: %d.%d.%d (revision %d)\r\n", pVersion->major, pVersion->minor ,pVersion->build, pVersion->revision); - int addr; - - addr = VL53L0X_scan(); - printf("Device found at: %i\r\n", addr); - //uint8_t data; - //data=0; - if(Status == VL53L0X_ERROR_NONE) + int addr; + + addr = VL53L0X_scan(); + printf("Device found at: %i\r\n", addr); + //uint8_t data; + //data=0; + if(Status == VL53L0X_ERROR_NONE) { - printf ("Call of VL53L0X_DataInit\n"); - uint16_t osc_calibrate_val=0; - Status = VL53L0X_RdWord(&MyDevice, VL53L0X_REG_OSC_CALIBRATE_VAL,&osc_calibrate_val); - printf("%i\n",osc_calibrate_val); - Status = VL53L0X_DataInit(&MyDevice); // Data initialization - print_pal_error(Status); + printf ("Call of VL53L0X_DataInit\n"); + uint16_t osc_calibrate_val=0; + Status = VL53L0X_RdWord(&MyDevice, VL53L0X_REG_OSC_CALIBRATE_VAL,&osc_calibrate_val); + printf("%i\n",osc_calibrate_val); + Status = VL53L0X_DataInit(&MyDevice); // Data initialization + print_pal_error(Status); } if(Status == VL53L0X_ERROR_NONE) @@ -136,11 +241,12 @@ printf("Device Type : %s\n", DeviceInfo.Type); printf("Device ID : %s\n", DeviceInfo.ProductId); printf("ProductRevisionMajor : %d\n", DeviceInfo.ProductRevisionMajor); - printf("ProductRevisionMinor : %d\n", DeviceInfo.ProductRevisionMinor); + printf("ProductRevisionMinor : %d\n", DeviceInfo.ProductRevisionMinor); - if ((DeviceInfo.ProductRevisionMinor != 1) && (DeviceInfo.ProductRevisionMinor != 1)) { - printf("Error expected cut 1.1 but found cut %d.%d\n", - DeviceInfo.ProductRevisionMajor, DeviceInfo.ProductRevisionMinor); + if ((DeviceInfo.ProductRevisionMinor != 1) && (DeviceInfo.ProductRevisionMinor != 1)) + { + printf("Error expected cut 1.1 but found cut %d.%d\n", + DeviceInfo.ProductRevisionMajor, DeviceInfo.ProductRevisionMinor); Status = VL53L0X_ERROR_NOT_SUPPORTED; } } @@ -155,7 +261,7 @@ uint8_t VhvSettings; uint8_t PhaseCal; - if(Status == VL53L0X_ERROR_NONE) + if(Status == VL53L0X_ERROR_NONE) { printf ("Call of VL53L0X_StaticInit\n"); Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization @@ -186,18 +292,46 @@ Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in single ranging mode print_pal_error(Status); } + + if (Status == VL53L0X_ERROR_NONE) + { + Status = VL53L0X_SetLimitCheckValue(pMyDevice, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, (FixPoint1616_t)(0.25*65536)); + print_pal_error(Status); + } + if (Status == VL53L0X_ERROR_NONE) + { + Status = VL53L0X_SetLimitCheckValue(pMyDevice, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, (FixPoint1616_t)(16*65536)); + print_pal_error(Status); + } + if (Status == VL53L0X_ERROR_NONE) + { + Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(pMyDevice, 200000); + print_pal_error(Status); + } + + if(Status == VL53L0X_ERROR_NONE) + { + printf ("Call of VL53L0X_SetRangeFractionEnable\n"); + Status = VL53L0X_SetRangeFractionEnable(pMyDevice, 0x01); + print_pal_error(Status); + } if(Status == VL53L0X_ERROR_NONE) { printf ("Call of VL53L0X_StartMeasurement\n"); Status = VL53L0X_StartMeasurement(pMyDevice); + print_pal_error(Status); } if(Status == VL53L0X_ERROR_NONE) { + uint32_t accumulatingNbr = 24; + uint32_t measureTimes = 1; + uint32_t measurement; - uint32_t no_of_measurements = 32; + uint32_t no_of_measurements = accumulatingNbr * measureTimes; + uint16_t* pResults = (uint16_t*)malloc(sizeof(uint16_t) * no_of_measurements); @@ -209,9 +343,9 @@ if(Status == VL53L0X_ERROR_NONE) { Status = VL53L0X_GetRangingMeasurementData(pMyDevice, pRangingMeasurementData); - - *(pResults + measurement) = pRangingMeasurementData->RangeMilliMeter; - printf("In loop measurement %lu: %d\n", measurement, pRangingMeasurementData->RangeMilliMeter); +// Status = VL53L0X_PerformSingleRangingMeasurement(pMyDevice, pRangingMeasurementData); + *(pResults + measurement) = pRangingMeasurementData->RangeMilliMeter * 100 + pRangingMeasurementData->RangeFractionalPart / 64 * 25; + //printf("In loop measurement %lu: %d.%d\n", measurement, pRangingMeasurementData->RangeMilliMeter, pRangingMeasurementData->RangeFractionalPart/64*25); // Clear the interrupt VL53L0X_ClearInterruptMask(pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); @@ -223,10 +357,41 @@ if(Status == VL53L0X_ERROR_NONE) { - for(measurement=0; measurement<no_of_measurements; measurement++) + for(int times = 0; times < measureTimes; times++) { - printf("measurement %lu: %d\n", measurement, *(pResults + measurement)); + float resMeasured = 0; + float resAccumulated = 0; + float resFiltered = 0; + float resMax = 0; + float resMin = 0; + + for(uint16_t aNbr = 0; aNbr < accumulatingNbr ; aNbr++) + { + resMeasured = *(pResults + aNbr + (uint16_t)(times * (float)accumulatingNbr)); + resAccumulated += resMeasured; + if(resMeasured > resMax) + { + resMax = resMeasured; + } + if(resMeasured < resMin) + { + resMin = resMeasured; + } + } + + resFiltered = (resAccumulated - resMax - resMin) / (accumulatingNbr - 2) / 100; + printf("[%3d] %3.3f \n", times, resFiltered); +// if (times % 10 == 9) +// { +// printf("\n"); +// } } + + +// for(measurement=0; measurement<no_of_measurements; measurement++) +// { +// printf("measurement %lu: %d\n", measurement, *(pResults + measurement)); +// } } free(pResults); @@ -248,8 +413,15 @@ if(Status == VL53L0X_ERROR_NONE) Status = VL53L0X_ClearInterruptMask(pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); + while (true) if (pc.readable()) + { pc.putc(pc.getc()); + if(Status == VL53L0X_ERROR_NONE) + { + Status = rangingTest(pMyDevice); + } + } }