Diff: lidar_lite.cpp

Revision:

6:678c6b604ac7

diff -r a7894e6982ea -r 678c6b604ac7 lidar_lite.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/lidar_lite.cpp	Thu Nov 12 16:44:10 2020 +0000
@@ -0,0 +1,317 @@
+#include "lidar_lite.h"
+
+LIDARLite::LIDARLite(I2C &i2c):i2c_(i2c){}
+
+/*------------------------------------------------------------------------------
+  Begin
+  Starts the sensor and I2C.
+  Parameters
+  ------------------------------------------------------------------------------
+  configuration: Default 0. Selects one of several preset configurations.
+  lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+    operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite::begin(int configuration, char  lidarliteAddress)
+{
+  configure(configuration, lidarliteAddress); // Configuration settings
+} /* LIDARLite::begin */
+
+/*------------------------------------------------------------------------------
+  Configure
+  Selects one of several preset configurations.
+  Parameters
+  ------------------------------------------------------------------------------
+  configuration:  Default 0.
+    0: Default mode, balanced performance.
+    1: Short range, high speed. Uses 0x1d maximum acquisition count.
+    2: Default range, higher speed short range. Turns on quick termination
+        detection for faster measurements at short range (with decreased
+        accuracy)
+    3: Maximum range. Uses 0xff maximum acquisition count.
+    4: High sensitivity detection. Overrides default valid measurement detection
+        algorithm, and uses a threshold value for high sensitivity and noise.
+    5: Low sensitivity detection. Overrides default valid measurement detection
+        algorithm, and uses a threshold value for low sensitivity and noise.
+  lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+    operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite::configure(int configuration,  char  lidarliteAddress)
+{
+  switch (configuration)
+  {
+    case 0: // Default mode, balanced performance
+      write(0x02,0x80,lidarliteAddress); // Default
+      write(0x04,0x08,lidarliteAddress); // Default
+      write(0x1c,0x00,lidarliteAddress); // Default
+    break;
+
+    case 1: // Short range, high speed
+      write(0x02,0x1d,lidarliteAddress);
+      write(0x04,0x08,lidarliteAddress); // Default
+      write(0x1c,0x00,lidarliteAddress); // Default
+    break;
+
+    case 2: // Default range, higher speed short range
+      write(0x02,0x80,lidarliteAddress); // Default
+      write(0x04,0x00,lidarliteAddress);
+      write(0x1c,0x00,lidarliteAddress); // Default
+    break;
+
+    case 3: // Maximum range
+      write(0x02,0xff,lidarliteAddress);
+      write(0x04,0x08,lidarliteAddress); // Default
+      write(0x1c,0x00,lidarliteAddress); // Default
+    break;
+
+    case 4: // High sensitivity detection, high erroneous measurements
+      write(0x02,0x80,lidarliteAddress); // Default
+      write(0x04,0x08,lidarliteAddress); // Default
+      write(0x1c,0x80,lidarliteAddress);
+    break;
+
+    case 5: // Low sensitivity detection, low erroneous measurements
+      write(0x02,0x80,lidarliteAddress); // Default
+      write(0x04,0x08,lidarliteAddress); // Default
+      write(0x1c,0xb0,lidarliteAddress);
+    break;
+  }
+} /* LIDARLite::configure */
+
+/*------------------------------------------------------------------------------
+  Set I2C Address
+  Set Alternate I2C Device Address. See Operation Manual for additional info.
+  Parameters
+  ------------------------------------------------------------------------------
+  newAddress: desired secondary I2C device address
+  disableDefault: a non-zero value here means the default 0x62 I2C device
+    address will be disabled.
+  lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+    operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite::setI2Caddr(char newAddress, bool disableDefault,  char  lidarliteAddress)
+{
+  char dataBytes[2];
+
+  // Read UNIT_ID serial number bytes and write them into I2C_ID byte locations
+  read ((0x16 | 0x80), 2, dataBytes, false, lidarliteAddress);
+  write(0x18, dataBytes[0], lidarliteAddress);
+  write(0x19, dataBytes[1], lidarliteAddress);
+
+  // Write the new I2C device address to registers
+  dataBytes[0] = newAddress;
+  write(0x1a, dataBytes[0], lidarliteAddress);
+
+  // Enable the new I2C device address using the default I2C device address
+  dataBytes[0] = 0;
+  write(0x1e, dataBytes[0], lidarliteAddress);
+
+  // If desired, disable default I2C device address (using the new I2C device address)
+  if (disableDefault)
+  {
+    dataBytes[0] = (1 << 3); // set bit to disable default address
+    write(0x1e, dataBytes[0], newAddress);
+  }
+} /* LIDARLite::setI2Caddr */
+
+/*------------------------------------------------------------------------------
+  Reset
+  Reset device. The device reloads default register settings, including the
+  default I2C address. Re-initialization takes approximately 22ms.
+  Parameters
+  ------------------------------------------------------------------------------
+  lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+    operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite::reset( char  lidarliteAddress)
+{
+  write(0x00,0x00,lidarliteAddress);
+} /* LIDARLite::reset */
+
+/*------------------------------------------------------------------------------
+  Distance
+  Take a distance measurement and read the result.
+  Process
+  ------------------------------------------------------------------------------
+  1.  Write 0x04 or 0x03 to register 0x00 to initiate an aquisition.
+  2.  Read register 0x01 (this is handled in the read() command)
+      - if the first bit is "1" then the sensor is busy, loop until the first
+        bit is "0"
+      - if the first bit is "0" then the sensor is ready
+  3.  Read two bytes from register 0x8f and save
+  4.  Shift the first value from 0x8f << 8 and add to second value from 0x8f.
+      The result is the measured distance in centimeters.
+  Parameters
+  ------------------------------------------------------------------------------
+  biasCorrection: Default true. Take aquisition with receiver bias
+    correction. If set to false measurements will be faster. Receiver bias
+    correction must be performed periodically. (e.g. 1 out of every 100
+    readings).
+  lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+    operating manual for instructions.
+------------------------------------------------------------------------------*/
+int LIDARLite::distance(bool biasCorrection,  char  lidarliteAddress)
+{
+  if(biasCorrection)
+  {
+    // Take acquisition & correlation processing with receiver bias correction
+    write(0x00,0x04,lidarliteAddress);
+  }
+  else
+  {
+    // Take acquisition & correlation processing without receiver bias correction
+    write(0x00,0x03,lidarliteAddress);
+  }
+  // Array to store high and low bytes of distance
+  char distanceArray[2];
+  // Read two bytes from register 0x8f (autoincrement for reading 0x0f and 0x10)
+  read(0x8f,2,distanceArray,true,lidarliteAddress);//0x8f
+  // Shift high byte and add to low byte
+  int distance = (distanceArray[0] << 8) + distanceArray[1];
+  return(distance);
+} /* LIDARLite::distance */
+
+/*------------------------------------------------------------------------------
+  Write
+  Perform I2C write to device.
+  Parameters
+  ------------------------------------------------------------------------------
+  myAddress: register address to write to.
+  myValue: value to write.
+  lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+    operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite::write(char myAddress, char myValue,  char  lidarliteAddress)
+{
+  char data[2] = {myAddress,myValue};//register value
+  int nackCatcher = i2c_.write((int)lidarliteAddress,data,2);
+  // A nack means the device is not responding, report the error over serial
+  /*
+  if(nackCatcher != 0)
+  {
+    //printf("> nack");
+  }*/
+
+  //wait_ms(1); // 1 ms delay for robustness with successive reads and writes
+} /* LIDARLite::write */
+
+/*------------------------------------------------------------------------------
+  Read
+  Perform I2C read from device. Will detect an unresponsive device and report
+  the error over serial. The optional busy flag monitoring
+  can be used to read registers that are updated at the end of a distance
+  measurement to obtain the new data.
+  Parameters
+  ------------------------------------------------------------------------------
+  myAddress: register address to read from.
+  numOfBytes: numbers of bytes to read. Can be 1 or 2.
+  arrayToSave: an array to store the read values.
+  monitorBusyFlag: if true, the routine will repeatedly read the status
+    register until the busy flag (LSB) is 0.
+------------------------------------------------------------------------------*/
+void LIDARLite::read(char myAddress, int numOfBytes, char arrayToSave[2], bool monitorBusyFlag, char  lidarliteAddress)
+{
+  char data;
+  int busyFlag = 0; // busyFlag monitors when the device is done with a measurement
+  bool errer_flag =false;
+  if(monitorBusyFlag)
+  {
+    busyFlag = 1; // Begin read immediately if not monitoring busy flag
+  }
+  int busyCounter = 0; // busyCounter counts number of times busy flag is checked, for timeout
+
+  while(busyFlag != 0) // Loop until device is not busy
+  {
+    // Read status register to check busy flag
+    data = 0x01;//Set the status register to be read
+    int nackCatcher = i2c_.write(lidarliteAddress,&data,1,true);
+    // A nack means the device is not responding, report the error over serial
+    if(nackCatcher != 0)
+    {
+      //printf("> nack");
+    }
+
+    i2c_.read(lidarliteAddress,&data,1); // Read register 0x01
+    busyFlag = data & 0x1; // Assign the LSB of the status register to busyFlag
+
+    busyCounter++; // Increment busyCounter for timeout
+
+    // Handle timeout condition, exit while loop and goto bailout
+    if(busyCounter > 9999)
+    {
+      errer_flag = true;
+      break;
+    }
+  }
+
+  // Device is not busy, begin read
+  if(busyFlag == 0 && !errer_flag)
+  {
+    data = myAddress;// Set the register to be read
+    int nackCatcher = i2c_.write(lidarliteAddress,&data,1,true);
+    // A nack means the device is not responding, report the error over serial
+    if(nackCatcher != 0)
+    {
+      //printf("> nack");
+    }
+
+    // Perform read of 1 or 2 bytes, save in arrayToSave
+    i2c_.read(lidarliteAddress, arrayToSave, numOfBytes);
+  }
+
+  // bailout reports error over serial
+  if(busyCounter > 9999 || errer_flag)
+  {
+    busyCounter = 0;
+    //printf("> read failed");
+  }
+} /* LIDARLite::read */
+
+/*------------------------------------------------------------------------------
+  Correlation Record To Serial
+  The correlation record used to calculate distance can be read from the device.
+  It has a bipolar wave shape, transitioning from a positive going portion to a
+  roughly symmetrical negative going pulse. The point where the signal crosses
+  zero represents the effective delay for the reference and return signals.
+  Process
+  ------------------------------------------------------------------------------
+  1.  Take a distance reading (there is no correlation record without at least
+      one distance reading being taken)
+  2.  Select memory bank by writing 0xc0 to register 0x5d
+  3.  Set test mode select by writing 0x07 to register 0x40
+  4.  For as many readings as you want to take (max is 1024)
+      1.  Read two bytes from 0xd2
+      2.  The Low byte is the value from the record
+      3.  The high byte is the sign from the record
+  Parameters
+  ------------------------------------------------------------------------------
+  separator: the separator between serial data words
+  numberOfReadings: Default: 256. Maximum of 1024
+  lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+    operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite::correlationRecordToSerial(char separator, int numberOfReadings,  char  lidarliteAddress)
+{
+
+  // Array to store read values
+  char correlationArray[2];
+  // Var to store value of correlation record
+  int correlationValue = 0;
+  //  Selects memory bank
+  write(0x5d,0xc0,lidarliteAddress);
+  // Test mode enable
+  write(0x40, 0x07,lidarliteAddress);
+  for(int i = 0; i<numberOfReadings; i++){
+    // Select single byte
+    read(0xd2,2,correlationArray,false,lidarliteAddress);
+    //  Low byte is the value of the correlation record
+    correlationValue = correlationArray[0];
+    // if upper byte lsb is set, the value is negative
+    if((int)correlationArray[1] == 1){
+      correlationValue |= 0xff00;
+    }
+    //printf((int)correlationValue);
+    //printf(separator);
+  }
+  // test mode disable
+  write(0x40,0x00,lidarliteAddress);
+} /* LIDARLite::correlationRecordToSerial */
\ No newline at end of file