Single Ranging example with High Accuracy for the VL53L0X Time-of-Flight sensor
Dependencies: mbed vl53l0x_api
main.cpp@2:98cd8c47c1ad, 2019-05-03 (annotated)
- Committer:
- adonisduo
- Date:
- Fri May 03 00:05:37 2019 +0000
- Revision:
- 2:98cd8c47c1ad
- Parent:
- 0:b6867e1a23fa
Released by HANSL
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mjarvisal | 0:b6867e1a23fa | 1 | #include "mbed.h" |
mjarvisal | 0:b6867e1a23fa | 2 | #include "vl53l0x_api.h" |
mjarvisal | 0:b6867e1a23fa | 3 | #include "vl53l0x_platform.h" |
mjarvisal | 0:b6867e1a23fa | 4 | #include "vl53l0x_i2c_platform.h" |
mjarvisal | 0:b6867e1a23fa | 5 | |
mjarvisal | 0:b6867e1a23fa | 6 | Serial pc(SERIAL_TX, SERIAL_RX); |
mjarvisal | 0:b6867e1a23fa | 7 | |
mjarvisal | 0:b6867e1a23fa | 8 | void print_pal_error(VL53L0X_Error Status){ |
mjarvisal | 0:b6867e1a23fa | 9 | char buf[VL53L0X_MAX_STRING_LENGTH]; |
mjarvisal | 0:b6867e1a23fa | 10 | VL53L0X_GetPalErrorString(Status, buf); |
mjarvisal | 0:b6867e1a23fa | 11 | printf("API Status: %i : %s\n", Status, buf); |
mjarvisal | 0:b6867e1a23fa | 12 | } |
mjarvisal | 0:b6867e1a23fa | 13 | |
mjarvisal | 0:b6867e1a23fa | 14 | void print_range_status(VL53L0X_RangingMeasurementData_t* pRangingMeasurementData){ |
mjarvisal | 0:b6867e1a23fa | 15 | char buf[VL53L0X_MAX_STRING_LENGTH]; |
mjarvisal | 0:b6867e1a23fa | 16 | uint8_t RangeStatus; |
mjarvisal | 0:b6867e1a23fa | 17 | |
mjarvisal | 0:b6867e1a23fa | 18 | /* |
mjarvisal | 0:b6867e1a23fa | 19 | * New Range Status: data is valid when pRangingMeasurementData->RangeStatus = 0 |
mjarvisal | 0:b6867e1a23fa | 20 | */ |
mjarvisal | 0:b6867e1a23fa | 21 | |
mjarvisal | 0:b6867e1a23fa | 22 | RangeStatus = pRangingMeasurementData->RangeStatus; |
mjarvisal | 0:b6867e1a23fa | 23 | |
mjarvisal | 0:b6867e1a23fa | 24 | VL53L0X_GetRangeStatusString(RangeStatus, buf); |
mjarvisal | 0:b6867e1a23fa | 25 | printf("Range Status: %i : %s\n", RangeStatus, buf); |
mjarvisal | 0:b6867e1a23fa | 26 | |
mjarvisal | 0:b6867e1a23fa | 27 | } |
mjarvisal | 0:b6867e1a23fa | 28 | |
mjarvisal | 0:b6867e1a23fa | 29 | |
mjarvisal | 0:b6867e1a23fa | 30 | VL53L0X_Error WaitMeasurementDataReady(VL53L0X_DEV Dev) { |
mjarvisal | 0:b6867e1a23fa | 31 | VL53L0X_Error Status = VL53L0X_ERROR_NONE; |
mjarvisal | 0:b6867e1a23fa | 32 | uint8_t NewDatReady=0; |
mjarvisal | 0:b6867e1a23fa | 33 | uint32_t LoopNb; |
mjarvisal | 0:b6867e1a23fa | 34 | |
mjarvisal | 0:b6867e1a23fa | 35 | // Wait until it finished |
mjarvisal | 0:b6867e1a23fa | 36 | // use timeout to avoid deadlock |
mjarvisal | 0:b6867e1a23fa | 37 | if (Status == VL53L0X_ERROR_NONE) { |
mjarvisal | 0:b6867e1a23fa | 38 | LoopNb = 0; |
mjarvisal | 0:b6867e1a23fa | 39 | do { |
mjarvisal | 0:b6867e1a23fa | 40 | Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDatReady); |
mjarvisal | 0:b6867e1a23fa | 41 | if ((NewDatReady == 0x01) || Status != VL53L0X_ERROR_NONE) { |
mjarvisal | 0:b6867e1a23fa | 42 | break; |
mjarvisal | 0:b6867e1a23fa | 43 | } |
mjarvisal | 0:b6867e1a23fa | 44 | LoopNb = LoopNb + 1; |
mjarvisal | 0:b6867e1a23fa | 45 | VL53L0X_PollingDelay(Dev); |
mjarvisal | 0:b6867e1a23fa | 46 | } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP); |
mjarvisal | 0:b6867e1a23fa | 47 | |
mjarvisal | 0:b6867e1a23fa | 48 | if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) { |
mjarvisal | 0:b6867e1a23fa | 49 | Status = VL53L0X_ERROR_TIME_OUT; |
mjarvisal | 0:b6867e1a23fa | 50 | } |
mjarvisal | 0:b6867e1a23fa | 51 | } |
mjarvisal | 0:b6867e1a23fa | 52 | |
mjarvisal | 0:b6867e1a23fa | 53 | return Status; |
mjarvisal | 0:b6867e1a23fa | 54 | } |
mjarvisal | 0:b6867e1a23fa | 55 | |
mjarvisal | 0:b6867e1a23fa | 56 | VL53L0X_Error WaitStopCompleted(VL53L0X_DEV Dev) { |
mjarvisal | 0:b6867e1a23fa | 57 | VL53L0X_Error Status = VL53L0X_ERROR_NONE; |
mjarvisal | 0:b6867e1a23fa | 58 | uint32_t StopCompleted=0; |
mjarvisal | 0:b6867e1a23fa | 59 | uint32_t LoopNb; |
mjarvisal | 0:b6867e1a23fa | 60 | |
mjarvisal | 0:b6867e1a23fa | 61 | // Wait until it finished |
mjarvisal | 0:b6867e1a23fa | 62 | // use timeout to avoid deadlock |
mjarvisal | 0:b6867e1a23fa | 63 | if (Status == VL53L0X_ERROR_NONE) { |
mjarvisal | 0:b6867e1a23fa | 64 | LoopNb = 0; |
mjarvisal | 0:b6867e1a23fa | 65 | do { |
mjarvisal | 0:b6867e1a23fa | 66 | Status = VL53L0X_GetStopCompletedStatus(Dev, &StopCompleted); |
mjarvisal | 0:b6867e1a23fa | 67 | if ((StopCompleted == 0x00) || Status != VL53L0X_ERROR_NONE) { |
mjarvisal | 0:b6867e1a23fa | 68 | break; |
mjarvisal | 0:b6867e1a23fa | 69 | } |
mjarvisal | 0:b6867e1a23fa | 70 | LoopNb = LoopNb + 1; |
mjarvisal | 0:b6867e1a23fa | 71 | VL53L0X_PollingDelay(Dev); |
mjarvisal | 0:b6867e1a23fa | 72 | } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP); |
mjarvisal | 0:b6867e1a23fa | 73 | |
mjarvisal | 0:b6867e1a23fa | 74 | if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) { |
mjarvisal | 0:b6867e1a23fa | 75 | Status = VL53L0X_ERROR_TIME_OUT; |
mjarvisal | 0:b6867e1a23fa | 76 | } |
mjarvisal | 0:b6867e1a23fa | 77 | |
mjarvisal | 0:b6867e1a23fa | 78 | } |
mjarvisal | 0:b6867e1a23fa | 79 | |
mjarvisal | 0:b6867e1a23fa | 80 | return Status; |
mjarvisal | 0:b6867e1a23fa | 81 | } |
mjarvisal | 0:b6867e1a23fa | 82 | |
adonisduo | 2:98cd8c47c1ad | 83 | VL53L0X_Error rangingTest(VL53L0X_Dev_t *pMyDevice) |
adonisduo | 2:98cd8c47c1ad | 84 | { |
adonisduo | 2:98cd8c47c1ad | 85 | VL53L0X_Error Status = VL53L0X_ERROR_NONE; |
adonisduo | 2:98cd8c47c1ad | 86 | VL53L0X_RangingMeasurementData_t RangingMeasurementData; |
adonisduo | 2:98cd8c47c1ad | 87 | int i; |
adonisduo | 2:98cd8c47c1ad | 88 | FixPoint1616_t LimitCheckCurrent; |
adonisduo | 2:98cd8c47c1ad | 89 | uint32_t refSpadCount; |
adonisduo | 2:98cd8c47c1ad | 90 | uint8_t isApertureSpads; |
adonisduo | 2:98cd8c47c1ad | 91 | uint8_t VhvSettings; |
adonisduo | 2:98cd8c47c1ad | 92 | uint8_t PhaseCal; |
adonisduo | 2:98cd8c47c1ad | 93 | |
adonisduo | 2:98cd8c47c1ad | 94 | if(Status == VL53L0X_ERROR_NONE) |
adonisduo | 2:98cd8c47c1ad | 95 | { |
adonisduo | 2:98cd8c47c1ad | 96 | printf ("Call of VL53L0X_StaticInit\n"); |
adonisduo | 2:98cd8c47c1ad | 97 | Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization |
adonisduo | 2:98cd8c47c1ad | 98 | print_pal_error(Status); |
adonisduo | 2:98cd8c47c1ad | 99 | } |
adonisduo | 2:98cd8c47c1ad | 100 | |
adonisduo | 2:98cd8c47c1ad | 101 | if(Status == VL53L0X_ERROR_NONE) |
adonisduo | 2:98cd8c47c1ad | 102 | { |
adonisduo | 2:98cd8c47c1ad | 103 | printf ("Call of VL53L0X_PerformRefCalibration\n"); |
adonisduo | 2:98cd8c47c1ad | 104 | Status = VL53L0X_PerformRefCalibration(pMyDevice, |
adonisduo | 2:98cd8c47c1ad | 105 | &VhvSettings, &PhaseCal); // Device Initialization |
adonisduo | 2:98cd8c47c1ad | 106 | print_pal_error(Status); |
adonisduo | 2:98cd8c47c1ad | 107 | } |
adonisduo | 2:98cd8c47c1ad | 108 | |
adonisduo | 2:98cd8c47c1ad | 109 | if(Status == VL53L0X_ERROR_NONE) // needed if a coverglass is used and no calibration has been performed |
adonisduo | 2:98cd8c47c1ad | 110 | { |
adonisduo | 2:98cd8c47c1ad | 111 | printf ("Call of VL53L0X_PerformRefSpadManagement\n"); |
adonisduo | 2:98cd8c47c1ad | 112 | Status = VL53L0X_PerformRefSpadManagement(pMyDevice, |
adonisduo | 2:98cd8c47c1ad | 113 | &refSpadCount, &isApertureSpads); // Device Initialization |
adonisduo | 2:98cd8c47c1ad | 114 | printf ("refSpadCount = %d, isApertureSpads = %d\n", refSpadCount, isApertureSpads); |
adonisduo | 2:98cd8c47c1ad | 115 | print_pal_error(Status); |
adonisduo | 2:98cd8c47c1ad | 116 | } |
adonisduo | 2:98cd8c47c1ad | 117 | |
adonisduo | 2:98cd8c47c1ad | 118 | if(Status == VL53L0X_ERROR_NONE) |
adonisduo | 2:98cd8c47c1ad | 119 | { |
adonisduo | 2:98cd8c47c1ad | 120 | |
adonisduo | 2:98cd8c47c1ad | 121 | // no need to do this when we use VL53L0X_PerformSingleRangingMeasurement |
adonisduo | 2:98cd8c47c1ad | 122 | printf ("Call of VL53L0X_SetDeviceMode\n"); |
adonisduo | 2:98cd8c47c1ad | 123 | Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_SINGLE_RANGING); // Setup in single ranging mode |
adonisduo | 2:98cd8c47c1ad | 124 | print_pal_error(Status); |
adonisduo | 2:98cd8c47c1ad | 125 | } |
adonisduo | 2:98cd8c47c1ad | 126 | |
adonisduo | 2:98cd8c47c1ad | 127 | // Enable/Disable Sigma and Signal check |
adonisduo | 2:98cd8c47c1ad | 128 | if (Status == VL53L0X_ERROR_NONE) { |
adonisduo | 2:98cd8c47c1ad | 129 | Status = VL53L0X_SetLimitCheckEnable(pMyDevice, |
adonisduo | 2:98cd8c47c1ad | 130 | VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, 1); |
adonisduo | 2:98cd8c47c1ad | 131 | } |
adonisduo | 2:98cd8c47c1ad | 132 | if (Status == VL53L0X_ERROR_NONE) { |
adonisduo | 2:98cd8c47c1ad | 133 | Status = VL53L0X_SetLimitCheckEnable(pMyDevice, |
adonisduo | 2:98cd8c47c1ad | 134 | VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, 1); |
adonisduo | 2:98cd8c47c1ad | 135 | } |
adonisduo | 2:98cd8c47c1ad | 136 | |
adonisduo | 2:98cd8c47c1ad | 137 | if (Status == VL53L0X_ERROR_NONE) { |
adonisduo | 2:98cd8c47c1ad | 138 | Status = VL53L0X_SetLimitCheckValue(pMyDevice, |
adonisduo | 2:98cd8c47c1ad | 139 | VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, |
adonisduo | 2:98cd8c47c1ad | 140 | (FixPoint1616_t)(0.25*65536)); |
adonisduo | 2:98cd8c47c1ad | 141 | } |
adonisduo | 2:98cd8c47c1ad | 142 | if (Status == VL53L0X_ERROR_NONE) { |
adonisduo | 2:98cd8c47c1ad | 143 | Status = VL53L0X_SetLimitCheckValue(pMyDevice, |
adonisduo | 2:98cd8c47c1ad | 144 | VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, |
adonisduo | 2:98cd8c47c1ad | 145 | (FixPoint1616_t)(18*65536)); |
adonisduo | 2:98cd8c47c1ad | 146 | } |
adonisduo | 2:98cd8c47c1ad | 147 | if (Status == VL53L0X_ERROR_NONE) { |
adonisduo | 2:98cd8c47c1ad | 148 | Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(pMyDevice, |
adonisduo | 2:98cd8c47c1ad | 149 | 200000); |
adonisduo | 2:98cd8c47c1ad | 150 | } |
adonisduo | 2:98cd8c47c1ad | 151 | |
adonisduo | 2:98cd8c47c1ad | 152 | if(Status == VL53L0X_ERROR_NONE) |
adonisduo | 2:98cd8c47c1ad | 153 | { |
adonisduo | 2:98cd8c47c1ad | 154 | printf ("Call of VL53L0X_SetRangeFractionEnable\n"); |
adonisduo | 2:98cd8c47c1ad | 155 | Status = VL53L0X_SetRangeFractionEnable(pMyDevice, 0x01); |
adonisduo | 2:98cd8c47c1ad | 156 | } |
adonisduo | 2:98cd8c47c1ad | 157 | /* |
adonisduo | 2:98cd8c47c1ad | 158 | * Step 4 : Test ranging mode |
adonisduo | 2:98cd8c47c1ad | 159 | */ |
adonisduo | 2:98cd8c47c1ad | 160 | |
adonisduo | 2:98cd8c47c1ad | 161 | if(Status == VL53L0X_ERROR_NONE) |
adonisduo | 2:98cd8c47c1ad | 162 | { |
adonisduo | 2:98cd8c47c1ad | 163 | for(i=0;i<100;i++){ |
adonisduo | 2:98cd8c47c1ad | 164 | //printf ("Call of VL53L0X_PerformSingleRangingMeasurement\n"); |
adonisduo | 2:98cd8c47c1ad | 165 | Status = VL53L0X_PerformSingleRangingMeasurement(pMyDevice, |
adonisduo | 2:98cd8c47c1ad | 166 | &RangingMeasurementData); |
adonisduo | 2:98cd8c47c1ad | 167 | |
adonisduo | 2:98cd8c47c1ad | 168 | //print_pal_error(Status); |
adonisduo | 2:98cd8c47c1ad | 169 | //print_range_status(&RangingMeasurementData); |
adonisduo | 2:98cd8c47c1ad | 170 | |
adonisduo | 2:98cd8c47c1ad | 171 | VL53L0X_GetLimitCheckCurrent(pMyDevice, |
adonisduo | 2:98cd8c47c1ad | 172 | VL53L0X_CHECKENABLE_RANGE_IGNORE_THRESHOLD, &LimitCheckCurrent); |
adonisduo | 2:98cd8c47c1ad | 173 | |
adonisduo | 2:98cd8c47c1ad | 174 | printf("RANGE IGNORE THRESHOLD: %f\n", (float)LimitCheckCurrent/65536.0f); |
adonisduo | 2:98cd8c47c1ad | 175 | |
adonisduo | 2:98cd8c47c1ad | 176 | |
adonisduo | 2:98cd8c47c1ad | 177 | if (Status != VL53L0X_ERROR_NONE) break; |
adonisduo | 2:98cd8c47c1ad | 178 | |
adonisduo | 2:98cd8c47c1ad | 179 | //printf("Measured distance: %i\n\n", RangingMeasurementData.RangeMilliMeter); |
adonisduo | 2:98cd8c47c1ad | 180 | |
adonisduo | 2:98cd8c47c1ad | 181 | |
adonisduo | 2:98cd8c47c1ad | 182 | } |
adonisduo | 2:98cd8c47c1ad | 183 | } |
adonisduo | 2:98cd8c47c1ad | 184 | return Status; |
adonisduo | 2:98cd8c47c1ad | 185 | } |
adonisduo | 2:98cd8c47c1ad | 186 | |
mjarvisal | 0:b6867e1a23fa | 187 | |
mjarvisal | 0:b6867e1a23fa | 188 | int main() |
mjarvisal | 0:b6867e1a23fa | 189 | { |
mjarvisal | 0:b6867e1a23fa | 190 | |
mjarvisal | 0:b6867e1a23fa | 191 | VL53L0X_Error Status = VL53L0X_ERROR_NONE; |
mjarvisal | 0:b6867e1a23fa | 192 | VL53L0X_Dev_t MyDevice; |
mjarvisal | 0:b6867e1a23fa | 193 | VL53L0X_Dev_t *pMyDevice = &MyDevice; |
mjarvisal | 0:b6867e1a23fa | 194 | VL53L0X_Version_t Version; |
mjarvisal | 0:b6867e1a23fa | 195 | VL53L0X_Version_t *pVersion = &Version; |
mjarvisal | 0:b6867e1a23fa | 196 | VL53L0X_DeviceInfo_t DeviceInfo; |
mjarvisal | 0:b6867e1a23fa | 197 | |
mjarvisal | 0:b6867e1a23fa | 198 | int32_t status_int; |
mjarvisal | 0:b6867e1a23fa | 199 | |
adonisduo | 2:98cd8c47c1ad | 200 | pc.baud(460800); |
mjarvisal | 0:b6867e1a23fa | 201 | pc.printf("VL53L0X API Simple Ranging Example\r\n"); |
mjarvisal | 0:b6867e1a23fa | 202 | |
mjarvisal | 0:b6867e1a23fa | 203 | // Initialize Comms |
mjarvisal | 0:b6867e1a23fa | 204 | pMyDevice->I2cDevAddr = 0x52; |
mjarvisal | 0:b6867e1a23fa | 205 | pMyDevice->comms_type = 1; |
mjarvisal | 0:b6867e1a23fa | 206 | pMyDevice->comms_speed_khz = 400; |
mjarvisal | 0:b6867e1a23fa | 207 | |
mjarvisal | 0:b6867e1a23fa | 208 | pc.printf("Init comms\r\n"); |
mjarvisal | 0:b6867e1a23fa | 209 | |
mjarvisal | 0:b6867e1a23fa | 210 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 211 | { |
mjarvisal | 0:b6867e1a23fa | 212 | status_int = VL53L0X_GetVersion(pVersion); |
mjarvisal | 0:b6867e1a23fa | 213 | if (status_int != 0) |
mjarvisal | 0:b6867e1a23fa | 214 | Status = VL53L0X_ERROR_CONTROL_INTERFACE; |
mjarvisal | 0:b6867e1a23fa | 215 | } |
mjarvisal | 0:b6867e1a23fa | 216 | pc.printf("VL53L0X API Version: %d.%d.%d (revision %d)\r\n", pVersion->major, pVersion->minor ,pVersion->build, pVersion->revision); |
mjarvisal | 0:b6867e1a23fa | 217 | |
adonisduo | 2:98cd8c47c1ad | 218 | int addr; |
adonisduo | 2:98cd8c47c1ad | 219 | |
adonisduo | 2:98cd8c47c1ad | 220 | addr = VL53L0X_scan(); |
adonisduo | 2:98cd8c47c1ad | 221 | printf("Device found at: %i\r\n", addr); |
adonisduo | 2:98cd8c47c1ad | 222 | //uint8_t data; |
adonisduo | 2:98cd8c47c1ad | 223 | //data=0; |
adonisduo | 2:98cd8c47c1ad | 224 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 225 | { |
adonisduo | 2:98cd8c47c1ad | 226 | printf ("Call of VL53L0X_DataInit\n"); |
adonisduo | 2:98cd8c47c1ad | 227 | uint16_t osc_calibrate_val=0; |
adonisduo | 2:98cd8c47c1ad | 228 | Status = VL53L0X_RdWord(&MyDevice, VL53L0X_REG_OSC_CALIBRATE_VAL,&osc_calibrate_val); |
adonisduo | 2:98cd8c47c1ad | 229 | printf("%i\n",osc_calibrate_val); |
adonisduo | 2:98cd8c47c1ad | 230 | Status = VL53L0X_DataInit(&MyDevice); // Data initialization |
adonisduo | 2:98cd8c47c1ad | 231 | print_pal_error(Status); |
mjarvisal | 0:b6867e1a23fa | 232 | } |
mjarvisal | 0:b6867e1a23fa | 233 | |
mjarvisal | 0:b6867e1a23fa | 234 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 235 | { |
mjarvisal | 0:b6867e1a23fa | 236 | Status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo); |
mjarvisal | 0:b6867e1a23fa | 237 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 238 | { |
mjarvisal | 0:b6867e1a23fa | 239 | printf("VL53L0X_GetDeviceInfo:\n"); |
mjarvisal | 0:b6867e1a23fa | 240 | printf("Device Name : %s\n", DeviceInfo.Name); |
mjarvisal | 0:b6867e1a23fa | 241 | printf("Device Type : %s\n", DeviceInfo.Type); |
mjarvisal | 0:b6867e1a23fa | 242 | printf("Device ID : %s\n", DeviceInfo.ProductId); |
mjarvisal | 0:b6867e1a23fa | 243 | printf("ProductRevisionMajor : %d\n", DeviceInfo.ProductRevisionMajor); |
adonisduo | 2:98cd8c47c1ad | 244 | printf("ProductRevisionMinor : %d\n", DeviceInfo.ProductRevisionMinor); |
mjarvisal | 0:b6867e1a23fa | 245 | |
adonisduo | 2:98cd8c47c1ad | 246 | if ((DeviceInfo.ProductRevisionMinor != 1) && (DeviceInfo.ProductRevisionMinor != 1)) |
adonisduo | 2:98cd8c47c1ad | 247 | { |
adonisduo | 2:98cd8c47c1ad | 248 | printf("Error expected cut 1.1 but found cut %d.%d\n", |
adonisduo | 2:98cd8c47c1ad | 249 | DeviceInfo.ProductRevisionMajor, DeviceInfo.ProductRevisionMinor); |
mjarvisal | 0:b6867e1a23fa | 250 | Status = VL53L0X_ERROR_NOT_SUPPORTED; |
mjarvisal | 0:b6867e1a23fa | 251 | } |
mjarvisal | 0:b6867e1a23fa | 252 | } |
mjarvisal | 0:b6867e1a23fa | 253 | print_pal_error(Status); |
mjarvisal | 0:b6867e1a23fa | 254 | } |
mjarvisal | 0:b6867e1a23fa | 255 | |
mjarvisal | 0:b6867e1a23fa | 256 | VL53L0X_RangingMeasurementData_t RangingMeasurementData; |
mjarvisal | 0:b6867e1a23fa | 257 | VL53L0X_RangingMeasurementData_t *pRangingMeasurementData = &RangingMeasurementData; |
mjarvisal | 0:b6867e1a23fa | 258 | Status = VL53L0X_ERROR_NONE; |
mjarvisal | 0:b6867e1a23fa | 259 | uint32_t refSpadCount; |
mjarvisal | 0:b6867e1a23fa | 260 | uint8_t isApertureSpads; |
mjarvisal | 0:b6867e1a23fa | 261 | uint8_t VhvSettings; |
mjarvisal | 0:b6867e1a23fa | 262 | uint8_t PhaseCal; |
mjarvisal | 0:b6867e1a23fa | 263 | |
adonisduo | 2:98cd8c47c1ad | 264 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 265 | { |
mjarvisal | 0:b6867e1a23fa | 266 | printf ("Call of VL53L0X_StaticInit\n"); |
mjarvisal | 0:b6867e1a23fa | 267 | Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization |
mjarvisal | 0:b6867e1a23fa | 268 | // StaticInit will set interrupt by default |
mjarvisal | 0:b6867e1a23fa | 269 | print_pal_error(Status); |
mjarvisal | 0:b6867e1a23fa | 270 | } |
mjarvisal | 0:b6867e1a23fa | 271 | |
mjarvisal | 0:b6867e1a23fa | 272 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 273 | { |
mjarvisal | 0:b6867e1a23fa | 274 | printf ("Call of VL53L0X_PerformRefCalibration\n"); |
mjarvisal | 0:b6867e1a23fa | 275 | Status = VL53L0X_PerformRefCalibration(pMyDevice, |
mjarvisal | 0:b6867e1a23fa | 276 | &VhvSettings, &PhaseCal); // Device Initialization |
mjarvisal | 0:b6867e1a23fa | 277 | print_pal_error(Status); |
mjarvisal | 0:b6867e1a23fa | 278 | } |
mjarvisal | 0:b6867e1a23fa | 279 | |
mjarvisal | 0:b6867e1a23fa | 280 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 281 | { |
mjarvisal | 0:b6867e1a23fa | 282 | printf ("Call of VL53L0X_PerformRefSpadManagement\n"); |
mjarvisal | 0:b6867e1a23fa | 283 | Status = VL53L0X_PerformRefSpadManagement(pMyDevice, |
mjarvisal | 0:b6867e1a23fa | 284 | &refSpadCount, &isApertureSpads); // Device Initialization |
mjarvisal | 0:b6867e1a23fa | 285 | print_pal_error(Status); |
mjarvisal | 0:b6867e1a23fa | 286 | } |
mjarvisal | 0:b6867e1a23fa | 287 | |
mjarvisal | 0:b6867e1a23fa | 288 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 289 | { |
mjarvisal | 0:b6867e1a23fa | 290 | |
mjarvisal | 0:b6867e1a23fa | 291 | printf ("Call of VL53L0X_SetDeviceMode\n"); |
mjarvisal | 0:b6867e1a23fa | 292 | Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in single ranging mode |
mjarvisal | 0:b6867e1a23fa | 293 | print_pal_error(Status); |
mjarvisal | 0:b6867e1a23fa | 294 | } |
adonisduo | 2:98cd8c47c1ad | 295 | |
adonisduo | 2:98cd8c47c1ad | 296 | if (Status == VL53L0X_ERROR_NONE) |
adonisduo | 2:98cd8c47c1ad | 297 | { |
adonisduo | 2:98cd8c47c1ad | 298 | Status = VL53L0X_SetLimitCheckValue(pMyDevice, VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE, (FixPoint1616_t)(0.25*65536)); |
adonisduo | 2:98cd8c47c1ad | 299 | print_pal_error(Status); |
adonisduo | 2:98cd8c47c1ad | 300 | } |
adonisduo | 2:98cd8c47c1ad | 301 | if (Status == VL53L0X_ERROR_NONE) |
adonisduo | 2:98cd8c47c1ad | 302 | { |
adonisduo | 2:98cd8c47c1ad | 303 | Status = VL53L0X_SetLimitCheckValue(pMyDevice, VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE, (FixPoint1616_t)(16*65536)); |
adonisduo | 2:98cd8c47c1ad | 304 | print_pal_error(Status); |
adonisduo | 2:98cd8c47c1ad | 305 | } |
adonisduo | 2:98cd8c47c1ad | 306 | if (Status == VL53L0X_ERROR_NONE) |
adonisduo | 2:98cd8c47c1ad | 307 | { |
adonisduo | 2:98cd8c47c1ad | 308 | Status = VL53L0X_SetMeasurementTimingBudgetMicroSeconds(pMyDevice, 200000); |
adonisduo | 2:98cd8c47c1ad | 309 | print_pal_error(Status); |
adonisduo | 2:98cd8c47c1ad | 310 | } |
adonisduo | 2:98cd8c47c1ad | 311 | |
adonisduo | 2:98cd8c47c1ad | 312 | if(Status == VL53L0X_ERROR_NONE) |
adonisduo | 2:98cd8c47c1ad | 313 | { |
adonisduo | 2:98cd8c47c1ad | 314 | printf ("Call of VL53L0X_SetRangeFractionEnable\n"); |
adonisduo | 2:98cd8c47c1ad | 315 | Status = VL53L0X_SetRangeFractionEnable(pMyDevice, 0x01); |
adonisduo | 2:98cd8c47c1ad | 316 | print_pal_error(Status); |
adonisduo | 2:98cd8c47c1ad | 317 | } |
mjarvisal | 0:b6867e1a23fa | 318 | |
mjarvisal | 0:b6867e1a23fa | 319 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 320 | { |
mjarvisal | 0:b6867e1a23fa | 321 | printf ("Call of VL53L0X_StartMeasurement\n"); |
mjarvisal | 0:b6867e1a23fa | 322 | Status = VL53L0X_StartMeasurement(pMyDevice); |
adonisduo | 2:98cd8c47c1ad | 323 | |
mjarvisal | 0:b6867e1a23fa | 324 | print_pal_error(Status); |
mjarvisal | 0:b6867e1a23fa | 325 | } |
mjarvisal | 0:b6867e1a23fa | 326 | |
mjarvisal | 0:b6867e1a23fa | 327 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 328 | { |
adonisduo | 2:98cd8c47c1ad | 329 | uint32_t accumulatingNbr = 24; |
adonisduo | 2:98cd8c47c1ad | 330 | uint32_t measureTimes = 1; |
adonisduo | 2:98cd8c47c1ad | 331 | |
mjarvisal | 0:b6867e1a23fa | 332 | uint32_t measurement; |
adonisduo | 2:98cd8c47c1ad | 333 | uint32_t no_of_measurements = accumulatingNbr * measureTimes; |
adonisduo | 2:98cd8c47c1ad | 334 | |
mjarvisal | 0:b6867e1a23fa | 335 | |
mjarvisal | 0:b6867e1a23fa | 336 | uint16_t* pResults = (uint16_t*)malloc(sizeof(uint16_t) * no_of_measurements); |
mjarvisal | 0:b6867e1a23fa | 337 | |
mjarvisal | 0:b6867e1a23fa | 338 | for(measurement=0; measurement<no_of_measurements; measurement++) |
mjarvisal | 0:b6867e1a23fa | 339 | { |
mjarvisal | 0:b6867e1a23fa | 340 | |
mjarvisal | 0:b6867e1a23fa | 341 | Status = WaitMeasurementDataReady(pMyDevice); |
mjarvisal | 0:b6867e1a23fa | 342 | |
mjarvisal | 0:b6867e1a23fa | 343 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 344 | { |
mjarvisal | 0:b6867e1a23fa | 345 | Status = VL53L0X_GetRangingMeasurementData(pMyDevice, pRangingMeasurementData); |
adonisduo | 2:98cd8c47c1ad | 346 | // Status = VL53L0X_PerformSingleRangingMeasurement(pMyDevice, pRangingMeasurementData); |
adonisduo | 2:98cd8c47c1ad | 347 | *(pResults + measurement) = pRangingMeasurementData->RangeMilliMeter * 100 + pRangingMeasurementData->RangeFractionalPart / 64 * 25; |
adonisduo | 2:98cd8c47c1ad | 348 | //printf("In loop measurement %lu: %d.%d\n", measurement, pRangingMeasurementData->RangeMilliMeter, pRangingMeasurementData->RangeFractionalPart/64*25); |
mjarvisal | 0:b6867e1a23fa | 349 | |
mjarvisal | 0:b6867e1a23fa | 350 | // Clear the interrupt |
mjarvisal | 0:b6867e1a23fa | 351 | VL53L0X_ClearInterruptMask(pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); |
mjarvisal | 0:b6867e1a23fa | 352 | VL53L0X_PollingDelay(pMyDevice); |
mjarvisal | 0:b6867e1a23fa | 353 | } else { |
mjarvisal | 0:b6867e1a23fa | 354 | break; |
mjarvisal | 0:b6867e1a23fa | 355 | } |
mjarvisal | 0:b6867e1a23fa | 356 | } |
mjarvisal | 0:b6867e1a23fa | 357 | |
mjarvisal | 0:b6867e1a23fa | 358 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 359 | { |
adonisduo | 2:98cd8c47c1ad | 360 | for(int times = 0; times < measureTimes; times++) |
mjarvisal | 0:b6867e1a23fa | 361 | { |
adonisduo | 2:98cd8c47c1ad | 362 | float resMeasured = 0; |
adonisduo | 2:98cd8c47c1ad | 363 | float resAccumulated = 0; |
adonisduo | 2:98cd8c47c1ad | 364 | float resFiltered = 0; |
adonisduo | 2:98cd8c47c1ad | 365 | float resMax = 0; |
adonisduo | 2:98cd8c47c1ad | 366 | float resMin = 0; |
adonisduo | 2:98cd8c47c1ad | 367 | |
adonisduo | 2:98cd8c47c1ad | 368 | for(uint16_t aNbr = 0; aNbr < accumulatingNbr ; aNbr++) |
adonisduo | 2:98cd8c47c1ad | 369 | { |
adonisduo | 2:98cd8c47c1ad | 370 | resMeasured = *(pResults + aNbr + (uint16_t)(times * (float)accumulatingNbr)); |
adonisduo | 2:98cd8c47c1ad | 371 | resAccumulated += resMeasured; |
adonisduo | 2:98cd8c47c1ad | 372 | if(resMeasured > resMax) |
adonisduo | 2:98cd8c47c1ad | 373 | { |
adonisduo | 2:98cd8c47c1ad | 374 | resMax = resMeasured; |
adonisduo | 2:98cd8c47c1ad | 375 | } |
adonisduo | 2:98cd8c47c1ad | 376 | if(resMeasured < resMin) |
adonisduo | 2:98cd8c47c1ad | 377 | { |
adonisduo | 2:98cd8c47c1ad | 378 | resMin = resMeasured; |
adonisduo | 2:98cd8c47c1ad | 379 | } |
adonisduo | 2:98cd8c47c1ad | 380 | } |
adonisduo | 2:98cd8c47c1ad | 381 | |
adonisduo | 2:98cd8c47c1ad | 382 | resFiltered = (resAccumulated - resMax - resMin) / (accumulatingNbr - 2) / 100; |
adonisduo | 2:98cd8c47c1ad | 383 | printf("[%3d] %3.3f \n", times, resFiltered); |
adonisduo | 2:98cd8c47c1ad | 384 | // if (times % 10 == 9) |
adonisduo | 2:98cd8c47c1ad | 385 | // { |
adonisduo | 2:98cd8c47c1ad | 386 | // printf("\n"); |
adonisduo | 2:98cd8c47c1ad | 387 | // } |
mjarvisal | 0:b6867e1a23fa | 388 | } |
adonisduo | 2:98cd8c47c1ad | 389 | |
adonisduo | 2:98cd8c47c1ad | 390 | |
adonisduo | 2:98cd8c47c1ad | 391 | // for(measurement=0; measurement<no_of_measurements; measurement++) |
adonisduo | 2:98cd8c47c1ad | 392 | // { |
adonisduo | 2:98cd8c47c1ad | 393 | // printf("measurement %lu: %d\n", measurement, *(pResults + measurement)); |
adonisduo | 2:98cd8c47c1ad | 394 | // } |
mjarvisal | 0:b6867e1a23fa | 395 | } |
mjarvisal | 0:b6867e1a23fa | 396 | |
mjarvisal | 0:b6867e1a23fa | 397 | free(pResults); |
mjarvisal | 0:b6867e1a23fa | 398 | } |
mjarvisal | 0:b6867e1a23fa | 399 | |
mjarvisal | 0:b6867e1a23fa | 400 | |
mjarvisal | 0:b6867e1a23fa | 401 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 402 | { |
mjarvisal | 0:b6867e1a23fa | 403 | printf ("Call of VL53L0X_StopMeasurement\n"); |
mjarvisal | 0:b6867e1a23fa | 404 | Status = VL53L0X_StopMeasurement(pMyDevice); |
mjarvisal | 0:b6867e1a23fa | 405 | } |
mjarvisal | 0:b6867e1a23fa | 406 | |
mjarvisal | 0:b6867e1a23fa | 407 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 408 | { |
mjarvisal | 0:b6867e1a23fa | 409 | printf ("Wait Stop to be competed\n"); |
mjarvisal | 0:b6867e1a23fa | 410 | Status = WaitStopCompleted(pMyDevice); |
mjarvisal | 0:b6867e1a23fa | 411 | } |
mjarvisal | 0:b6867e1a23fa | 412 | |
mjarvisal | 0:b6867e1a23fa | 413 | if(Status == VL53L0X_ERROR_NONE) |
mjarvisal | 0:b6867e1a23fa | 414 | Status = VL53L0X_ClearInterruptMask(pMyDevice, |
mjarvisal | 0:b6867e1a23fa | 415 | VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY); |
adonisduo | 2:98cd8c47c1ad | 416 | |
mjarvisal | 0:b6867e1a23fa | 417 | while (true) |
mjarvisal | 0:b6867e1a23fa | 418 | if (pc.readable()) |
adonisduo | 2:98cd8c47c1ad | 419 | { |
mjarvisal | 0:b6867e1a23fa | 420 | pc.putc(pc.getc()); |
adonisduo | 2:98cd8c47c1ad | 421 | if(Status == VL53L0X_ERROR_NONE) |
adonisduo | 2:98cd8c47c1ad | 422 | { |
adonisduo | 2:98cd8c47c1ad | 423 | Status = rangingTest(pMyDevice); |
adonisduo | 2:98cd8c47c1ad | 424 | } |
adonisduo | 2:98cd8c47c1ad | 425 | } |
mjarvisal | 0:b6867e1a23fa | 426 | } |
mjarvisal | 0:b6867e1a23fa | 427 |