VCNL4010.cpp

00001 // VCNL4010 IR proximity sensor
00002 
00003 #include "mbed.h"
00004 #include "math.h"
00005 #include "VCNL4010.h"
00006 
00007 
00008 VCNL4010::VCNL4010(PinName sda, PinName scl, bool internalPullups, int iredCurrent)
00009     : i2c(sda, scl, internalPullups)
00010 {
00011     // Calculate the scaling factor with a minimum proximitiy count of 5.
00012     // In actual practice, the minimum will usually be a lot higher, but
00013     // this is a safe default that gives us valid distance calculations
00014     // across almost the whole possible range of count values.  (Why not
00015     // zero?  Because of the inverse relationship between distance and
00016     // brightness == proximity count.  1/0 isn't meaningful, so we have
00017     // to use a non-zero minimum in the scaling calculation.  5 is so
00018     // low that it'll probably never actually happen in real readings,
00019     // but still gives us a reasonable scaled range.)
00020     calibrating = false;
00021     minProxCount = 100;
00022     maxProxCount = 65535;
00023     parkProxCount = 20000;
00024     dcOffset = 0;
00025     lastProxCount = 0;
00026     calcScalingFactor();
00027     
00028     // remember the desired IRED current setting
00029     this->iredCurrent = iredCurrent;
00030 }
00031 
00032 // Initialize the sensor device
00033 void VCNL4010::init()
00034 {
00035     // debugging instrumentation
00036     printf("VCNL4010 initializing\r\n");
00037     
00038     // reset the I2C bus
00039     i2c.reset();
00040 
00041     // Set the proximity sampling rate to the fastest available rate of
00042     // 250 samples/second (4ms/sample).  This isn't quite fast enough for
00043     // perfect plunger motion tracking - a minimum sampling frequency of
00044     // 400/s is needed to avoid aliasing during the bounce-back phase of
00045     // release motions.  But the plunger-independent part of the code
00046     // does some data processing to tolerate aliasing for even slower
00047     // sensors than this one, so this isn't a showstopper.  Apart from
00048     // the potential for aliasing during fast motion, 250/s is plenty
00049     // fast enough for responsive input and smooth animation.
00050     writeReg(0x82, 0x07);
00051     
00052     // Set the current for the IR LED (the light source for proximity
00053     // measurements).  This is in units of 10mA, up to 200mA.  If the
00054     // parameter is zero in the configuration, apply a default.  Make
00055     // sure it's in range (1..20).
00056     //
00057     // Note that the nominal current level isn't the same as the actual
00058     // current load on the sensor's power supply.  The nominal current
00059     // set here is the instantaneous current the chip uses to generate
00060     // IR pulses.  The pulses have a low duty cycle, so the continuous
00061     // current drawn on the chip's power inputs is much lower.  The
00062     // data sheet says that the total continuous power supply current
00063     // drawn with the most power-hungry settings (IRED maxed out at
00064     // 200mA, sampling frequency maxed at 250 Hz) is only 4mA.  So
00065     // there's no need to worry about blowing a fuse on the USB port
00066     // or frying the KL25Z 3.3V regulator - the chip draws negligible
00067     // power in those terms, even at the maximum IRED setting.
00068     uint8_t cur = static_cast<uint8_t>(iredCurrent);
00069     cur = (cur == 0 ? 10 : cur < 1 ? 1 : cur > 20 ? 20 : cur);
00070     writeReg(0x83, cur);
00071 
00072     // disable self-timed measurements - we'll start measurements on demand
00073     writeReg(0x80, 0x00);
00074     
00075     // start the sample timer, which we use to gather timing statistics 
00076     sampleTimer.start();
00077 
00078     // debugging instrumentation
00079     printf("VCNL4010 initialization done\r\n");
00080 }
00081 
00082 // Start a proximity measurement.  This initiates a proximity reading
00083 // in the chip, and returns immediately, allowing the KL25Z to tend to
00084 // other tasks while waiting for the reading to complete.  proxReady()
00085 // can be used to poll for completion.
00086 void VCNL4010::startProxReading()
00087 {
00088     // set the prox_od (initiate proximity on demand) bit (0x08) in
00089     // the command register, if it's not already set
00090     uint8_t b = readReg(0x80);
00091     if ((b & 0x08) == 0)
00092     {
00093         tSampleStart = sampleTimer.read_us();
00094         writeReg(0x80, b | 0x08);
00095     }
00096 }
00097 
00098 // Check if a proximity sample is ready.  Implicitly starts a new reading
00099 // if one isn't already either completed or in progress.  Returns true if
00100 // a reading is ready, false if not.
00101 bool VCNL4010::proxReady()
00102 {
00103     // read the command register to get the status bits
00104     uint8_t b = readReg(0x80);
00105     
00106     // if the prox_data_rdy bit (0x20) is set, a reading is ready
00107     if ((b & 0x20) != 0)
00108         return true;
00109         
00110     // Not ready.  Since the caller is polling, they must expect a reading
00111     // to be in progress; if not, start one now.  A reading in progress is
00112     // indicated and initiated by the prox_od bit 
00113     if ((b & 0x08) == 0)
00114     {
00115         tSampleStart = sampleTimer.read_us();
00116         writeReg(0x80, b | 0x08);
00117     }
00118         
00119     // a reading is available if the prox_data_rdy (0x08) is set
00120     return (b & 0x20) != 0;
00121 }
00122 
00123 // Read the current proximity reading.  If a reading isn't ready, 
00124 // we'll block until one is, up to the specified timeout interval.
00125 // Returns zero if a reading was successfully retrieved, or a
00126 // non-zero error code if a timeout or error occurs.
00127 //
00128 // Note that the returned proximity count value is the raw reading
00129 // from the sensor, which indicates the intensity of the reflected
00130 // light detected on the sensor, on an abstract scale from 0 to
00131 // 65535.  The proximity count is inversely related to the distance
00132 // to the target, but the relationship also depends upon many other
00133 // factors, such as the size and reflectivity of the target, ambient
00134 // light, and internal reflections within the sensor itself and
00135 // within the overall apparatus.
00136 int VCNL4010::getProx(int &proxCount,
00137     uint32_t &tMid, uint32_t &dt, uint32_t timeout_us)
00138 {
00139     // If the chip isn't responding, try resetting it.  I2C will
00140     // generally report 0xFF on all byte reads when a device isn't
00141     // responding to commands, since the pull-up resistors on SDA
00142     // will make all data bits look like '1' on read.  It's
00143     // conceivable that a device could lock up while holding SDA
00144     // low, too, so a value of 0x00 could also be reported.  So to
00145     // sense if the device is answering, we should try reading a
00146     // register that, when things are working properly, should
00147     // always hold a value that's not either 0x00 or 0xFF.  For
00148     // the VCNL4010, we can read the product ID register, which
00149     // should report ID value 0x21 per the data sheet.  The low
00150     // nybble is a product revision number, so we shouldn't
00151     // insist on the value 0x21 - it could be 0x22 or 0x23, etc,
00152     // in future revisions of this chip.  But in any case, the
00153     // register should definitely not be 0x00 or 0xFF, so it's
00154     // a good solid test.
00155     uint8_t prodId = readReg(0x81);
00156     if (prodId == 0x00 || prodId == 0xFF)
00157     {
00158         // try resetting the chip
00159         init();
00160         
00161         // check if that cleared the problem; if not, give up and
00162         // return an error
00163         prodId = readReg(0x81);
00164         if (prodId == 0x00 || prodId == 0xFF)
00165             return 1;
00166     }
00167     
00168     // wait for the sample
00169     Timer t;
00170     t.start();
00171     for (;;)
00172     {
00173         // check for a sample
00174         if (proxReady())
00175             break;
00176             
00177         // if we've exceeded the timeout, return failure
00178         if (t.read_us() > timeout_us)
00179             return -1;
00180     }
00181     
00182     // figure the time since we initiated the reading
00183     dt = sampleTimer.read_us() - tSampleStart;
00184     
00185     // figure the midpoint time
00186     tMid = tSampleStart + dt/2;
00187 
00188     // read the result from the sensor, as a 16-bit proximity count value    
00189     int N = (static_cast<int>(readReg(0x87)) << 8) | readReg(0x88);
00190     
00191     // remember the last raw reading
00192     lastProxCount = N;
00193     
00194     // start a new reading, so that the sensor is collecting the next
00195     // reading concurrently with the time-consuming floating-point math
00196     // we're about to do
00197     startProxReading();
00198     
00199     // if calibration is in progress, note the new min/max proximity
00200     // count readings, if applicable
00201     if (calibrating) 
00202     {
00203         if (N < minProxCount)
00204             minProxCount = N;
00205         if (N > maxProxCount)
00206             maxProxCount = N;
00207     }
00208     
00209     // report the raw count back to the caller
00210     proxCount = N;
00211     
00212     // success
00213     return 0;
00214 }
00215 
00216 // Restore the saved calibration data from the configuration
00217 void VCNL4010::restoreCalibration(Config &config)
00218 {
00219     // remember the calibrated minimum proximity count
00220     this->minProxCount = config.plunger.cal.raw0;
00221     this->maxProxCount = config.plunger.cal.raw1;
00222     this->parkProxCount = config.plunger.cal.raw2;
00223     
00224     // figure the scaling factor for distance calculations
00225     calcScalingFactor();
00226 }
00227 
00228 // Begin calibration    
00229 void VCNL4010::beginCalibration()
00230 {
00231     // reset the min/max proximity count to the last reading
00232     calibrating = true;
00233     minProxCount = lastProxCount;
00234     maxProxCount = lastProxCount;
00235     parkProxCount = lastProxCount;
00236 }
00237 
00238 // End calibration
00239 void VCNL4010::endCalibration(Config &config)
00240 {
00241     // save the proximity count range data from the calibration in the
00242     // caller's configuration, so that we can restore the scaling
00243     // factor calculation on the next boot
00244     config.plunger.cal.raw0 = minProxCount;
00245     config.plunger.cal.raw1 = maxProxCount;
00246     config.plunger.cal.raw2 = parkProxCount;
00247     
00248     // calculate the new scaling factor for conversions to distance
00249     calcScalingFactor();
00250     
00251     // Set the new calibration range in distance units.  The range
00252     // in distance units is fixed, since we choose the scaling factor
00253     // specifically to cover the fixed range.
00254     config.plunger.cal.zero = 10922;
00255     config.plunger.cal.min = 0;
00256     config.plunger.cal.max = 65535;
00257     
00258     // we're no longer calibrating
00259     calibrating = false;
00260 }
00261 
00262 // Power law function for the relationship between sensor count
00263 // readings and distance.  For our distance calculations, we use
00264 // this relationship:
00265 //
00266 //    distance = <scaling factor> * 1/power(count - <DC offset>) + <scaling offset>
00267 //
00268 // where all of the constants in <angle brackets> are determined
00269 // through calibration.
00270 //
00271 // We use the square root of the count as our power law relation.
00272 // This was determined empirically (based on observation).  This is
00273 // also the power law we'd expect from a naive application of physics,
00274 // on the principle that the observed brightness of a point light
00275 // source varies inversely with the square of the distance.
00276 //
00277 // The VCNL4010 data sheet doesn't specify a formulaic relationship,
00278 // which isn't surprising given that the relationship is undoubtedly
00279 // much more complex than just a power law equation, and also because
00280 // Vishay doesn't market this chip as a distance sensor in the first
00281 // place.  It's a *proximity* sensor, which means it's only meant to
00282 // answer a yes/no question, "is an object within range?", and not
00283 // the quantitative question "how far?".  So there's no reason for
00284 // Vishay to specify a precise relationship between distance and
00285 // brightness; all we have to know is that there's some kind of
00286 // inverse relationship, since beyond that, everything's just
00287 // relative.  The data sheet does at least offer a (low-res) graph
00288 // of the distance-vs-proximity-count relationship under one set of
00289 // test conditions, and interestingly, that graph suggests a rather
00290 // different power law, more like ~1/distance^3.1.  The graph also
00291 // makes it clear that the response isn't uniform - it doesn't
00292 // follow *any* power law exactly, but is something more complex
00293 // than that.  This is another non-surprise, given that environmental
00294 // factors will inevitably confound the readings to some degree. 
00295 //
00296 // At any rate, in the data I've gathered, it seems that a simple 1/R^2
00297 // power law is pretty close to reality, so I'm using that.  (Brightness
00298 // varies with 1/R^2, so distance varies with 1/sqrt(brightness).)  If
00299 // this turns out to produce noticeably non-linear results in other
00300 // people's installations, we might have to revisit this with something
00301 // more customized to the local setup.  For example, we could gather
00302 // calibration data points across the whole plunger travel range and
00303 // then do a best-fit calculation to determine the best exponent
00304 // (which would still assume that there's *some* 1/R^x relationship
00305 // for some exponent x, but it wouldn't assume it's necessarily R^2.)
00306 // 
00307 static inline float power(int x)
00308 {
00309     return sqrtf(static_cast<float>(x)); 
00310 }
00311 
00312 // convert from a raw sensor count value to distance units, using our
00313 // current calibration data
00314 int VCNL4010::countToDistance(int count)
00315 {
00316     // remove the DC offset from teh signal
00317     count -= dcOffset;
00318     
00319     // if the adjusted count (excess of DC offset) is zero or negative,
00320     // peg it to the minimum end = maximum retraction point
00321     if (count <= 0)
00322         return 65535;
00323 
00324     // figure the distance based on our inverse power curve
00325     float d = scalingFactor/power(count) + scalingOffset;
00326     
00327     // constrain it to the valid range and convert to int for return
00328     return d < 0.0f ? 0 : d > 65535.0f ? 65535 : static_cast<int>(d);
00329 }
00330 
00331 // Calculate the scaling factors for our power-law formula for
00332 // converting proximity count (brightness) readings to distances.
00333 // We call this upon completing a new calibration pass, and during
00334 // initialization, when loading saved calibration data.
00335 void VCNL4010::calcScalingFactor()
00336 {
00337     // Don't let the minimum go below 100.  The inverse relationship makes
00338     // the calculation meaningless at zero and unstable at very small
00339     // count values, so we need a reasonable floor to keep things in a
00340     // usable range.  In practice, the minimum observed value will usually
00341     // be quite a lot higher (2000 to 20000 in my testing), which the
00342     // Vishay application note attributes to stray reflections from the
00343     // chip's mounting apparatus, ambient light, and noise within the
00344     // detector itself.  But just in case, set a floor that will ensure
00345     // reasonable calculations.
00346     if (minProxCount < 100)
00347         minProxCount = 100;
00348         
00349     // Set a ceiling of 65535, since the sensor can't go higher
00350     if (maxProxCount > 65535)
00351         maxProxCount = 65535;
00352         
00353     // Figure the scaling factor and offset over the range from the park
00354     // position to the maximum retracted position, which corresponds to
00355     // the minimum count (lowest intensity reflection) we've observed.
00356     //
00357     // Do all calculations with the counts *after* subtracting out the
00358     // signal's DC offset, which is the brightness level registered on the
00359     // sensor when there's no reflective target in range.  We can't directly
00360     // measure the DC offset in a plunger setup, since that would require
00361     // removing the plunger entirely, but we can guess that the minimum
00362     // reading observed during calibration is approximately equal to the
00363     // DC offset.  The minimum brightness occurs when the plunger is at the
00364     // most distance point in its travel range from the sensor, which is
00365     // when it's pulled all the way back.  The plunger travel distance is
00366     // just about at the limit of the VCNL4010's sensitivity, so the inverse
00367     // curve should be very nearly flat at this point, thus this is a very
00368     // close approximation of the true DC offset.
00369     const int dcOffsetDelta = 50;
00370     dcOffset = minProxCount > dcOffsetDelta ? minProxCount - dcOffsetDelta : 0;
00371     int park = parkProxCount - dcOffset;
00372     float parkInv = 1.0f/power(park);
00373     scalingFactor = 54612.5f / (1.0f/power(minProxCount - dcOffset) - parkInv);
00374     scalingOffset = 10922.5f - (scalingFactor * parkInv);
00375 }
00376 
00377 // Read an I2C register on the device
00378 uint8_t VCNL4010::readReg(uint8_t registerAddr)
00379 {
00380     // write the request
00381     uint8_t data_write[1] = { registerAddr };
00382     if (i2c.write(I2C_ADDR, data_write, 1, false))
00383         return 0x00;
00384 
00385     // read the result
00386     uint8_t data_read[1];
00387     if (i2c.read(I2C_ADDR, data_read, 1))
00388         return 0x00;
00389     
00390     // return the result
00391     return data_read[0];
00392 }
00393 
00394 // Write to an I2C register on the device 
00395 void VCNL4010::writeReg(uint8_t registerAddr, uint8_t data)
00396 {
00397     // set up the write: register number, data byte
00398     uint8_t data_write[2] = { registerAddr, data };
00399     i2c.write(I2C_ADDR, data_write, 2);
00400 }