VL6180X.cpp

00001 // VL6180X Time of Flight sensor interface
00002 
00003 #include "mbed.h"
00004 #include "VL6180X.h"
00005 
00006 VL6180X::VL6180X(PinName sda, PinName scl, uint8_t addr, PinName gpio0, 
00007     bool internalPullups)
00008     : i2c(sda, scl, internalPullups), gpio0Pin(gpio0)
00009 {
00010     // remember the address
00011     this->addr = addr;
00012     
00013     // start in single-shot distance mode
00014     distMode = 0;
00015     rangeStarted = false;
00016     
00017     // initially reset the sensor by holding GPIO0/CE low
00018     gpio0Pin.mode(PullNone);
00019     gpio0Pin.output();
00020     gpio0Pin.write(0);
00021 }
00022 
00023 VL6180X::~VL6180X() 
00024 {
00025 }
00026 
00027 bool VL6180X::init()
00028 {
00029     // hold reset low for 10ms
00030     gpio0Pin.output();
00031     gpio0Pin.write(0);
00032     wait_us(10000);
00033     
00034     // release reset and allow 10ms for the sensor to reboot
00035     gpio0Pin.input();
00036     wait_us(10000);
00037         
00038     // reset the I2C bus
00039     i2c.reset();
00040     
00041     // check that the sensor's reset register reads as '1'
00042     Timer t;
00043     t.start();
00044     while (readReg8(VL6180X_SYSTEM_FRESH_OUT_OF_RESET) != 1)
00045     {
00046         if (t.read_us() > 1000000)
00047             return false;
00048     }
00049     
00050     // clear reset flag
00051     writeReg8(VL6180X_SYSTEM_FRESH_OUT_OF_RESET, 0);
00052         
00053     // give the device 50ms before sending the startup sequence
00054     wait_ms(50);
00055     
00056     // Send the mandatory initial register assignments, per the manufacturer's app notes:
00057     // http://www.st.com/st-web-ui/static/active/en/resource/technical/document/application_note/DM00122600.pdf
00058     writeReg8(0x0207, 0x01);
00059     writeReg8(0x0208, 0x01);
00060     writeReg8(0x0096, 0x00);
00061     writeReg8(0x0097, 0xfd);
00062     writeReg8(0x00e3, 0x00);
00063     writeReg8(0x00e4, 0x04);
00064     writeReg8(0x00e5, 0x02);
00065     writeReg8(0x00e6, 0x01);
00066     writeReg8(0x00e7, 0x03);
00067     writeReg8(0x00f5, 0x02);
00068     writeReg8(0x00d9, 0x05);
00069     writeReg8(0x00db, 0xce);
00070     writeReg8(0x00dc, 0x03);
00071     writeReg8(0x00dd, 0xf8);
00072     writeReg8(0x009f, 0x00);
00073     writeReg8(0x00a3, 0x3c);
00074     writeReg8(0x00b7, 0x00);
00075     writeReg8(0x00bb, 0x3c);
00076     writeReg8(0x00b2, 0x09);
00077     writeReg8(0x00ca, 0x09);
00078     writeReg8(0x0198, 0x01);
00079     writeReg8(0x01b0, 0x17);
00080     writeReg8(0x01ad, 0x00);
00081     writeReg8(0x00ff, 0x05);
00082     writeReg8(0x0100, 0x05);
00083     writeReg8(0x0199, 0x05);
00084     writeReg8(0x01a6, 0x1b);
00085     writeReg8(0x01ac, 0x3e);
00086     writeReg8(0x01a7, 0x1f);
00087     writeReg8(0x0030, 0x00);    
00088     
00089     // allow time to settle
00090     wait_us(1000);
00091         
00092     // start the sample timer 
00093     sampleTimer.start();
00094 
00095     // success
00096     return true;
00097 }
00098  
00099 void VL6180X::setDefaults()
00100 {
00101     writeReg8(VL6180X_SYSTEM_GROUPED_PARAMETER_HOLD, 0x01);         // set parameter hold while updating settings
00102     
00103     writeReg8(VL6180X_SYSTEM_INTERRUPT_CONFIG_GPIO, 4);             // Enable interrupts from range only
00104     writeReg8(VL6180X_SYSTEM_MODE_GPIO1, 0x00);                     // Disable GPIO1
00105     writeReg8(VL6180X_SYSRANGE_VHV_REPEAT_RATE, 0xFF);              // Set auto calibration period (Max = 255)/(OFF = 0)
00106     writeReg8(VL6180X_SYSRANGE_INTERMEASUREMENT_PERIOD, 0x09);      // Set default ranging inter-measurement period to 100ms
00107     writeReg8(VL6180X_SYSRANGE_MAX_CONVERGENCE_TIME, 63);           // Max range convergence time 63ms
00108     writeReg8(VL6180X_SYSRANGE_RANGE_CHECK_ENABLES, 0x00);          // S/N disable, ignore disable, early convergence test disable
00109     writeReg16(VL6180X_SYSRANGE_EARLY_CONVERGENCE_ESTIMATE, 0x00);  // abort range measurement if convergence rate below this value
00110     writeReg8(VL6180X_READOUT_AVERAGING_SAMPLE_PERIOD, averagingSamplePeriod);  // Sample averaging period (1.3ms + N*64.5us)
00111     writeReg8(VL6180X_SYSRANGE_THRESH_LOW, 0x00);                   // low threshold
00112     writeReg8(VL6180X_SYSRANGE_THRESH_HIGH, 0xff);                  // high threshold
00113 
00114     writeReg8(VL6180X_SYSTEM_GROUPED_PARAMETER_HOLD, 0x00);         // end parameter hold
00115 
00116     // perform a single calibration; wait until it's done (within reason)
00117     Timer t;
00118     t.start();
00119     writeReg8(VL6180X_SYSRANGE_VHV_RECALIBRATE, 0x01);
00120     while (readReg8(VL6180X_SYSRANGE_VHV_RECALIBRATE) != 0)
00121     {
00122         // if we've been waiting too long, abort
00123         if (t.read_us() > 100000)
00124             break;
00125     }
00126 }
00127 
00128 void VL6180X::getID(struct VL6180X_ID &id)
00129 {
00130     id.model = readReg8(VL6180X_IDENTIFICATION_MODEL_ID);
00131     id.modelRevMajor = readReg8(VL6180X_IDENTIFICATION_MODEL_REV_MAJOR) & 0x07;
00132     id.modelRevMinor = readReg8(VL6180X_IDENTIFICATION_MODEL_REV_MINOR) & 0x07;
00133     id.moduleRevMajor = readReg8(VL6180X_IDENTIFICATION_MODULE_REV_MAJOR) & 0x07;
00134     id.moduleRevMinor = readReg8(VL6180X_IDENTIFICATION_MODULE_REV_MINOR) & 0x07;
00135     
00136     uint16_t date = readReg16(VL6180X_IDENTIFICATION_DATE);
00137     uint16_t time = readReg16(VL6180X_IDENTIFICATION_TIME) * 2;
00138     id.manufDate.year = 2010 + ((date >> 12) & 0x0f);
00139     id.manufDate.month = (date >> 8) & 0x0f;
00140     id.manufDate.day = (date >> 3) & 0x1f;
00141     id.manufDate.phase = uint8_t(date & 0x07);
00142     id.manufDate.hh = time/3600;
00143     id.manufDate.mm = (time % 3600) / 60;
00144     id.manufDate.ss = time % 60;
00145 }
00146 
00147 void VL6180X::continuousDistanceMode(bool on)
00148 {
00149     if (distMode != on)
00150     {
00151         // remember the new mode
00152         distMode = on;
00153         
00154         // Set continuous or single-shot mode.  If starting continuous
00155         // mode, set bits 0x01 (range mode = continuous) + 0x02 (start
00156         // collecting samples now).  If ending the mode, set all bits
00157         // to zero to select single-shot mode without starting a reading.
00158         if (on)
00159         {
00160             writeReg8(VL6180X_SYSTEM_INTERRUPT_CONFIG_GPIO, 4);   // Enable interrupts for ranging only
00161             writeReg8(VL6180X_SYSALS_INTERMEASUREMENT_PERIOD, 0); // minimum measurement interval (10ms)
00162             writeReg8(VL6180X_SYSRANGE_START, 0x03);
00163         }
00164         else
00165             writeReg8(VL6180X_SYSRANGE_START, 0x00);
00166     }
00167 }
00168 
00169 bool VL6180X::rangeReady()
00170 {
00171     // check if the status register says a sample is ready (bits 0-2/0x07)
00172     // or an error has occurred (bits 6-7/0xC0)
00173     return ((readReg8(VL6180X_RESULT_INTERRUPT_STATUS_GPIO) & 0xC7) != 0);
00174 }
00175  
00176 void VL6180X::startRangeReading()
00177 {
00178     // start a new range reading if one isn't already in progress
00179     if (!rangeStarted)
00180     {
00181         tSampleStart = sampleTimer.read_us();
00182         writeReg8(VL6180X_SYSTEM_INTERRUPT_CLEAR, 0x07);
00183         writeReg8(VL6180X_SYSRANGE_START, 0x00);
00184         writeReg8(VL6180X_SYSRANGE_START, 0x01);
00185         rangeStarted = true;
00186     }
00187 }
00188 
00189 int VL6180X::getRange(uint8_t &distance, uint32_t &tMid, uint32_t &dt, uint32_t timeout_us)
00190 {
00191     // start a reading if one isn't already in progress
00192     startRangeReading();
00193     
00194     // we're going to wait until this reading ends, so consider the
00195     // 'start' command consumed, no matter what happens next
00196     rangeStarted = false;
00197     
00198     // wait for the sample
00199     Timer t;
00200     t.start();
00201     for (;;)
00202     {
00203         // check for a sample
00204         if (rangeReady())
00205             break;
00206             
00207         // if we've exceeded the timeout, return failure
00208         if (t.read_us() > timeout_us)
00209         {
00210             writeReg8(VL6180X_SYSRANGE_START, 0x00);
00211             return -1;
00212         }
00213     }
00214     
00215     // check for errors
00216     uint8_t err = (readReg8(VL6180X_RESULT_RANGE_STATUS) >> 4) & 0x0F;
00217     
00218     // read the distance
00219     distance = readReg8(VL6180X_RESULT_RANGE_VAL);
00220     
00221     // Read the convergence time, and compute the overall sample time.
00222     // Per the data sheet, the total execution time is the sum of the
00223     // fixed 3.2ms pre-calculation time, the convergence time, and the
00224     // readout averaging time.  We can query the convergence time for
00225     // each reading from the sensor.  The averaging time is a controlled
00226     // by the READOUT_AVERAGING_SAMPLE_PERIOD setting, which we set to
00227     // our constant value averagingSamplePeriod.
00228     dt = 
00229         3200                                                // fixed 3.2ms pre-calculation period
00230         + readReg32(VL6180X_RESULT_RANGE_RETURN_CONV_TIME)  // convergence time
00231         + (1300 + 48*averagingSamplePeriod);                // readout averaging period
00232         
00233     // figure the midpoint of the sample time - the starting time
00234     // plus half the collection time
00235     tMid = tSampleStart + dt/2;
00236     
00237     // clear the data-ready interrupt
00238     writeReg8(VL6180X_SYSTEM_INTERRUPT_CLEAR, 0x07);
00239 
00240     // return the error code
00241     return err;
00242 }
00243 
00244 void VL6180X::getRangeStats(VL6180X_RangeStats &stats)
00245 {
00246     stats.returnRate = readReg16(VL6180X_RESULT_RANGE_RETURN_RATE);
00247     stats.refReturnRate = readReg16(VL6180X_RESULT_RANGE_REFERENCE_RATE);
00248     stats.returnCnt = readReg32(VL6180X_RESULT_RANGE_RETURN_SIGNAL_COUNT);
00249     stats.refReturnCnt = readReg32(VL6180X_RESULT_RANGE_REFERENCE_SIGNAL_COUNT);
00250     stats.ambCnt = readReg32(VL6180X_RESULT_RANGE_RETURN_AMB_COUNT);
00251     stats.refAmbCnt = readReg32(VL6180X_RESULT_RANGE_REFERENCE_AMB_COUNT);
00252     stats.convTime = readReg32(VL6180X_RESULT_RANGE_RETURN_CONV_TIME);
00253     stats.refConvTime = readReg32(VL6180X_RESULT_RANGE_REFERENCE_CONV_TIME);
00254 }
00255      
00256 uint8_t VL6180X::readReg8(uint16_t registerAddr)
00257 {
00258     // write the request - MSB+LSB of register address
00259     uint8_t data_write[2];
00260     data_write[0] = (registerAddr >> 8) & 0xFF;
00261     data_write[1] = registerAddr & 0xFF;
00262     if (i2c.write(addr << 1, data_write, 2, false))
00263         return 0x00;
00264 
00265     // read the result
00266     uint8_t data_read[1];
00267     if (i2c.read(addr << 1, data_read, 1))
00268         return 0x00;
00269     
00270     // return the result
00271     return data_read[0];
00272 }
00273  
00274 uint16_t VL6180X::readReg16(uint16_t registerAddr)
00275 {
00276     // write the request - MSB+LSB of register address
00277     uint8_t data_write[2];
00278     data_write[0] = (registerAddr >> 8) & 0xFF;
00279     data_write[1] = registerAddr & 0xFF;
00280     if (i2c.write(addr << 1, data_write, 2, false))
00281         return 0;
00282     
00283     // read the result
00284     uint8_t data_read[2];
00285     if (i2c.read(addr << 1, data_read, 2))
00286         return 00;
00287     
00288     // return the result
00289     return (data_read[0] << 8) | data_read[1];
00290 }
00291  
00292 uint32_t VL6180X::readReg32(uint16_t registerAddr)
00293 {
00294     // write the request - MSB+LSB of register address
00295     uint8_t data_write[2];
00296     data_write[0] = (registerAddr >> 8) & 0xFF;
00297     data_write[1] = registerAddr & 0xFF;
00298     if (i2c.write(addr << 1, data_write, 2, false))
00299         return 0;
00300     
00301     // read the result
00302     uint8_t data_read[4];
00303     if (i2c.read(addr << 1, data_read, 4))
00304         return 0;
00305     
00306     // return the result
00307     return (data_read[0] << 24) | (data_read[1] << 16) | (data_read[2] << 8) | data_read[1];
00308 }
00309  
00310 void VL6180X::writeReg8(uint16_t registerAddr, uint8_t data)
00311 {
00312     uint8_t data_write[3];
00313     data_write[0] = (registerAddr >> 8) & 0xFF;
00314     data_write[1] = registerAddr & 0xFF;
00315     data_write[2] = data & 0xFF; 
00316     i2c.write(addr << 1, data_write, 3);
00317 }
00318  
00319 void VL6180X::writeReg16(uint16_t registerAddr, uint16_t data)
00320 {
00321     uint8_t data_write[4];
00322     data_write[0] = (registerAddr >> 8) & 0xFF;
00323     data_write[1] = registerAddr & 0xFF;
00324     data_write[2] = (data >> 8) & 0xFF;
00325     data_write[3] = data & 0xFF; 
00326     i2c.write(addr << 1, data_write, 4); 
00327 }