mohamed achour / Mbed OS VL53L0X-mbedOS-master

ff

Dependencies:   vl53l0x_api libxDot-mbed5

main.cpp@0:b6867e1a23fa, 2016-08-23 (annotated)

Committer:
mjarvisal
Date:
Tue Aug 23 05:14:18 2016 +0000
Revision:
0:b6867e1a23fa
Child:
2:8ac50bd2c7a1
Initial release

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
mjarvisal 0:b6867e1a23fa 83
mjarvisal 0:b6867e1a23fa 84 int main()
mjarvisal 0:b6867e1a23fa 85 {
mjarvisal 0:b6867e1a23fa 86
mjarvisal 0:b6867e1a23fa 87 VL53L0X_Error Status = VL53L0X_ERROR_NONE;
mjarvisal 0:b6867e1a23fa 88 VL53L0X_Dev_t MyDevice;
mjarvisal 0:b6867e1a23fa 89 VL53L0X_Dev_t *pMyDevice = &MyDevice;
mjarvisal 0:b6867e1a23fa 90 VL53L0X_Version_t Version;
mjarvisal 0:b6867e1a23fa 91 VL53L0X_Version_t *pVersion = &Version;
mjarvisal 0:b6867e1a23fa 92 VL53L0X_DeviceInfo_t DeviceInfo;
mjarvisal 0:b6867e1a23fa 93
mjarvisal 0:b6867e1a23fa 94 int32_t status_int;
mjarvisal 0:b6867e1a23fa 95
mjarvisal 0:b6867e1a23fa 96 pc.printf("VL53L0X API Simple Ranging Example\r\n");
mjarvisal 0:b6867e1a23fa 97
mjarvisal 0:b6867e1a23fa 98 // Initialize Comms
mjarvisal 0:b6867e1a23fa 99 pMyDevice->I2cDevAddr = 0x52;
mjarvisal 0:b6867e1a23fa 100 pMyDevice->comms_type = 1;
mjarvisal 0:b6867e1a23fa 101 pMyDevice->comms_speed_khz = 400;
mjarvisal 0:b6867e1a23fa 102
mjarvisal 0:b6867e1a23fa 103 pc.printf("Init comms\r\n");
mjarvisal 0:b6867e1a23fa 104
mjarvisal 0:b6867e1a23fa 105 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 106 {
mjarvisal 0:b6867e1a23fa 107 status_int = VL53L0X_GetVersion(pVersion);
mjarvisal 0:b6867e1a23fa 108 if (status_int != 0)
mjarvisal 0:b6867e1a23fa 109 Status = VL53L0X_ERROR_CONTROL_INTERFACE;
mjarvisal 0:b6867e1a23fa 110 }
mjarvisal 0:b6867e1a23fa 111 pc.printf("VL53L0X API Version: %d.%d.%d (revision %d)\r\n", pVersion->major, pVersion->minor ,pVersion->build, pVersion->revision);
mjarvisal 0:b6867e1a23fa 112
mjarvisal 0:b6867e1a23fa 113 int addr;
mjarvisal 0:b6867e1a23fa 114
mjarvisal 0:b6867e1a23fa 115 addr = VL53L0X_scan();
mjarvisal 0:b6867e1a23fa 116 printf("Device found at: %i\r\n", addr);
mjarvisal 0:b6867e1a23fa 117 //uint8_t data;
mjarvisal 0:b6867e1a23fa 118 //data=0;
mjarvisal 0:b6867e1a23fa 119 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 120 {
mjarvisal 0:b6867e1a23fa 121 printf ("Call of VL53L0X_DataInit\n");
mjarvisal 0:b6867e1a23fa 122 uint16_t osc_calibrate_val=0;
mjarvisal 0:b6867e1a23fa 123 Status = VL53L0X_RdWord(&MyDevice, VL53L0X_REG_OSC_CALIBRATE_VAL,&osc_calibrate_val);
mjarvisal 0:b6867e1a23fa 124 printf("%i\n",osc_calibrate_val);
mjarvisal 0:b6867e1a23fa 125 Status = VL53L0X_DataInit(&MyDevice); // Data initialization
mjarvisal 0:b6867e1a23fa 126 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 127 }
mjarvisal 0:b6867e1a23fa 128
mjarvisal 0:b6867e1a23fa 129 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 130 {
mjarvisal 0:b6867e1a23fa 131 Status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo);
mjarvisal 0:b6867e1a23fa 132 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 133 {
mjarvisal 0:b6867e1a23fa 134 printf("VL53L0X_GetDeviceInfo:\n");
mjarvisal 0:b6867e1a23fa 135 printf("Device Name : %s\n", DeviceInfo.Name);
mjarvisal 0:b6867e1a23fa 136 printf("Device Type : %s\n", DeviceInfo.Type);
mjarvisal 0:b6867e1a23fa 137 printf("Device ID : %s\n", DeviceInfo.ProductId);
mjarvisal 0:b6867e1a23fa 138 printf("ProductRevisionMajor : %d\n", DeviceInfo.ProductRevisionMajor);
mjarvisal 0:b6867e1a23fa 139 printf("ProductRevisionMinor : %d\n", DeviceInfo.ProductRevisionMinor);
mjarvisal 0:b6867e1a23fa 140
mjarvisal 0:b6867e1a23fa 141 if ((DeviceInfo.ProductRevisionMinor != 1) && (DeviceInfo.ProductRevisionMinor != 1)) {
mjarvisal 0:b6867e1a23fa 142 printf("Error expected cut 1.1 but found cut %d.%d\n",
mjarvisal 0:b6867e1a23fa 143 DeviceInfo.ProductRevisionMajor, DeviceInfo.ProductRevisionMinor);
mjarvisal 0:b6867e1a23fa 144 Status = VL53L0X_ERROR_NOT_SUPPORTED;
mjarvisal 0:b6867e1a23fa 145 }
mjarvisal 0:b6867e1a23fa 146 }
mjarvisal 0:b6867e1a23fa 147 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 148 }
mjarvisal 0:b6867e1a23fa 149
mjarvisal 0:b6867e1a23fa 150 VL53L0X_RangingMeasurementData_t RangingMeasurementData;
mjarvisal 0:b6867e1a23fa 151 VL53L0X_RangingMeasurementData_t *pRangingMeasurementData = &RangingMeasurementData;
mjarvisal 0:b6867e1a23fa 152 Status = VL53L0X_ERROR_NONE;
mjarvisal 0:b6867e1a23fa 153 uint32_t refSpadCount;
mjarvisal 0:b6867e1a23fa 154 uint8_t isApertureSpads;
mjarvisal 0:b6867e1a23fa 155 uint8_t VhvSettings;
mjarvisal 0:b6867e1a23fa 156 uint8_t PhaseCal;
mjarvisal 0:b6867e1a23fa 157
mjarvisal 0:b6867e1a23fa 158 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 159 {
mjarvisal 0:b6867e1a23fa 160 printf ("Call of VL53L0X_StaticInit\n");
mjarvisal 0:b6867e1a23fa 161 Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization
mjarvisal 0:b6867e1a23fa 162 // StaticInit will set interrupt by default
mjarvisal 0:b6867e1a23fa 163 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 164 }
mjarvisal 0:b6867e1a23fa 165
mjarvisal 0:b6867e1a23fa 166 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 167 {
mjarvisal 0:b6867e1a23fa 168 printf ("Call of VL53L0X_PerformRefCalibration\n");
mjarvisal 0:b6867e1a23fa 169 Status = VL53L0X_PerformRefCalibration(pMyDevice,
mjarvisal 0:b6867e1a23fa 170 &VhvSettings, &PhaseCal); // Device Initialization
mjarvisal 0:b6867e1a23fa 171 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 172 }
mjarvisal 0:b6867e1a23fa 173
mjarvisal 0:b6867e1a23fa 174 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 175 {
mjarvisal 0:b6867e1a23fa 176 printf ("Call of VL53L0X_PerformRefSpadManagement\n");
mjarvisal 0:b6867e1a23fa 177 Status = VL53L0X_PerformRefSpadManagement(pMyDevice,
mjarvisal 0:b6867e1a23fa 178 &refSpadCount, &isApertureSpads); // Device Initialization
mjarvisal 0:b6867e1a23fa 179 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 180 }
mjarvisal 0:b6867e1a23fa 181
mjarvisal 0:b6867e1a23fa 182 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 183 {
mjarvisal 0:b6867e1a23fa 184
mjarvisal 0:b6867e1a23fa 185 printf ("Call of VL53L0X_SetDeviceMode\n");
mjarvisal 0:b6867e1a23fa 186 Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in single ranging mode
mjarvisal 0:b6867e1a23fa 187 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 188 }
mjarvisal 0:b6867e1a23fa 189
mjarvisal 0:b6867e1a23fa 190 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 191 {
mjarvisal 0:b6867e1a23fa 192 printf ("Call of VL53L0X_StartMeasurement\n");
mjarvisal 0:b6867e1a23fa 193 Status = VL53L0X_StartMeasurement(pMyDevice);
mjarvisal 0:b6867e1a23fa 194 print_pal_error(Status);
mjarvisal 0:b6867e1a23fa 195 }
mjarvisal 0:b6867e1a23fa 196
mjarvisal 0:b6867e1a23fa 197 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 198 {
mjarvisal 0:b6867e1a23fa 199 uint32_t measurement;
mjarvisal 0:b6867e1a23fa 200 uint32_t no_of_measurements = 32;
mjarvisal 0:b6867e1a23fa 201
mjarvisal 0:b6867e1a23fa 202 uint16_t* pResults = (uint16_t*)malloc(sizeof(uint16_t) * no_of_measurements);
mjarvisal 0:b6867e1a23fa 203
mjarvisal 0:b6867e1a23fa 204 for(measurement=0; measurement<no_of_measurements; measurement++)
mjarvisal 0:b6867e1a23fa 205 {
mjarvisal 0:b6867e1a23fa 206
mjarvisal 0:b6867e1a23fa 207 Status = WaitMeasurementDataReady(pMyDevice);
mjarvisal 0:b6867e1a23fa 208
mjarvisal 0:b6867e1a23fa 209 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 210 {
mjarvisal 0:b6867e1a23fa 211 Status = VL53L0X_GetRangingMeasurementData(pMyDevice, pRangingMeasurementData);
mjarvisal 0:b6867e1a23fa 212
mjarvisal 0:b6867e1a23fa 213 *(pResults + measurement) = pRangingMeasurementData->RangeMilliMeter;
mjarvisal 0:b6867e1a23fa 214 printf("In loop measurement %lu: %d\n", measurement, pRangingMeasurementData->RangeMilliMeter);
mjarvisal 0:b6867e1a23fa 215
mjarvisal 0:b6867e1a23fa 216 // Clear the interrupt
mjarvisal 0:b6867e1a23fa 217 VL53L0X_ClearInterruptMask(pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
mjarvisal 0:b6867e1a23fa 218 VL53L0X_PollingDelay(pMyDevice);
mjarvisal 0:b6867e1a23fa 219 } else {
mjarvisal 0:b6867e1a23fa 220 break;
mjarvisal 0:b6867e1a23fa 221 }
mjarvisal 0:b6867e1a23fa 222 }
mjarvisal 0:b6867e1a23fa 223
mjarvisal 0:b6867e1a23fa 224 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 225 {
mjarvisal 0:b6867e1a23fa 226 for(measurement=0; measurement<no_of_measurements; measurement++)
mjarvisal 0:b6867e1a23fa 227 {
mjarvisal 0:b6867e1a23fa 228 printf("measurement %lu: %d\n", measurement, *(pResults + measurement));
mjarvisal 0:b6867e1a23fa 229 }
mjarvisal 0:b6867e1a23fa 230 }
mjarvisal 0:b6867e1a23fa 231
mjarvisal 0:b6867e1a23fa 232 free(pResults);
mjarvisal 0:b6867e1a23fa 233 }
mjarvisal 0:b6867e1a23fa 234
mjarvisal 0:b6867e1a23fa 235
mjarvisal 0:b6867e1a23fa 236 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 237 {
mjarvisal 0:b6867e1a23fa 238 printf ("Call of VL53L0X_StopMeasurement\n");
mjarvisal 0:b6867e1a23fa 239 Status = VL53L0X_StopMeasurement(pMyDevice);
mjarvisal 0:b6867e1a23fa 240 }
mjarvisal 0:b6867e1a23fa 241
mjarvisal 0:b6867e1a23fa 242 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 243 {
mjarvisal 0:b6867e1a23fa 244 printf ("Wait Stop to be competed\n");
mjarvisal 0:b6867e1a23fa 245 Status = WaitStopCompleted(pMyDevice);
mjarvisal 0:b6867e1a23fa 246 }
mjarvisal 0:b6867e1a23fa 247
mjarvisal 0:b6867e1a23fa 248 if(Status == VL53L0X_ERROR_NONE)
mjarvisal 0:b6867e1a23fa 249 Status = VL53L0X_ClearInterruptMask(pMyDevice,
mjarvisal 0:b6867e1a23fa 250 VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
mjarvisal 0:b6867e1a23fa 251 while (true)
mjarvisal 0:b6867e1a23fa 252 if (pc.readable())
mjarvisal 0:b6867e1a23fa 253 pc.putc(pc.getc());
mjarvisal 0:b6867e1a23fa 254 }
mjarvisal 0:b6867e1a23fa 255