mohamed achour / Mbed OS VL53L0X-mbedOS-master

ff

Dependencies:   vl53l0x_api libxDot-mbed5

main.cpp@2:8ac50bd2c7a1, 2017-04-21 (annotated)

Committer:
andcor02
Date:
Fri Apr 21 11:22:11 2017 +0000
Revision:
2:8ac50bd2c7a1
Parent:
0:b6867e1a23fa
Child:
3:83ec4984a7af
for dot

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
andcor02 2:8ac50bd2c7a1 6 Serial pc(USBTX, USBRX);
andcor02 2:8ac50bd2c7a1 7 DigitalOut led(LED1);
mjarvisal 0:b6867e1a23fa 8
mjarvisal 0:b6867e1a23fa 9 void print_pal_error(VL53L0X_Error Status){
mjarvisal 0:b6867e1a23fa 10 char buf[VL53L0X_MAX_STRING_LENGTH];
mjarvisal 0:b6867e1a23fa 11 VL53L0X_GetPalErrorString(Status, buf);
mjarvisal 0:b6867e1a23fa 12 printf("API Status: %i : %s\n", Status, buf);
mjarvisal 0:b6867e1a23fa 13 }
mjarvisal 0:b6867e1a23fa 14
mjarvisal 0:b6867e1a23fa 15 void print_range_status(VL53L0X_RangingMeasurementData_t* pRangingMeasurementData){
mjarvisal 0:b6867e1a23fa 16 char buf[VL53L0X_MAX_STRING_LENGTH];
mjarvisal 0:b6867e1a23fa 17 uint8_t RangeStatus;
mjarvisal 0:b6867e1a23fa 18
mjarvisal 0:b6867e1a23fa 19 /*
mjarvisal 0:b6867e1a23fa 20 * New Range Status: data is valid when pRangingMeasurementData->RangeStatus = 0
mjarvisal 0:b6867e1a23fa 21 */
mjarvisal 0:b6867e1a23fa 22
mjarvisal 0:b6867e1a23fa 23 RangeStatus = pRangingMeasurementData->RangeStatus;
mjarvisal 0:b6867e1a23fa 24
mjarvisal 0:b6867e1a23fa 25 VL53L0X_GetRangeStatusString(RangeStatus, buf);
mjarvisal 0:b6867e1a23fa 26 printf("Range Status: %i : %s\n", RangeStatus, buf);
mjarvisal 0:b6867e1a23fa 27
mjarvisal 0:b6867e1a23fa 28 }
mjarvisal 0:b6867e1a23fa 29
mjarvisal 0:b6867e1a23fa 30
mjarvisal 0:b6867e1a23fa 31 VL53L0X_Error WaitMeasurementDataReady(VL53L0X_DEV Dev) {
mjarvisal 0:b6867e1a23fa 32 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:b6867e1a23fa 33 uint8_t NewDatReady=0;
mjarvisal 0:b6867e1a23fa 34 uint32_t LoopNb;
mjarvisal 0:b6867e1a23fa 35
mjarvisal 0:b6867e1a23fa 36 // Wait until it finished
mjarvisal 0:b6867e1a23fa 37 // use timeout to avoid deadlock
mjarvisal 0:b6867e1a23fa 38 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:b6867e1a23fa 39 LoopNb = 0;
mjarvisal 0:b6867e1a23fa 40 do {
mjarvisal 0:b6867e1a23fa 41 Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDatReady);
mjarvisal 0:b6867e1a23fa 42 if ((NewDatReady == 0x01) || Status != VL53L0X_ERROR_NONE) {
mjarvisal 0:b6867e1a23fa 43 break;
mjarvisal 0:b6867e1a23fa 44 }
mjarvisal 0:b6867e1a23fa 45 LoopNb = LoopNb + 1;
mjarvisal 0:b6867e1a23fa 46 VL53L0X_PollingDelay(Dev);
mjarvisal 0:b6867e1a23fa 47 } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
mjarvisal 0:b6867e1a23fa 48
mjarvisal 0:b6867e1a23fa 49 if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
mjarvisal 0:b6867e1a23fa 50 Status = VL53L0X_ERROR_TIME_OUT;
mjarvisal 0:b6867e1a23fa 51 }
mjarvisal 0:b6867e1a23fa 52 }
mjarvisal 0:b6867e1a23fa 53
mjarvisal 0:b6867e1a23fa 54 return Status;
mjarvisal 0:b6867e1a23fa 55 }
mjarvisal 0:b6867e1a23fa 56
mjarvisal 0:b6867e1a23fa 57 VL53L0X_Error WaitStopCompleted(VL53L0X_DEV Dev) {
mjarvisal 0:b6867e1a23fa 58 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:b6867e1a23fa 59 uint32_t StopCompleted=0;
mjarvisal 0:b6867e1a23fa 60 uint32_t LoopNb;
mjarvisal 0:b6867e1a23fa 61
mjarvisal 0:b6867e1a23fa 62 // Wait until it finished
mjarvisal 0:b6867e1a23fa 63 // use timeout to avoid deadlock
mjarvisal 0:b6867e1a23fa 64 if (Status == VL53L0X_ERROR_NONE) {
mjarvisal 0:b6867e1a23fa 65 LoopNb = 0;
mjarvisal 0:b6867e1a23fa 66 do {
mjarvisal 0:b6867e1a23fa 67 Status = VL53L0X_GetStopCompletedStatus(Dev, &StopCompleted);
mjarvisal 0:b6867e1a23fa 68 if ((StopCompleted == 0x00) || Status != VL53L0X_ERROR_NONE) {
mjarvisal 0:b6867e1a23fa 69 break;
mjarvisal 0:b6867e1a23fa 70 }
mjarvisal 0:b6867e1a23fa 71 LoopNb = LoopNb + 1;
mjarvisal 0:b6867e1a23fa 72 VL53L0X_PollingDelay(Dev);
mjarvisal 0:b6867e1a23fa 73 } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
mjarvisal 0:b6867e1a23fa 74
mjarvisal 0:b6867e1a23fa 75 if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
mjarvisal 0:b6867e1a23fa 76 Status = VL53L0X_ERROR_TIME_OUT;
mjarvisal 0:b6867e1a23fa 77 }
mjarvisal 0:b6867e1a23fa 78
mjarvisal 0:b6867e1a23fa 79 }
mjarvisal 0:b6867e1a23fa 80
mjarvisal 0:b6867e1a23fa 81 return Status;
mjarvisal 0:b6867e1a23fa 82 }
mjarvisal 0:b6867e1a23fa 83
mjarvisal 0:b6867e1a23fa 84
mjarvisal 0:b6867e1a23fa 85 int main()
mjarvisal 0:b6867e1a23fa 86 {
mjarvisal 0:b6867e1a23fa 87
mjarvisal 0:b6867e1a23fa 88 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:b6867e1a23fa 89 VL53L0X_Dev_t MyDevice;
mjarvisal 0:b6867e1a23fa 90 VL53L0X_Dev_t *pMyDevice = &MyDevice;
mjarvisal 0:b6867e1a23fa 91 VL53L0X_Version_t Version;
mjarvisal 0:b6867e1a23fa 92 VL53L0X_Version_t *pVersion = &Version;
mjarvisal 0:b6867e1a23fa 93 VL53L0X_DeviceInfo_t DeviceInfo;
mjarvisal 0:b6867e1a23fa 94
mjarvisal 0:b6867e1a23fa 95 int32_t status_int;
mjarvisal 0:b6867e1a23fa 96
mjarvisal 0:b6867e1a23fa 97 pc.printf("VL53L0X API Simple Ranging Example\r\n");
mjarvisal 0:b6867e1a23fa 98
mjarvisal 0:b6867e1a23fa 99 // Initialize Comms
mjarvisal 0:b6867e1a23fa 100 pMyDevice->I2cDevAddr = 0x52;
mjarvisal 0:b6867e1a23fa 101 pMyDevice->comms_type = 1;
mjarvisal 0:b6867e1a23fa 102 pMyDevice->comms_speed_khz = 400;
mjarvisal 0:b6867e1a23fa 103
mjarvisal 0:b6867e1a23fa 104 pc.printf("Init comms\r\n");
mjarvisal 0:b6867e1a23fa 105
mjarvisal 0:b6867e1a23fa 106 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 107 {
mjarvisal 0:b6867e1a23fa 108 status_int = VL53L0X_GetVersion(pVersion);
mjarvisal 0:b6867e1a23fa 109 if (status_int != 0)
mjarvisal 0:b6867e1a23fa 110 Status = VL53L0X_ERROR_CONTROL_INTERFACE;
mjarvisal 0:b6867e1a23fa 111 }
mjarvisal 0:b6867e1a23fa 112 pc.printf("VL53L0X API Version: %d.%d.%d (revision %d)\r\n", pVersion->major, pVersion->minor ,pVersion->build, pVersion->revision);
mjarvisal 0:b6867e1a23fa 113
mjarvisal 0:b6867e1a23fa 114 int addr;
mjarvisal 0:b6867e1a23fa 115
mjarvisal 0:b6867e1a23fa 116 addr = VL53L0X_scan();
mjarvisal 0:b6867e1a23fa 117 printf("Device found at: %i\r\n", addr);
mjarvisal 0:b6867e1a23fa 118 //uint8_t data;
mjarvisal 0:b6867e1a23fa 119 //data=0;
mjarvisal 0:b6867e1a23fa 120 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 121 {
mjarvisal 0:b6867e1a23fa 122 printf ("Call of VL53L0X_DataInit\n");
mjarvisal 0:b6867e1a23fa 123 uint16_t osc_calibrate_val=0;
mjarvisal 0:b6867e1a23fa 124 Status = VL53L0X_RdWord(&MyDevice, VL53L0X_REG_OSC_CALIBRATE_VAL,&osc_calibrate_val);
mjarvisal 0:b6867e1a23fa 125 printf("%i\n",osc_calibrate_val);
mjarvisal 0:b6867e1a23fa 126 Status = VL53L0X_DataInit(&MyDevice); // Data initialization
mjarvisal 0:b6867e1a23fa 127 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 128 }
mjarvisal 0:b6867e1a23fa 129
mjarvisal 0:b6867e1a23fa 130 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 131 {
mjarvisal 0:b6867e1a23fa 132 Status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo);
mjarvisal 0:b6867e1a23fa 133 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 134 {
mjarvisal 0:b6867e1a23fa 135 printf("VL53L0X_GetDeviceInfo:\n");
mjarvisal 0:b6867e1a23fa 136 printf("Device Name : %s\n", DeviceInfo.Name);
mjarvisal 0:b6867e1a23fa 137 printf("Device Type : %s\n", DeviceInfo.Type);
mjarvisal 0:b6867e1a23fa 138 printf("Device ID : %s\n", DeviceInfo.ProductId);
mjarvisal 0:b6867e1a23fa 139 printf("ProductRevisionMajor : %d\n", DeviceInfo.ProductRevisionMajor);
mjarvisal 0:b6867e1a23fa 140 printf("ProductRevisionMinor : %d\n", DeviceInfo.ProductRevisionMinor);
mjarvisal 0:b6867e1a23fa 141
mjarvisal 0:b6867e1a23fa 142 if ((DeviceInfo.ProductRevisionMinor != 1) && (DeviceInfo.ProductRevisionMinor != 1)) {
mjarvisal 0:b6867e1a23fa 143 printf("Error expected cut 1.1 but found cut %d.%d\n",
mjarvisal 0:b6867e1a23fa 144 DeviceInfo.ProductRevisionMajor, DeviceInfo.ProductRevisionMinor);
mjarvisal 0:b6867e1a23fa 145 Status = VL53L0X_ERROR_NOT_SUPPORTED;
mjarvisal 0:b6867e1a23fa 146 }
mjarvisal 0:b6867e1a23fa 147 }
mjarvisal 0:b6867e1a23fa 148 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 149 }
mjarvisal 0:b6867e1a23fa 150
mjarvisal 0:b6867e1a23fa 151 VL53L0X_RangingMeasurementData_t RangingMeasurementData;
mjarvisal 0:b6867e1a23fa 152 VL53L0X_RangingMeasurementData_t *pRangingMeasurementData = &RangingMeasurementData;
mjarvisal 0:b6867e1a23fa 153 Status = VL53L0X_ERROR_NONE;
mjarvisal 0:b6867e1a23fa 154 uint32_t refSpadCount;
mjarvisal 0:b6867e1a23fa 155 uint8_t isApertureSpads;
mjarvisal 0:b6867e1a23fa 156 uint8_t VhvSettings;
mjarvisal 0:b6867e1a23fa 157 uint8_t PhaseCal;
mjarvisal 0:b6867e1a23fa 158
mjarvisal 0:b6867e1a23fa 159 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 160 {
mjarvisal 0:b6867e1a23fa 161 printf ("Call of VL53L0X_StaticInit\n");
mjarvisal 0:b6867e1a23fa 162 Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization
mjarvisal 0:b6867e1a23fa 163 // StaticInit will set interrupt by default
mjarvisal 0:b6867e1a23fa 164 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 165 }
mjarvisal 0:b6867e1a23fa 166
mjarvisal 0:b6867e1a23fa 167 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 168 {
mjarvisal 0:b6867e1a23fa 169 printf ("Call of VL53L0X_PerformRefCalibration\n");
mjarvisal 0:b6867e1a23fa 170 Status = VL53L0X_PerformRefCalibration(pMyDevice,
mjarvisal 0:b6867e1a23fa 171 &VhvSettings, &PhaseCal); // Device Initialization
mjarvisal 0:b6867e1a23fa 172 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 173 }
mjarvisal 0:b6867e1a23fa 174
mjarvisal 0:b6867e1a23fa 175 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 176 {
mjarvisal 0:b6867e1a23fa 177 printf ("Call of VL53L0X_PerformRefSpadManagement\n");
mjarvisal 0:b6867e1a23fa 178 Status = VL53L0X_PerformRefSpadManagement(pMyDevice,
mjarvisal 0:b6867e1a23fa 179 &refSpadCount, &isApertureSpads); // Device Initialization
mjarvisal 0:b6867e1a23fa 180 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 181 }
mjarvisal 0:b6867e1a23fa 182
mjarvisal 0:b6867e1a23fa 183 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 184 {
mjarvisal 0:b6867e1a23fa 185
mjarvisal 0:b6867e1a23fa 186 printf ("Call of VL53L0X_SetDeviceMode\n");
mjarvisal 0:b6867e1a23fa 187 Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in single ranging mode
mjarvisal 0:b6867e1a23fa 188 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 189 }
mjarvisal 0:b6867e1a23fa 190
mjarvisal 0:b6867e1a23fa 191 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 192 {
mjarvisal 0:b6867e1a23fa 193 printf ("Call of VL53L0X_StartMeasurement\n");
mjarvisal 0:b6867e1a23fa 194 Status = VL53L0X_StartMeasurement(pMyDevice);
mjarvisal 0:b6867e1a23fa 195 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 196 }
mjarvisal 0:b6867e1a23fa 197
mjarvisal 0:b6867e1a23fa 198 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 199 {
mjarvisal 0:b6867e1a23fa 200 uint32_t measurement;
andcor02 2:8ac50bd2c7a1 201 uint32_t no_of_measurements = 6000;
mjarvisal 0:b6867e1a23fa 202
mjarvisal 0:b6867e1a23fa 203 uint16_t* pResults = (uint16_t*)malloc(sizeof(uint16_t) * no_of_measurements);
mjarvisal 0:b6867e1a23fa 204
mjarvisal 0:b6867e1a23fa 205 for(measurement=0; measurement<no_of_measurements; measurement++)
mjarvisal 0:b6867e1a23fa 206 {
mjarvisal 0:b6867e1a23fa 207
mjarvisal 0:b6867e1a23fa 208 Status = WaitMeasurementDataReady(pMyDevice);
mjarvisal 0:b6867e1a23fa 209
mjarvisal 0:b6867e1a23fa 210 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 211 {
mjarvisal 0:b6867e1a23fa 212 Status = VL53L0X_GetRangingMeasurementData(pMyDevice, pRangingMeasurementData);
mjarvisal 0:b6867e1a23fa 213
mjarvisal 0:b6867e1a23fa 214 *(pResults + measurement) = pRangingMeasurementData->RangeMilliMeter;
mjarvisal 0:b6867e1a23fa 215 printf("In loop measurement %lu: %d\n", measurement, pRangingMeasurementData->RangeMilliMeter);
andcor02 2:8ac50bd2c7a1 216 wait(1);
andcor02 2:8ac50bd2c7a1 217 led=!led;
mjarvisal 0:b6867e1a23fa 218 // Clear the interrupt
mjarvisal 0:b6867e1a23fa 219 VL53L0X_ClearInterruptMask(pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
mjarvisal 0:b6867e1a23fa 220 VL53L0X_PollingDelay(pMyDevice);
mjarvisal 0:b6867e1a23fa 221 } else {
mjarvisal 0:b6867e1a23fa 222 break;
mjarvisal 0:b6867e1a23fa 223 }
mjarvisal 0:b6867e1a23fa 224 }
mjarvisal 0:b6867e1a23fa 225
mjarvisal 0:b6867e1a23fa 226 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 227 {
mjarvisal 0:b6867e1a23fa 228 for(measurement=0; measurement<no_of_measurements; measurement++)
mjarvisal 0:b6867e1a23fa 229 {
mjarvisal 0:b6867e1a23fa 230 printf("measurement %lu: %d\n", measurement, *(pResults + measurement));
mjarvisal 0:b6867e1a23fa 231 }
mjarvisal 0:b6867e1a23fa 232 }
mjarvisal 0:b6867e1a23fa 233
mjarvisal 0:b6867e1a23fa 234 free(pResults);
mjarvisal 0:b6867e1a23fa 235 }
mjarvisal 0:b6867e1a23fa 236
mjarvisal 0:b6867e1a23fa 237
mjarvisal 0:b6867e1a23fa 238 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 239 {
mjarvisal 0:b6867e1a23fa 240 printf ("Call of VL53L0X_StopMeasurement\n");
mjarvisal 0:b6867e1a23fa 241 Status = VL53L0X_StopMeasurement(pMyDevice);
mjarvisal 0:b6867e1a23fa 242 }
mjarvisal 0:b6867e1a23fa 243
mjarvisal 0:b6867e1a23fa 244 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 245 {
mjarvisal 0:b6867e1a23fa 246 printf ("Wait Stop to be competed\n");
mjarvisal 0:b6867e1a23fa 247 Status = WaitStopCompleted(pMyDevice);
mjarvisal 0:b6867e1a23fa 248 }
mjarvisal 0:b6867e1a23fa 249
mjarvisal 0:b6867e1a23fa 250 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 251 Status = VL53L0X_ClearInterruptMask(pMyDevice,
mjarvisal 0:b6867e1a23fa 252 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
mjarvisal 0:b6867e1a23fa 253 while (true)
mjarvisal 0:b6867e1a23fa 254 if (pc.readable())
mjarvisal 0:b6867e1a23fa 255 pc.putc(pc.getc());
mjarvisal 0:b6867e1a23fa 256 }
mjarvisal 0:b6867e1a23fa 257