Kyung-hwan Kim / Mbed 2 deprecated VL53L0X_SingleRanging_HighAccuracy_HANSL

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?

UserRevisionLine numberNew 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