basic functionalities done. remaining one function: get_correlation_record

src/examples/lidar_lite_test.cpp

@@ -2,16 +2,32 @@
 
 int main(int argc, char *argv[])
 {
+	std::cout << "***************************************************" << std::endl;
+  std::cout << "*           LIDAR LITE TEST APPLICATION           *" << std::endl;
+	std::cout << "***************************************************" << std::endl;
 
-	std::string serial="A700evSl";
-	
-	CLidarLite* laser_ptr = new CLidarLite(serial);
+	std::string serial = "A700evSl";
+
+	CLidarLite* laser_ptr = new CLidarLite(serial,0);
 	
 	try
 	{
 		laser_ptr->open();
-		laser_ptr->reset();
-		laser_ptr->set_max_acq_count(0x80);
+		
+		// Test 1: Get 10 measurements with bias correction
+	  std::cout << std::endl << "***** Example 1: Measurements WITH bias correction: *******" << std::endl;
+		for (int ii = 0; ii < 10; ++ii)
+		 	std::cout << "  - Range: " << laser_ptr->get_range(true) << " [cm]" << std::endl;
+		
+		// New configuration mode
+	  std::cout << std::endl << "***** Example 2: Set new configuration mode. **************" << std::endl;
+		laser_ptr->set_new_config(5);
+
+		// Test 2: Get 10 measurements without bias correction
+	  std::cout << std::endl << "***** Example 3: Measurements WITHOUT bias correction *****" << std::endl;
+		for (int ii = 0; ii < 10; ++ii)
+		 	std::cout << "  - Range: " << laser_ptr->get_range(false) << " [cm]" << std::endl;
+
 		laser_ptr->close();
 
 	}catch(CLidarLiteException &e)

src/exceptions/lidar_lite_exceptions.cpp

@@ -5,7 +5,7 @@
 
 #include "exceptions/lidar_lite_exceptions.h"
 
-const std::string lidar_lite_error_message="LidarLite error - ";
+const std::string lidar_lite_error_message="LidarLite - ";
 
 CLidarLiteException::CLidarLiteException(const std::string& where,const std::string &error_msg):CException(where,lidar_lite_error_message)
 {

src/lidar_lite.cpp

 #include "lidar_lite.h"
 
-CLidarLite::CLidarLite(const std::string &serial)
+//******************************************************************************
+// PUBLIC API 
+//******************************************************************************
+
+CLidarLite::CLidarLite(const std::string &serial, const int config)
 {
   this->serial_ = serial;
+  this->config_ = config;
   this->status_ = IDLE;
+  this->max_unbiases_meas_ = 0;
 
   // On power-up or reset, the device performs a self-test sequence and initializes
   // all registers with default values. After roughly 22 ms distance measurements
@@ -20,7 +26,7 @@ CLidarLite::~CLidarLite(void)
 void CLidarLite::open(void)
 {
   if (this->status_ != IDLE)
-    throw CLidarLiteException(_HERE_, "Device cannot be OPENED because it is already running.");
+    throw CLidarLiteException(_HERE_, "[CLidarLite]: Device cannot be OPENED because it is already running.");
 
   try
   {
@@ -29,37 +35,24 @@ void CLidarLite::open(void)
 		// open serial port
     std::cout << "[CLidarLite]: Opening Lidar Lite. " << std::endl; 
     this->adapter_->open(this->serial_);
-    std::cout << "  - Serial number: " << this->serial_ << std::endl;
-    std::cout << "  - Firmware revision of USB_I2C adapter: " << (int)this->adapter_->get_revision() << std::endl;
+    usleep(5000); // Delay to initialize sensor.
+
+    std::cout << "  - USB_I2C serial number: " << this->serial_ << std::endl;
+    std::cout << "  - USB_I2C firmware revision: " << (int)this->adapter_->get_revision() << std::endl;
 
 		// Set gpio 2 and 3 as i2c    
     this->adapter_->config_gpio(gpio2,i2c);
     this->adapter_->config_gpio(gpio3,i2c);
 
-
-    // TEST WORK IN PROGRESS **************************
-
-    write(ACQ_COMMAND,0x04);
-
-    //Read STATUS. Repeat until bit 0 (LSB) goes low.
-    unsigned char busy;
-    do 
-      busy = read(STATUS,1);
-    while( busy & 0x01 );
-    
-    // regular read example
-    // std::cout << "Register " << std::hex << ACQ_CONFIG_REG << " with value: " << read(ACQ_CONFIG_REG,1) << std::dec << std::endl;
-
-    // read two bytes
-    std::cout << "[CLidarLite]: Lidar-Lite measurement: " << read(FULL_DELAY_HIGH,2) << " [cm]" << std::endl;
-    
-    // ************************************************
-
   }catch(CException &e)
   {
-  	throw CLidarLiteException(_HERE_, e.what());
-  }
-  
+    throw CLidarLiteException(_HERE_, e.what());
+  } 
+
+  // Configure device
+  config_dev();
+
+  // Set status to running
   this->status_ = RUNNING;
 }
 
@@ -68,49 +61,75 @@ void CLidarLite::close(void)
   if (this->status_ != RUNNING)
     throw CLidarLiteException(_HERE_, "Device cannot be CLOSED because it is not running.");
 
-	try
-	{
-    if(this->adapter_ != NULL)
-    {
-      this->adapter_ = NULL;
-      delete this->adapter_;    
-    }
-  }catch(CException &e)
+  if(this->adapter_ != NULL)
   {
-  	throw CLidarLiteException(_HERE_, e.what());
+    this->adapter_ = NULL;
+    delete this->adapter_;    
   }
 }
 
-void CLidarLite::blink_led_usb_adapter(void)
+void CLidarLite::reset(void)
 {
   if (this->status_ != RUNNING)
-    throw CLidarLiteException(_HERE_, "Usb adapter LED will not BLINK because Lidar-Lite is not running.");
+    throw CLidarLiteException(_HERE_, "Device cannot be RESET because it is not running.");
 
-  try
+  // reset operation  
+  write(ACQ_COMMAND,0x00);
+  std::cout << "[CLidarLite]: Device RESET done." << std::endl;
+
+  // On power-up or reset, the device performs a self-test sequence and initializes
+  // all registers with default values. After roughly 22 ms distance measurements
+  // can be taken with the I2C interface or the Mode Control Pin.
+  usleep(22000);
+
+  // Configure device
+  config_dev();
+}
+
+void CLidarLite::set_new_config(const int config)
+{
+  this->config_ = config;
+  
+  // Configure device
+  config_dev();
+}
+
+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)
   {
-    for (int ii = 0; ii < 2; ++ii)
-    {
-      this->adapter_->turn_led_off();
-      usleep(200000);
-      this->adapter_->turn_led_on();
-      usleep(200000);
-    }
-  }catch(CException &e)
+    if (this->max_unbiases_meas_ > 99)
+      std::cout << "\033[1;33m" << "[CLidarLite]: - WARNING : " << "\033[1;37m\033[0m" << "Forcing bias correction. Maximum measurements reached without bias correction (100)." << std::endl; 
+    write(ACQ_COMMAND,0x04);
+    this->max_unbiases_meas_ = 0;
+  }
+  else
   {
-    throw CLidarLiteException(_HERE_, e.what());
+    write(ACQ_COMMAND,0x03);
+    ++this->max_unbiases_meas_;
   }
-}
 
-std::string CLidarLite::hex_to_str(const int &val)
-{
-  std::ostringstream ret;
-  ret << "0x" << std::setfill('0') << std::setw(2) << std::hex << val;
-  return ret.str();
+  // 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
+  range = read(FULL_DELAY_HIGH,2);
+
+  return range;
 }
 
-//******************
-// DEVICE SPECIFIC *
-//******************
+//******************************************************************************
+// PRIVATE: DEVICE SPECIFIC FUNCTIONS
+//******************************************************************************
 
 int CLidarLite::read(unsigned char addr, int len)
 {
@@ -159,38 +178,85 @@ void CLidarLite::write(unsigned char addr, unsigned char cmd)
   }
 }
 
-void CLidarLite::reset(void)
-{
-  if (this->status_ != RUNNING)
-    throw CLidarLiteException(_HERE_, "Device cannot be RESET because it is not running.");
-
+void CLidarLite::config_dev(void)
+{  
+  if (this->config_<0 || this->config_>5)
+    throw CLidarLiteException(_HERE_, "Device cannot be CONFIGURED. Wrong configuration mode. Please select a value between [0~5].");
+  
   try { 
-    write(ACQ_COMMAND,0x00);
+    switch (this->config_)
+    {
+      case 0: // Default mode, balanced performance
+        write(SIG_COUNT_VAL,0x80);    // Default
+        write(ACQ_CONFIG_REG,0x08);   // Default
+        write(THRESHOLD_BYPASS,0x00); // Default
+      break;
+  
+      case 1: // Short range, high speed
+        write(SIG_COUNT_VAL,0x1d);
+        write(ACQ_CONFIG_REG,0x08);   // Default
+        write(THRESHOLD_BYPASS,0x00); // Default
+      break;
+  
+      case 2: // Default range, higher speed short range
+        write(SIG_COUNT_VAL,0x80);    // Default
+        write(ACQ_CONFIG_REG,0x00);
+        write(THRESHOLD_BYPASS,0x00); // Default
+      break;
+  
+      case 3: // Maximum range
+        write(SIG_COUNT_VAL,0xff);
+        write(ACQ_CONFIG_REG,0x08);   // Default
+        write(THRESHOLD_BYPASS,0x00); // Default
+      break;
+  
+      case 4: // High sensitivity detection, high erroneous measurements
+        write(SIG_COUNT_VAL,0x80);    // Default
+        write(ACQ_CONFIG_REG,0x08);   // Default
+        write(THRESHOLD_BYPASS,0x80);
+      break;
+  
+      case 5: // Low sensitivity detection, low erroneous measurements
+        write(SIG_COUNT_VAL,0x80);    // Default
+        write(ACQ_CONFIG_REG,0x08);   // Default
+        write(THRESHOLD_BYPASS,0xb0);
+      break;
+    }
+    std::cout << "[CLidarLite]: Configuration mode set to: " << this->config_ << std::endl;
   }
   catch(CException &e)
   {
     throw CLidarLiteException(_HERE_, e.what());
   }
-
-  // On power-up or reset, the device performs a self-test sequence and initializes
-  // all registers with default values. After roughly 22 ms distance measurements
-  // can be taken with the I2C interface or the Mode Control Pin.
-  usleep(22000);
 }
 
-void CLidarLite::set_max_acq_count(const unsigned char &num_acq)
+//******************************************************************************
+// PRIVATE: MISC FUNCTIONS 
+//******************************************************************************
+
+void CLidarLite::blink_led_usb_adapter(void)
 {
   if (this->status_ != RUNNING)
-    throw CLidarLiteException(_HERE_, "MAXIMUM ACQUISITION COUNT cannot be set because the device is not running.");
+    throw CLidarLiteException(_HERE_, "USB_I2C adapter LED will not BLINK because Lidar-Lite is not running.");
 
-  try { 
-    write(SIG_COUNT_VAL,num_acq);
-    std::cout << "[CLidarLite]: Maximum acquisition count set to: " << hex_to_str(0x80) << std::endl;
-  }
-  catch(CException &e)
+  try
+  {
+    for (int ii = 0; ii < 2; ++ii)
+    {
+      this->adapter_->turn_led_off();
+      usleep(200000);
+      this->adapter_->turn_led_on();
+      usleep(200000);
+    }
+  }catch(CException &e)
   {
     throw CLidarLiteException(_HERE_, e.what());
   }
 }
 
-
+std::string CLidarLite::hex_to_str(const int &val)
+{
+  std::ostringstream ret;
+  ret << "0x" << std::setfill('0') << std::setw(2) << std::hex << val;
+  return ret.str();
+}

src/lidar_lite.h

@@ -5,42 +5,45 @@
 #include <sstream>  
 #include <string>    
 #include <iomanip>
+#include <cmath>
 #include <usb_i2c.h>
 #include "exceptions/lidar_lite_exceptions.h"
 
 // The device has a 7-bit slave address with a default value of 0x62.
+// Fill in new address here if changed. 
+// See operating manual for instructions.
 unsigned char DEVICE_ID = 0x62;
 
 enum V3_REGISTER_DEF
 {  
-	ACQ_COMMAND = 		0x00,	// Device command.
-	STATUS = 			0x01,	// System status.
-	SIG_COUNT_VAL = 	0x02,	// Maximum acquisition count.
-	ACQ_CONFIG_REG = 	0x04,	// Acquisition mode control.
-	VELOCITY = 			0x09,	// Velocity measurement output.
-	PEAK_CORR = 		0x0c,	// Peak value in correlation record.
-	NOISE_PEAK = 		0x0d,	// Correlation record noise floor.
+	ACQ_COMMAND = 			0x00,	// Device command.
+	STATUS = 						0x01,	// System status.
+	SIG_COUNT_VAL = 		0x02,	// Maximum acquisition count.
+	ACQ_CONFIG_REG = 		0x04,	// Acquisition mode control.
+	VELOCITY = 					0x09,	// Velocity measurement output.
+	PEAK_CORR = 				0x0c,	// Peak value in correlation record.
+	NOISE_PEAK = 				0x0d,	// Correlation record noise floor.
 	SIGNAL_STRENGTH = 	0x0e,	// Received signal strength.
 	FULL_DELAY_HIGH = 	0x0f,	// Distance measurement high byte.
-	FULL_DELAY_LOW = 	0x10,	// Distance measurement low byte.
+	FULL_DELAY_LOW = 		0x10,	// Distance measurement low byte.
 	OUTER_LOOP_COUNT = 	0x11,	// Burst measurement count control.
-	REF_COUNT_VAL = 	0x12,	// Reference acquisition count.
+	REF_COUNT_VAL = 		0x12,	// Reference acquisition count.
 	LAST_DELAY_HIGH = 	0x14,	// Previous distance measurement high byte.
-	LAST_DELAY_LOW = 	0x15,	// Previous distance measurement low byte.
-	UNIT_ID_HIGH = 		0x16,	// Serial number high byte.
-	UNIT_ID_LOW = 		0x17,	// Serial number low byte.
-	I2C_ID_HIGH = 		0x18,	// Write serial number high byte for I2C address unlock.
-	I2C_ID_LOW = 		0x19,	// Write serial number low byte for I2C address unlock.
-	I2C_SEC_ADDR = 		0x1a,	// Write new I2C address after unlock.
+	LAST_DELAY_LOW = 		0x15,	// Previous distance measurement low byte.
+	UNIT_ID_HIGH = 			0x16,	// Serial number high byte.
+	UNIT_ID_LOW = 			0x17,	// Serial number low byte.
+	I2C_ID_HIGH = 			0x18,	// Write serial number high byte for I2C address unlock.
+	I2C_ID_LOW = 				0x19,	// Write serial number low byte for I2C address unlock.
+	I2C_SEC_ADDR = 			0x1a,	// Write new I2C address after unlock.
 	THRESHOLD_BYPASS = 	0x1c,	// Peak detection threshold bypass.
-	I2C_CONFIG = 		0x1e,	// Default address response control.
-	COMMAND = 			0x40,	// State command.
-	MEASURE_DELAY = 	0x45,	// Delay between automatic measurements.
-	PEAK_BCK = 			0x4c,	// Second largest peak value in correlation record.
-	CORR_DATA = 		0x52,	// Correlation record data low byte.
-	CORR_DATA_SIGN = 	0x53,	// Correlation record data high byte.
-	ACQ_SETTINGS =		0x5d,	// Correlation record memory bank select.
-	POWER_CONTROL =		0x65, 	// Power state control.
+	I2C_CONFIG = 				0x1e,	// Default address response control.
+	COMMAND = 					0x40,	// State command.
+	MEASURE_DELAY = 		0x45,	// Delay between automatic measurements.
+	PEAK_BCK = 					0x4c,	// Second largest peak value in correlation record.
+	CORR_DATA = 				0x52,	// Correlation record data low byte.
+	CORR_DATA_SIGN = 		0x53,	// Correlation record data high byte.
+	ACQ_SETTINGS =			0x5d,	// Correlation record memory bank select.
+	POWER_CONTROL =			0x65,	// Power state control.
 };
 
 enum DEV_STATUS 
@@ -52,21 +55,24 @@ enum DEV_STATUS
 class CLidarLite
 {
   private:
-	std::string serial_;	// Serial port (e.g., run dmesg).
-	CUSBI2C* adapter_;		// Device object.
-	int status_;			// Device status.
 
-	// Blinks the usb_i2c adapter led.	
-	void blink_led_usb_adapter(void);
+		CUSBI2C* adapter_;			// Device object.
+		std::string serial_;		// Serial port (e.g., run dmesg).
+		int config_;						// Device configuration option (see Class header).
+		int status_;						// Device status.
+		int max_unbiases_meas_;	// Maximum of unbiased readings (not more than 100).
 
-	// Converts an HEX number (int object) to string. Print purposes.
-	std::string hex_to_str(const int &val);
+		// Blinks the usb_i2c adapter led.	
+		void blink_led_usb_adapter(void);
 
-	// Writes a command (cmd) to the register of the I2C device (addr).
-	void write(unsigned char addr, unsigned char cmd);
+		// Converts an HEX number (int object) to string. Print purposes.
+		std::string hex_to_str(const int &val);
 
-	// Read byte/s (len=[1,2]) from the register of the I2C device (addr).
-	int read(unsigned char addr, int len);
+		// Writes a command (cmd) to the register of the I2C device (addr).
+		void write(unsigned char addr, unsigned char cmd);
+
+		// Read byte/s (len=[1,2]) from the register of the I2C device (addr).
+		int read(unsigned char addr, int len);
 
     // Successive 2-byte readings (Autoincrement of address: A note about 0x8f vs 0x0f)
     // Set the highest bit of any register to "1" if you set the high byte of a register 
@@ -76,24 +82,44 @@ class CLidarLite
     // take two single readings from 0x0f and 0x10, or we could take 2 byte read from 
     // register 0x8f. 0x8f = 10001111 and 0x0f = 00001111, meaning that 0x8f is 0x0f with 
     // the high byte set to "1", ergo it autoincrements.
-	unsigned short int addr_to_read_2bytes(unsigned char first_byte_addr);
+		unsigned short int addr_to_read_2bytes(unsigned char first_byte_addr);
+
+	  // Configure Lidar Lite device with parameters from Class constructor
+	  void config_dev(void);
 
   public:
-    CLidarLite(const std::string &serial);
+
+  	//	- Inputs:
+  	//		- serial: Device serial ID (e.g. run 'dmesg').
+  	// 		- config:	Lidar Lite 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.	
+    CLidarLite(const std::string &serial, const int config = 0);
     ~CLidarLite(void);
 
-	void open(void);
-	void close(void);
-	void reset(void);
+		void open(void);
+		void close(void);
+		void reset(void);
 
-	// The maximum acquisition count limits the number of times the device will
-	// integrate acquisitions to find a correlation record peak (from a returned signal),
-	// which occurs at long range or with low target reflectivity. This controls the
-	// minimum measurement rate and maximum range. The unit-less relationship
-	// is roughly as follows: rate = 1/n and range = n^(1/4), where n is the number of
-	// acquisitions.
-	void set_max_acq_count(const unsigned char &num_acq);
+		// Set new device configuration (see constructor for details on modes)
+		void set_new_config(const int config);
 
+	  // Get range
+	  // Take a distance measurement and return the result.
+		//	- Inputs:
+	  // 		- 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).
+		int get_range(bool biasCorrection = true);
 };
 
 #endif