Driver version

ReadMe.md

@@ -16,17 +16,17 @@ For detailed specifications, pinout, and connection diagrams, please refer to th
 - [iriutils](https://devel.iri.upc.edu/pub/labrobotica/drivers/iriutils/trunk) - IRI C++ library
 
   - Download the library: `svn co https://devel.iri.upc.edu/pub/labrobotica/drivers/iriutils/trunk comm`
-  - Compile and install: `cd iriutils/build && cmake .. && make && sudo make install`
+  - Compile and install: `cd iriutils/build && cmake -D CMAKE_BUILD_TYPE=RELEASE .. && make && sudo make install`
 
 - [comm](https://devel.iri.upc.edu/pub/labrobotica/drivers/comm/trunk) - IRI C++ library
 
   - Download the library: `svn co https://devel.iri.upc.edu/pub/labrobotica/drivers/comm/trunk comm`
-  - Compile and install: `cd comm/build && cmake .. && make && sudo make install`
+  - Compile and install: `cd comm/build && cmake -D CMAKE_BUILD_TYPE=RELEASE .. && make && sudo make install`
 
 - [usb_i2c_adapter](https://devel.iri.upc.edu/pub/labrobotica/drivers/usb_i2c_adapter/trunk) - IRI C++ library
 
   - Download the library: `svn co https://devel.iri.upc.edu/pub/labrobotica/drivers/usb_i2c_adapter/trunk usb_i2c_adapter`
-  - Compile and install: `cd usb_i2c_adapter/build && cmake .. && make && sudo make install`
+  - Compile and install: `cd usb_i2c_adapter/build && cmake -D CMAKE_BUILD_TYPE=RELEASE .. && make && sudo make install`
 
 ### Example of usage
 

src/lidar_lite.cpp

 #include "lidar_lite.h"
 #include <exceptions/lidar_lite_exceptions.h>
 
+#include <ctime.h>
+
 //******************************************************************************
 // PUBLIC API 
 //******************************************************************************
@@ -26,7 +28,10 @@ CLidarLite::CLidarLite(const std::string &serial, const int config)
 CLidarLite::~CLidarLite(void)
 {
 	if (this->status_ == RUNNING)
+  {
+    reset();
     close();
+  }
 }
 
 void CLidarLite::open(void)
@@ -105,8 +110,6 @@ int CLidarLite::get_range(bool biasCorrection)
   if (this->status_ != RUNNING)
     throw CLidarLiteException(_HERE_, "RANGE MEASUREMENT cannot be obtained because the device is not running.");
 
-  int range = NAN;
-
   // Initiate an aquisition.
   if(biasCorrection || this->max_unbiases_meas_ > 99)
   {
@@ -121,13 +124,8 @@ int CLidarLite::get_range(bool biasCorrection)
     ++this->max_unbiases_meas_;
   }
 
-  // Read STATUS. Repeat until bit 0 (LSB) goes low.
-  unsigned char busy;
-  do 
-    busy = read(STATUS,1);
-  while( busy & 0x01 );
-  
   // Read two bytes from register 0x8f and save
+  int range = NAN;
   range = read(FULL_DELAY_HIGH,2);
 
   return range;
@@ -191,6 +189,10 @@ int CLidarLite::read(unsigned char addr, int len)
   if (len == 2)
     addr = addr_to_read_2bytes(addr); // See header file for a description of this functionality.
   
+  // Read STATUS. Repeat until bit 0 (LSB) goes low.
+  // wait_busy();
+  // NOTE: Commented because using this USB adapter the following write actions are slower than usual busy time.
+
   unsigned char data[len];
   try { 
     this->adapter_->write(DEVICE_ID,&addr,1);
@@ -209,10 +211,41 @@ int CLidarLite::read(unsigned char addr, int len)
     unsigned short int read_val = data[0]*256+data[1];
     val = (int)read_val;
   }
-
   return val;
 }
 
+void CLidarLite::wait_busy(void)
+{
+  try { 
+    unsigned char addr = STATUS;
+    this->adapter_->write(DEVICE_ID,&addr,1);
+  }
+  catch(CException &e)
+  {
+    throw CLidarLiteException(_HERE_, e.what());
+  }
+
+  bool busy = true;
+  int busyCounter = 0;
+
+  try { 
+    while (busy) 
+    {
+      unsigned char data;
+      this->adapter_->read(DEVICE_ID,&data,1);
+      busy = data & 0x01;
+      busyCounter++; // Increment busyCounter for timeout
+      // Handle timeout condition, exit while loop and goto bailout
+      if(busyCounter > 99)
+        break;
+    }
+  }
+  catch(CException &e)
+  {
+    throw CLidarLiteException(_HERE_, e.what());
+  }
+}
+
 unsigned short int CLidarLite::addr_to_read_2bytes(unsigned char first_byte_addr)
 {
   unsigned short int val = (0x01<<7) | first_byte_addr;

src/lidar_lite.h

@@ -119,6 +119,8 @@ class CLidarLite
 		 */
 	  void config_dev(void);
 
+		void wait_busy(void);
+
   public:
 
 	  /**