Kyung-hwan Kim
Single Ranging example with High Accuracy for the VL53L0X Time-of-Flight sensor
Dependencies: mbed vl53l0x_api
Diff: main.cpp
- Revision:
- 2:98cd8c47c1ad
- Parent:
- 0:b6867e1a23fa
--- 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);
+ }
+ }
}