lidar_lite.cpp

00001 #include "lidar_lite.h"
00002 
00003 LIDARLite::LIDARLite(I2C &i2c):i2c_(i2c){}
00004 
00005 /*------------------------------------------------------------------------------
00006   Begin
00007   Starts the sensor and I2C.
00008   Parameters
00009   ------------------------------------------------------------------------------
00010   configuration: Default 0. Selects one of several preset configurations.
00011   lidarliteAddress: Default 0x62. Fill in new address here if changed. See
00012     operating manual for instructions.
00013 ------------------------------------------------------------------------------*/
00014 void LIDARLite::begin(int configuration, char  lidarliteAddress)
00015 {
00016   configure(configuration, lidarliteAddress); // Configuration settings
00017 } /* LIDARLite::begin */
00018 
00019 /*------------------------------------------------------------------------------
00020   Configure
00021   Selects one of several preset configurations.
00022   Parameters
00023   ------------------------------------------------------------------------------
00024   configuration:  Default 0.
00025     0: Default mode, balanced performance.
00026     1: Short range, high speed. Uses 0x1d maximum acquisition count.
00027     2: Default range, higher speed short range. Turns on quick termination
00028         detection for faster measurements at short range (with decreased
00029         accuracy)
00030     3: Maximum range. Uses 0xff maximum acquisition count.
00031     4: High sensitivity detection. Overrides default valid measurement detection
00032         algorithm, and uses a threshold value for high sensitivity and noise.
00033     5: Low sensitivity detection. Overrides default valid measurement detection
00034         algorithm, and uses a threshold value for low sensitivity and noise.
00035   lidarliteAddress: Default 0x62. Fill in new address here if changed. See
00036     operating manual for instructions.
00037 ------------------------------------------------------------------------------*/
00038 void LIDARLite::configure(int configuration,  char  lidarliteAddress)
00039 {
00040   switch (configuration)
00041   {
00042     case 0: // Default mode, balanced performance
00043       write(0x02,0x80,lidarliteAddress); // Default
00044       write(0x04,0x08,lidarliteAddress); // Default
00045       write(0x1c,0x00,lidarliteAddress); // Default
00046     break;
00047 
00048     case 1: // Short range, high speed
00049       write(0x02,0x1d,lidarliteAddress);
00050       write(0x04,0x08,lidarliteAddress); // Default
00051       write(0x1c,0x00,lidarliteAddress); // Default
00052     break;
00053 
00054     case 2: // Default range, higher speed short range
00055       write(0x02,0x80,lidarliteAddress); // Default
00056       write(0x04,0x00,lidarliteAddress);
00057       write(0x1c,0x00,lidarliteAddress); // Default
00058     break;
00059 
00060     case 3: // Maximum range
00061       write(0x02,0xff,lidarliteAddress);
00062       write(0x04,0x08,lidarliteAddress); // Default
00063       write(0x1c,0x00,lidarliteAddress); // Default
00064     break;
00065 
00066     case 4: // High sensitivity detection, high erroneous measurements
00067       write(0x02,0x80,lidarliteAddress); // Default
00068       write(0x04,0x08,lidarliteAddress); // Default
00069       write(0x1c,0x80,lidarliteAddress);
00070     break;
00071 
00072     case 5: // Low sensitivity detection, low erroneous measurements
00073       write(0x02,0x80,lidarliteAddress); // Default
00074       write(0x04,0x08,lidarliteAddress); // Default
00075       write(0x1c,0xb0,lidarliteAddress);
00076     break;
00077   }
00078 } /* LIDARLite::configure */
00079 
00080 /*------------------------------------------------------------------------------
00081   Set I2C Address
00082   Set Alternate I2C Device Address. See Operation Manual for additional info.
00083   Parameters
00084   ------------------------------------------------------------------------------
00085   newAddress: desired secondary I2C device address
00086   disableDefault: a non-zero value here means the default 0x62 I2C device
00087     address will be disabled.
00088   lidarliteAddress: Default 0x62. Fill in new address here if changed. See
00089     operating manual for instructions.
00090 ------------------------------------------------------------------------------*/
00091 void LIDARLite::setI2Caddr(char newAddress, bool disableDefault,  char  lidarliteAddress)
00092 {
00093   char dataBytes[2];
00094 
00095   // Read UNIT_ID serial number bytes and write them into I2C_ID byte locations
00096   read ((0x16 | 0x80), 2, dataBytes, false, lidarliteAddress);
00097   write(0x18, dataBytes[0], lidarliteAddress);
00098   write(0x19, dataBytes[1], lidarliteAddress);
00099 
00100   // Write the new I2C device address to registers
00101   dataBytes[0] = newAddress;
00102   write(0x1a, dataBytes[0], lidarliteAddress);
00103 
00104   // Enable the new I2C device address using the default I2C device address
00105   dataBytes[0] = 0;
00106   write(0x1e, dataBytes[0], lidarliteAddress);
00107 
00108   // If desired, disable default I2C device address (using the new I2C device address)
00109   if (disableDefault)
00110   {
00111     dataBytes[0] = (1 << 3); // set bit to disable default address
00112     write(0x1e, dataBytes[0], newAddress);
00113   }
00114 } /* LIDARLite::setI2Caddr */
00115 
00116 /*------------------------------------------------------------------------------
00117   Reset
00118   Reset device. The device reloads default register settings, including the
00119   default I2C address. Re-initialization takes approximately 22ms.
00120   Parameters
00121   ------------------------------------------------------------------------------
00122   lidarliteAddress: Default 0x62. Fill in new address here if changed. See
00123     operating manual for instructions.
00124 ------------------------------------------------------------------------------*/
00125 void LIDARLite::reset( char  lidarliteAddress)
00126 {
00127   write(0x00,0x00,lidarliteAddress);
00128 } /* LIDARLite::reset */
00129 
00130 /*------------------------------------------------------------------------------
00131   Distance
00132   Take a distance measurement and read the result.
00133   Process
00134   ------------------------------------------------------------------------------
00135   1.  Write 0x04 or 0x03 to register 0x00 to initiate an aquisition.
00136   2.  Read register 0x01 (this is handled in the read() command)
00137       - if the first bit is "1" then the sensor is busy, loop until the first
00138         bit is "0"
00139       - if the first bit is "0" then the sensor is ready
00140   3.  Read two bytes from register 0x8f and save
00141   4.  Shift the first value from 0x8f << 8 and add to second value from 0x8f.
00142       The result is the measured distance in centimeters.
00143   Parameters
00144   ------------------------------------------------------------------------------
00145   biasCorrection: Default true. Take aquisition with receiver bias
00146     correction. If set to false measurements will be faster. Receiver bias
00147     correction must be performed periodically. (e.g. 1 out of every 100
00148     readings).
00149   lidarliteAddress: Default 0x62. Fill in new address here if changed. See
00150     operating manual for instructions.
00151 ------------------------------------------------------------------------------*/
00152 int LIDARLite::distance(bool biasCorrection,  char  lidarliteAddress)
00153 {
00154   if(biasCorrection)
00155   {
00156     // Take acquisition & correlation processing with receiver bias correction
00157     write(0x00,0x04,lidarliteAddress);
00158   }
00159   else
00160   {
00161     // Take acquisition & correlation processing without receiver bias correction
00162     write(0x00,0x03,lidarliteAddress);
00163   }
00164   // Array to store high and low bytes of distance
00165   char distanceArray[2];
00166   // Read two bytes from register 0x8f (autoincrement for reading 0x0f and 0x10)
00167   read(0x8f,2,distanceArray,true,lidarliteAddress);//0x8f
00168   // Shift high byte and add to low byte
00169   int distance = (distanceArray[0] << 8) + distanceArray[1];
00170   return(distance);
00171 } /* LIDARLite::distance */
00172 
00173 /*------------------------------------------------------------------------------
00174   Write
00175   Perform I2C write to device.
00176   Parameters
00177   ------------------------------------------------------------------------------
00178   myAddress: register address to write to.
00179   myValue: value to write.
00180   lidarliteAddress: Default 0x62. Fill in new address here if changed. See
00181     operating manual for instructions.
00182 ------------------------------------------------------------------------------*/
00183 void LIDARLite::write(char myAddress, char myValue,  char  lidarliteAddress)
00184 {
00185   char data[2] = {myAddress,myValue};//register value
00186   int nackCatcher = i2c_.write((int)lidarliteAddress,data,2);
00187   // A nack means the device is not responding, report the error over serial
00188   /*
00189   if(nackCatcher != 0)
00190   {
00191     //printf("> nack");
00192   }*/
00193 
00194   //wait_ms(1); // 1 ms delay for robustness with successive reads and writes
00195 } /* LIDARLite::write */
00196 
00197 /*------------------------------------------------------------------------------
00198   Read
00199   Perform I2C read from device. Will detect an unresponsive device and report
00200   the error over serial. The optional busy flag monitoring
00201   can be used to read registers that are updated at the end of a distance
00202   measurement to obtain the new data.
00203   Parameters
00204   ------------------------------------------------------------------------------
00205   myAddress: register address to read from.
00206   numOfBytes: numbers of bytes to read. Can be 1 or 2.
00207   arrayToSave: an array to store the read values.
00208   monitorBusyFlag: if true, the routine will repeatedly read the status
00209     register until the busy flag (LSB) is 0.
00210 ------------------------------------------------------------------------------*/
00211 void LIDARLite::read(char myAddress, int numOfBytes, char arrayToSave[2], bool monitorBusyFlag, char  lidarliteAddress)
00212 {
00213   char data;
00214   int busyFlag = 0; // busyFlag monitors when the device is done with a measurement
00215   bool errer_flag =false;
00216   if(monitorBusyFlag)
00217   {
00218     busyFlag = 1; // Begin read immediately if not monitoring busy flag
00219   }
00220   int busyCounter = 0; // busyCounter counts number of times busy flag is checked, for timeout
00221 
00222   while(busyFlag != 0) // Loop until device is not busy
00223   {
00224     // Read status register to check busy flag
00225     data = 0x01;//Set the status register to be read
00226     int nackCatcher = i2c_.write(lidarliteAddress,&data,1,true);
00227     // A nack means the device is not responding, report the error over serial
00228     if(nackCatcher != 0)
00229     {
00230       //printf("> nack");
00231     }
00232 
00233     i2c_.read(lidarliteAddress,&data,1); // Read register 0x01
00234     busyFlag = data & 0x1; // Assign the LSB of the status register to busyFlag
00235 
00236     busyCounter++; // Increment busyCounter for timeout
00237 
00238     // Handle timeout condition, exit while loop and goto bailout
00239     if(busyCounter > 9999)
00240     {
00241       errer_flag = true;
00242       break;
00243     }
00244   }
00245 
00246   // Device is not busy, begin read
00247   if(busyFlag == 0 && !errer_flag)
00248   {
00249     data = myAddress;// Set the register to be read
00250     int nackCatcher = i2c_.write(lidarliteAddress,&data,1,true);
00251     // A nack means the device is not responding, report the error over serial
00252     if(nackCatcher != 0)
00253     {
00254       //printf("> nack");
00255     }
00256 
00257     // Perform read of 1 or 2 bytes, save in arrayToSave
00258     i2c_.read(lidarliteAddress, arrayToSave, numOfBytes);
00259   }
00260 
00261   // bailout reports error over serial
00262   if(busyCounter > 9999 || errer_flag)
00263   {
00264     busyCounter = 0;
00265     //printf("> read failed");
00266   }
00267 } /* LIDARLite::read */
00268 
00269 /*------------------------------------------------------------------------------
00270   Correlation Record To Serial
00271   The correlation record used to calculate distance can be read from the device.
00272   It has a bipolar wave shape, transitioning from a positive going portion to a
00273   roughly symmetrical negative going pulse. The point where the signal crosses
00274   zero represents the effective delay for the reference and return signals.
00275   Process
00276   ------------------------------------------------------------------------------
00277   1.  Take a distance reading (there is no correlation record without at least
00278       one distance reading being taken)
00279   2.  Select memory bank by writing 0xc0 to register 0x5d
00280   3.  Set test mode select by writing 0x07 to register 0x40
00281   4.  For as many readings as you want to take (max is 1024)
00282       1.  Read two bytes from 0xd2
00283       2.  The Low byte is the value from the record
00284       3.  The high byte is the sign from the record
00285   Parameters
00286   ------------------------------------------------------------------------------
00287   separator: the separator between serial data words
00288   numberOfReadings: Default: 256. Maximum of 1024
00289   lidarliteAddress: Default 0x62. Fill in new address here if changed. See
00290     operating manual for instructions.
00291 ------------------------------------------------------------------------------*/
00292 void LIDARLite::correlationRecordToSerial(char separator, int numberOfReadings,  char  lidarliteAddress)
00293 {
00294 
00295   // Array to store read values
00296   char correlationArray[2];
00297   // Var to store value of correlation record
00298   int correlationValue = 0;
00299   //  Selects memory bank
00300   write(0x5d,0xc0,lidarliteAddress);
00301   // Test mode enable
00302   write(0x40, 0x07,lidarliteAddress);
00303   for(int i = 0; i<numberOfReadings; i++){
00304     // Select single byte
00305     read(0xd2,2,correlationArray,false,lidarliteAddress);
00306     //  Low byte is the value of the correlation record
00307     correlationValue = correlationArray[0];
00308     // if upper byte lsb is set, the value is negative
00309     if((int)correlationArray[1] == 1){
00310       correlationValue |= 0xff00;
00311     }
00312     //printf((int)correlationValue);
00313     //printf(separator);
00314   }
00315   // test mode disable
00316   write(0x40,0x00,lidarliteAddress);
00317 } /* LIDARLite::correlationRecordToSerial */