VL53L1X.cpp

00001 #include "VL53L1X.h"
00002 #include "mbed.h"
00003  
00004 //Serial pc(USBTX,USBRX);
00005 //DigitalOut led1(LED1);
00006  
00007 uint8_t configBlock[] = {
00008   0x29, 0x02, 0x10, 0x00, 0x28, 0xBC, 0x7A, 0x81, //8
00009   0x80, 0x07, 0x95, 0x00, 0xED, 0xFF, 0xF7, 0xFD, //16
00010   0x9E, 0x0E, 0x00, 0x10, 0x01, 0x00, 0x00, 0x00, //24
00011   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x34, 0x00, //32
00012   0x28, 0x00, 0x0D, 0x0A, 0x00, 0x00, 0x00, 0x00, //40
00013   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x11, //48
00014   0x02, 0x00, 0x02, 0x08, 0x00, 0x08, 0x10, 0x01, //56
00015   0x01, 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00, 0x02, //64
00016   0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x0B, 0x00, //72
00017   0x00, 0x02, 0x0A, 0x21, 0x00, 0x00, 0x02, 0x00, //80
00018   0x00, 0x00, 0x00, 0xC8, 0x00, 0x00, 0x38, 0xFF, //88
00019   0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x91, 0x0F, //96
00020   0x00, 0xA5, 0x0D, 0x00, 0x80, 0x00, 0x0C, 0x08, //104
00021   0xB8, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x10, 0x00, //112
00022   0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x0F, //120
00023   0x0D, 0x0E, 0x0E, 0x01, 0x00, 0x02, 0xC7, 0xFF, //128
00024   0x8B, 0x00, 0x00, 0x00, 0x01, 0x01, 0x40 //129 - 135 (0x81 - 0x87)
00025 };
00026  
00027 VL53L1X::VL53L1X(PinName SDA, PinName SCL) : _i2c(SDA,SCL){
00028     //Set I2C fast and bring reset line high
00029    _i2c.frequency(400000);
00030     _deviceAddress = defaultAddress_VL53L1X << 1;
00031     }
00032     
00033 bool VL53L1X::begin()
00034 { 
00035   //Check the device ID
00036   uint16_t modelID = readRegister16(VL53L1_IDENTIFICATION__MODEL_ID);
00037   if (modelID != 0xEACC){
00038     return (false);
00039   }
00040   softReset();
00041  
00042   //Polls the bit 0 of the FIRMWARE__SYSTEM_STATUS register to see if the firmware is ready
00043   int counter = 0;
00044   int  Firmware = readRegister16(VL53L1_FIRMWARE__SYSTEM_STATUS);
00045   printf("Firmware = %x\r\n", Firmware);
00046   while ((Firmware & 0x01) == 0)
00047   {
00048     Firmware = readRegister16(VL53L1_FIRMWARE__SYSTEM_STATUS);
00049     printf("Firmware = %x\r\n", Firmware);
00050     if (counter++ == 100) return (false); //Sensor timed out
00051     wait(.1);
00052   }
00053  
00054   //Set I2C to 2.8V mode. In this mode 3.3V I2C is allowed.
00055   uint16_t result = readRegister16(VL53L1_PAD_I2C_HV__EXTSUP_CONFIG);
00056   result = (result & 0xFE) | 0x01;
00057   writeRegister16(VL53L1_PAD_I2C_HV__EXTSUP_CONFIG, result);
00058  
00059   //Gets trim resistors from chip
00060   for (uint16_t i = 0; i < 36; i++) {
00061       uint8_t regVal = readRegister(i + 1);
00062       configBlock[i] = regVal;
00063   }
00064   
00065   startMeasurement();
00066   return (true); //Sensor online!
00067 
00068 }
00069  
00070     
00071 void VL53L1X::startMeasurement(uint8_t offset)
00072 {
00073   offset = 0; //Start at a location within the configBlock array
00074   uint8_t address = 1 + offset; //Start at memory location 0x01, add offset
00075   char data_write[32];
00076   uint8_t leftToSend = sizeof(configBlock) - offset;
00077   while (leftToSend > 0)
00078   {
00079 
00080     data_write[0] = 0; //MSB of register address 
00081     data_write[1] = address; //LSB of register address 
00082     
00083     uint8_t toSend = 30; //Max I2C buffer on Arduino is 32, and we need 2 bytes for address
00084     if (toSend > leftToSend) toSend = leftToSend;
00085     for(int x = 0; x < toSend; x++)
00086     {
00087         data_write[x+2] = configBlock[x+address-1];
00088     }
00089     _i2c.write(_deviceAddress, data_write, toSend+2); 
00090 
00091     leftToSend -= toSend;
00092     address += toSend;
00093   }
00094 }
00095  
00096 bool VL53L1X::newDataReady(void)
00097 {
00098   int read = readRegister(VL53L1_GPIO__TIO_HV_STATUS);
00099   if (read != 0x03) return(true); //New measurement!
00100   return(false); //No new data
00101 }
00102  
00103 //Reset sensor via software
00104 void VL53L1X::softReset()
00105 {
00106   writeRegister(VL53L1_SOFT_RESET, 0x00); //Reset
00107   wait(.001); //Driver uses 100us
00108   writeRegister(VL53L1_SOFT_RESET, 0x01); //Exit reset
00109 }
00110  
00111 uint16_t VL53L1X::getDistance()
00112 {
00113   return (readRegister16(VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0));
00114 }
00115  
00116 uint16_t VL53L1X::getSignalRate()
00117 {
00118   //From vl53l1_api.c line 2041
00119   uint16_t reading = readRegister16(VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0);// << 9; //FIXPOINT97TOFIXPOINT1616
00120   //float signalRate = (float)reading/65536.0;
00121   return (reading);
00122 }
00123  
00124 void VL53L1X::setDistanceMode(uint8_t mode)
00125 {
00126     uint8_t periodA;
00127     uint8_t periodB;
00128     uint8_t phaseHigh;
00129     uint8_t phaseInit;
00130     
00131     switch (mode)
00132     {
00133         case 0:
00134           periodA = 0x07;
00135           periodB = 0x05;
00136           phaseHigh = 0x38;
00137           phaseInit = 6;
00138           break;
00139         case 1:
00140           periodA = 0x0B;
00141           periodB = 0x09;
00142           phaseHigh = 0x78;
00143           phaseInit = 10;
00144           break;
00145         case 2:
00146           periodA = 0x0F;
00147           periodB = 0x0D;
00148           phaseHigh = 0xB8;
00149           phaseInit = 14;
00150           break;
00151         //If user inputs wrong range, we default to long range
00152         default:
00153           periodA = 0x0F;
00154           periodB = 0x0D;
00155           phaseHigh = 0xB8;
00156           phaseInit = 14;
00157           break;
00158     }
00159     //timing
00160     writeRegister(VL53L1_RANGE_CONFIG__VCSEL_PERIOD_A, periodA);
00161     writeRegister(VL53L1_RANGE_CONFIG__VCSEL_PERIOD_B , periodB);
00162     writeRegister(VL53L1_RANGE_CONFIG__VALID_PHASE_HIGH, phaseHigh);
00163     
00164     //dynamic
00165     writeRegister(VL53L1_SD_CONFIG__WOI_SD0 , periodA);
00166     writeRegister(VL53L1_SD_CONFIG__WOI_SD1, periodB);
00167     writeRegister(VL53L1_SD_CONFIG__INITIAL_PHASE_SD0, phaseInit);
00168     writeRegister(VL53L1_SD_CONFIG__INITIAL_PHASE_SD1, phaseInit);
00169     
00170     _distanceMode = mode;
00171 }
00172 uint8_t VL53L1X::getDistanceMode()
00173 {
00174     return _distanceMode;
00175 }
00176  
00177 uint8_t VL53L1X::getRangeStatus()
00178 {
00179 #define VL53L1_DEVICEERROR_VCSELCONTINUITYTESTFAILURE  ( 1)
00180 #define VL53L1_DEVICEERROR_VCSELWATCHDOGTESTFAILURE    ( 2)
00181 #define VL53L1_DEVICEERROR_NOVHVVALUEFOUND             ( 3)
00182 #define VL53L1_DEVICEERROR_MSRCNOTARGET                ( 4)
00183 #define VL53L1_DEVICEERROR_RANGEPHASECHECK             ( 5)
00184 #define VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK         ( 6)
00185 #define VL53L1_DEVICEERROR_PHASECONSISTENCY            ( 7)
00186 #define VL53L1_DEVICEERROR_MINCLIP                     ( 8)
00187 #define VL53L1_DEVICEERROR_RANGECOMPLETE               ( 9)
00188 #define VL53L1_DEVICEERROR_ALGOUNDERFLOW               ( 10)
00189 #define VL53L1_DEVICEERROR_ALGOOVERFLOW                ( 11)
00190 #define VL53L1_DEVICEERROR_RANGEIGNORETHRESHOLD        ( 12)
00191 #define VL53L1_DEVICEERROR_USERROICLIP                 ( 13)
00192 #define VL53L1_DEVICEERROR_REFSPADCHARNOTENOUGHDPADS   ( 14)
00193 #define VL53L1_DEVICEERROR_REFSPADCHARMORETHANTARGET   ( 15)
00194 #define VL53L1_DEVICEERROR_REFSPADCHARLESSTHANTARGET   ( 16)
00195 #define VL53L1_DEVICEERROR_MULTCLIPFAIL                ( 17)
00196 #define VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY        ( 18)
00197 #define VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK ( 19)
00198 #define VL53L1_DEVICEERROR_EVENTCONSISTENCY            ( 20)
00199 #define VL53L1_DEVICEERROR_MINSIGNALEVENTCHECK         ( 21)
00200 #define VL53L1_DEVICEERROR_RANGECOMPLETE_MERGED_PULSE  ( 22)
00201  
00202 #define VL53L1_RANGESTATUS_RANGE_VALID       0 /*!<The Range is valid. */
00203 #define VL53L1_RANGESTATUS_SIGMA_FAIL        1 /*!<Sigma Fail. */
00204 #define VL53L1_RANGESTATUS_SIGNAL_FAIL       2 /*!<Signal fail. */
00205 #define VL53L1_RANGESTATUS_RANGE_VALID_MIN_RANGE_CLIPPED 3 /*!<Target is below minimum detection threshold. */
00206 #define VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL      4 /*!<Phase out of valid limits -  different to a wrap exit. */
00207 #define VL53L1_RANGESTATUS_HARDWARE_FAIL     5 /*!<Hardware fail. */
00208 #define VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL  6 /*!<The Range is valid but the wraparound check has not been done. */
00209 #define VL53L1_RANGESTATUS_WRAP_TARGET_FAIL     7 /*!<Wrapped target - no matching phase in other VCSEL period timing. */
00210 #define VL53L1_RANGESTATUS_PROCESSING_FAIL      8 /*!<Internal algo underflow or overflow in lite ranging. */
00211 #define VL53L1_RANGESTATUS_XTALK_SIGNAL_FAIL      9 /*!<Specific to lite ranging. */
00212 #define VL53L1_RANGESTATUS_SYNCRONISATION_INT     10 /*!<1st interrupt when starting ranging in back to back mode. Ignore data. */
00213 #define VL53L1_RANGESTATUS_RANGE_VALID_MERGED_PULSE   11 /*!<All Range ok but object is result of multiple pulses merging together.*/
00214 #define VL53L1_RANGESTATUS_TARGET_PRESENT_LACK_OF_SIGNAL  12 /*!<Used  by RQL  as different to phase fail. */
00215 #define VL53L1_RANGESTATUS_MIN_RANGE_FAIL     13 /*!<User ROI input is not valid e.g. beyond SPAD Array.*/
00216 #define VL53L1_RANGESTATUS_RANGE_INVALID      14 /*!<lld returned valid range but negative value ! */
00217 #define VL53L1_RANGESTATUS_NONE        255 /*!<No Update. */
00218  
00219   //Read status
00220   uint8_t measurementStatus = (readRegister(VL53L1_RESULT__RANGE_STATUS) & 0x1F);
00221   //Convert status from one to another - From vl53l1_api.c
00222   switch (measurementStatus) {
00223     case VL53L1_DEVICEERROR_GPHSTREAMCOUNT0READY:
00224       measurementStatus = VL53L1_RANGESTATUS_SYNCRONISATION_INT;
00225       break;
00226     case VL53L1_DEVICEERROR_RANGECOMPLETE_NO_WRAP_CHECK:
00227       measurementStatus = VL53L1_RANGESTATUS_RANGE_VALID_NO_WRAP_CHECK_FAIL;
00228       break;
00229     case VL53L1_DEVICEERROR_RANGEPHASECHECK:
00230       measurementStatus = VL53L1_RANGESTATUS_OUTOFBOUNDS_FAIL;
00231       break;
00232     case VL53L1_DEVICEERROR_MSRCNOTARGET:
00233       measurementStatus = VL53L1_RANGESTATUS_SIGNAL_FAIL;
00234       break;
00235     case VL53L1_DEVICEERROR_SIGMATHRESHOLDCHECK:
00236       measurementStatus = VL53L1_RANGESTATUS_SIGMA_FAIL;
00237       break;
00238     case VL53L1_DEVICEERROR_PHASECONSISTENCY:
00239       measurementStatus = VL53L1_RANGESTATUS_WRAP_TARGET_FAIL;
00240       break;
00241     case VL53L1_DEVICEERROR_RANGEIGNORETHRESHOLD:
00242       measurementStatus = VL53L1_RANGESTATUS_XTALK_SIGNAL_FAIL;
00243       break;
00244     case VL53L1_DEVICEERROR_MINCLIP:
00245       measurementStatus = VL53L1_RANGESTATUS_RANGE_VALID_MIN_RANGE_CLIPPED;
00246       break;
00247     case VL53L1_DEVICEERROR_RANGECOMPLETE:
00248       measurementStatus = VL53L1_RANGESTATUS_RANGE_VALID;
00249       break;
00250     default:
00251       measurementStatus = VL53L1_RANGESTATUS_NONE;
00252   }
00253  
00254   return measurementStatus;
00255 }
00256  
00257 uint8_t VL53L1X::readRegister(uint16_t registerAddr)
00258 {
00259   uint8_t data;
00260   char data_write[2];
00261   char data_read[1];
00262   data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
00263   data_write[1] = registerAddr & 0xFF; //LSB of register address 
00264   _i2c.write(_deviceAddress, data_write, 2,0); 
00265   _i2c.read(_deviceAddress,data_read,1,1);
00266   //Read Data from selected register
00267   data=data_read[0];
00268   return data;
00269 }
00270  
00271 uint16_t VL53L1X::readRegister16(uint16_t registerAddr)
00272 {
00273   uint8_t data_low;
00274   uint8_t data_high;
00275   uint16_t data;
00276  
00277   char data_write[2];
00278   char data_read[2];
00279   data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
00280   data_write[1] = registerAddr & 0xFF; //LSB of register address 
00281   _i2c.write(_deviceAddress, data_write, 2,0); 
00282   _i2c.read(_deviceAddress,data_read,2,1);
00283   data_high = data_read[0]; //Read Data from selected register
00284   data_low = data_read[1]; //Read Data from selected register
00285   data = (data_high << 8)|data_low;
00286  
00287   return data;
00288 }
00289  
00290 void VL53L1X::writeRegister(uint16_t registerAddr, uint8_t data)
00291 {
00292     char data_write[3];
00293     data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
00294     data_write[1] = registerAddr & 0xFF; //LSB of register address 
00295     data_write[2] = data & 0xFF; 
00296     _i2c.write(_deviceAddress, data_write, 3); 
00297 }
00298  
00299 void VL53L1X::writeRegister16(uint16_t registerAddr, uint16_t data)
00300 {
00301     char data_write[4];
00302     data_write[0] = (registerAddr >> 8) & 0xFF; //MSB of register address 
00303     data_write[1] = registerAddr & 0xFF; //LSB of register address 
00304     data_write[2] = (data >> 8) & 0xFF;
00305     data_write[3] = data & 0xFF; 
00306     _i2c.write(_deviceAddress, data_write, 4); 
00307 }
00308