VL53L0XSimplifier.cpp

00001 #include "mbed.h"
00002 #include "vl53l0x_api.h"
00003 #include "vl53l0x_platform.h"
00004 #include "vl53l0x_i2c_platform.h"
00005 #include "VL53L0XSimplifier.h"
00006 //CAMERA SENSOR 1FT
00007 Serial pc(SERIAL_TX, SERIAL_RX);
00008 
00009 void VL53Simplifier::print_pal_error(VL53L0X_Error Status){
00010     char buf[VL53L0X_MAX_STRING_LENGTH];
00011     VL53L0X_GetPalErrorString(Status, buf);
00012     printf("API Status: %i : %s\n", Status, buf);
00013 } 
00014 
00015 VL53L0X_Error VL53Simplifier::WaitMeasurementDataReady(VL53L0X_DEV Dev) {
00016     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00017     uint8_t NewDatReady=0;
00018     uint32_t LoopNb;
00019 
00020     // Wait until it finished
00021     // use timeout to avoid deadlock
00022     if (Status == VL53L0X_ERROR_NONE) {
00023         LoopNb = 0;
00024         do {
00025             Status = VL53L0X_GetMeasurementDataReady(Dev, &NewDatReady);
00026             if ((NewDatReady == 0x01) || Status != VL53L0X_ERROR_NONE) {
00027                 break;
00028             }
00029             LoopNb = LoopNb + 1;
00030             VL53L0X_PollingDelay(Dev);
00031         } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
00032 
00033         if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
00034             Status = VL53L0X_ERROR_TIME_OUT;
00035         }
00036     }
00037 
00038     return Status;
00039 }
00040 
00041 
00042 VL53L0X_Error VL53Simplifier::WaitStopCompleted(VL53L0X_DEV Dev) {
00043     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00044     uint32_t StopCompleted=0;
00045     uint32_t LoopNb;
00046 
00047     // Wait until it finished
00048     // use timeout to avoid deadlock
00049     if (Status == VL53L0X_ERROR_NONE) {
00050         LoopNb = 0;
00051         do {
00052             Status = VL53L0X_GetStopCompletedStatus(Dev, &StopCompleted);
00053             if ((StopCompleted == 0x00) || Status != VL53L0X_ERROR_NONE) {
00054                 break;
00055             }
00056             LoopNb = LoopNb + 1;
00057             VL53L0X_PollingDelay(Dev);
00058         } while (LoopNb < VL53L0X_DEFAULT_MAX_LOOP);
00059 
00060         if (LoopNb >= VL53L0X_DEFAULT_MAX_LOOP) {
00061             Status = VL53L0X_ERROR_TIME_OUT;
00062         }
00063 
00064     }
00065 
00066     return Status;
00067 }
00068 void VL53Simplifier::print_range_status(VL53L0X_RangingMeasurementData_t* pRangingMeasurementData){
00069     char buf[VL53L0X_MAX_STRING_LENGTH];
00070     uint8_t RangeStatus;
00071 
00072     /*
00073      * New Range Status: data is valid when pRangingMeasurementData->RangeStatus = 0
00074      */
00075 
00076     RangeStatus = pRangingMeasurementData->RangeStatus;
00077 
00078     VL53L0X_GetRangeStatusString(RangeStatus, buf);
00079     printf("Range Status: %i : %s\n", RangeStatus, buf);
00080 
00081 }
00082 void VL53Simplifier::runVL53() {
00083     // sensor camera
00084   
00085     VL53L0X_Error Status = VL53L0X_ERROR_NONE;
00086     VL53L0X_Dev_t MyDevice;
00087     VL53L0X_Dev_t *pMyDevice = &MyDevice;
00088     VL53L0X_Version_t                   Version;
00089     VL53L0X_Version_t                  *pVersion   = &Version;
00090     VL53L0X_DeviceInfo_t                DeviceInfo; 
00091     
00092     int32_t status_int;
00093     
00094     pc.printf("VL53L0X API Simple Ranging Example\r\n");
00095     
00096     // Initialize Comms
00097     pMyDevice->I2cDevAddr      = 0x52;
00098     pMyDevice->comms_type      =  1;
00099     pMyDevice->comms_speed_khz =  400;
00100     
00101     pc.printf("Init comms\r\n");
00102     
00103     if(Status == VL53L0X_ERROR_NONE)
00104     {
00105         status_int = VL53L0X_GetVersion(pVersion);
00106         if (status_int != 0)
00107             Status = VL53L0X_ERROR_CONTROL_INTERFACE;
00108     }
00109     pc.printf("VL53L0X API Version: %d.%d.%d (revision %d)\r\n", pVersion->major, pVersion->minor ,pVersion->build, pVersion->revision);
00110         
00111         int addr;
00112         
00113         addr = VL53L0X_scan();
00114         printf("Device found at: %i\r\n", addr);
00115         //uint8_t data;
00116         //data=0;
00117         if(Status == VL53L0X_ERROR_NONE)
00118     {
00119             printf ("Call of VL53L0X_DataInit\n");
00120             uint16_t osc_calibrate_val=0;
00121             Status = VL53L0X_RdWord(&MyDevice, VL53L0X_REG_OSC_CALIBRATE_VAL,&osc_calibrate_val);
00122             printf("%i\n",osc_calibrate_val);
00123       Status = VL53L0X_DataInit(&MyDevice); // Data initialization
00124       VL53Simplifier::print_pal_error(Status);
00125     }
00126 
00127         if(Status == VL53L0X_ERROR_NONE)
00128     {
00129         Status = VL53L0X_GetDeviceInfo(&MyDevice, &DeviceInfo);
00130         if(Status == VL53L0X_ERROR_NONE)
00131         {
00132             printf("VL53L0X_GetDeviceInfo:\n");
00133             printf("Device Name : %s\n", DeviceInfo.Name);
00134             printf("Device Type : %s\n", DeviceInfo.Type);
00135             printf("Device ID : %s\n", DeviceInfo.ProductId);
00136             printf("ProductRevisionMajor : %d\n", DeviceInfo.ProductRevisionMajor);
00137         printf("ProductRevisionMinor : %d\n", DeviceInfo.ProductRevisionMinor);
00138 
00139         if ((DeviceInfo.ProductRevisionMinor != 1) && (DeviceInfo.ProductRevisionMinor != 1)) {
00140             printf("Error expected cut 1.1 but found cut %d.%d\n",
00141                        DeviceInfo.ProductRevisionMajor, DeviceInfo.ProductRevisionMinor);
00142                 Status = VL53L0X_ERROR_NOT_SUPPORTED;
00143             }
00144         }
00145         VL53Simplifier::print_pal_error(Status);
00146     }
00147 
00148         VL53L0X_RangingMeasurementData_t    RangingMeasurementData;
00149         VL53L0X_RangingMeasurementData_t   *pRangingMeasurementData    = &RangingMeasurementData;
00150         Status = VL53L0X_ERROR_NONE;
00151         uint32_t refSpadCount;
00152         uint8_t isApertureSpads;
00153         uint8_t VhvSettings;
00154         uint8_t PhaseCal;
00155 
00156     if(Status == VL53L0X_ERROR_NONE)
00157         {
00158             printf ("Call of VL53L0X_StaticInit\n");
00159             Status = VL53L0X_StaticInit(pMyDevice); // Device Initialization
00160             // StaticInit will set interrupt by default
00161              VL53Simplifier::print_pal_error(Status);
00162         }
00163 
00164         if(Status == VL53L0X_ERROR_NONE)
00165         {
00166             printf ("Call of VL53L0X_PerformRefCalibration\n");
00167             Status = VL53L0X_PerformRefCalibration(pMyDevice,
00168                     &VhvSettings, &PhaseCal); // Device Initialization
00169              VL53Simplifier::print_pal_error(Status);
00170         }
00171 
00172         if(Status == VL53L0X_ERROR_NONE)
00173         {
00174             printf ("Call of VL53L0X_PerformRefSpadManagement\n");
00175             Status = VL53L0X_PerformRefSpadManagement(pMyDevice,
00176                     &refSpadCount, &isApertureSpads); // Device Initialization
00177              VL53Simplifier::print_pal_error(Status);
00178         }
00179 
00180         if(Status == VL53L0X_ERROR_NONE)
00181         {
00182 
00183             printf ("Call of VL53L0X_SetDeviceMode\n");
00184             Status = VL53L0X_SetDeviceMode(pMyDevice, VL53L0X_DEVICEMODE_CONTINUOUS_RANGING); // Setup in single ranging mode
00185              VL53Simplifier::print_pal_error(Status);
00186         }
00187 
00188         if(Status == VL53L0X_ERROR_NONE)
00189         {
00190             printf ("Call of VL53L0X_StartMeasurement\n");
00191             Status = VL53L0X_StartMeasurement(pMyDevice);
00192              VL53Simplifier::print_pal_error(Status);
00193         }
00194 
00195         if(Status == VL53L0X_ERROR_NONE)
00196         {
00197             uint32_t measurement;
00198             uint32_t no_of_measurements = 32;
00199 
00200             uint16_t* pResults = (uint16_t*)malloc(sizeof(uint16_t) * no_of_measurements);
00201 
00202             for(measurement=0; measurement<no_of_measurements; measurement++)
00203             {
00204 
00205                 Status = VL53Simplifier::WaitMeasurementDataReady(pMyDevice);
00206 
00207                 if(Status == VL53L0X_ERROR_NONE)
00208                 {
00209                     Status = VL53L0X_GetRangingMeasurementData(pMyDevice, pRangingMeasurementData);
00210 
00211                     *(pResults + measurement) = pRangingMeasurementData->RangeMilliMeter;
00212                     printf("In loop measurement %lu: %d\n", measurement, pRangingMeasurementData->RangeMilliMeter);
00213 
00214                     // Clear the interrupt
00215                     VL53L0X_ClearInterruptMask(pMyDevice, VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
00216                     VL53L0X_PollingDelay(pMyDevice);
00217                 } else {
00218                     break;
00219                 }
00220             }
00221 
00222             if(Status == VL53L0X_ERROR_NONE)
00223             {
00224                 for(measurement=0; measurement<no_of_measurements; measurement++)
00225                 {
00226                     printf("measurement %lu: %d\n", measurement, *(pResults + measurement));
00227                 }
00228             }
00229 
00230             free(pResults);
00231         }
00232 
00233 
00234         if(Status == VL53L0X_ERROR_NONE)
00235         {
00236             printf ("Call of VL53L0X_StopMeasurement\n");
00237             Status = VL53L0X_StopMeasurement(pMyDevice);
00238         }
00239 
00240         if(Status == VL53L0X_ERROR_NONE)
00241         {
00242             printf ("Wait Stop to be competed\n");
00243             Status = VL53Simplifier::WaitStopCompleted(pMyDevice);
00244         }
00245 
00246         if(Status == VL53L0X_ERROR_NONE)
00247         Status = VL53L0X_ClearInterruptMask(pMyDevice,
00248             VL53L0X_REG_SYSTEM_INTERRUPT_GPIO_NEW_SAMPLE_READY);
00249     while (true)
00250         if (pc.readable())
00251             pc.putc(pc.getc());
00252 }