added new function to se max acq count

src/examples/lidar_lite_test.cpp

@@ -4,14 +4,14 @@ int main(int argc, char *argv[])
 {
 
 	std::string serial="A700evSl";
-	unsigned char device_id=0x62;
-
-	CLidarLite* laser_ptr = new CLidarLite(device_id,serial);
+	
+	CLidarLite* laser_ptr = new CLidarLite(serial);
 	
 	try
 	{
 		laser_ptr->open();
 		laser_ptr->reset();
+		laser_ptr->set_max_acq_count(0x80);
 		laser_ptr->close();
 
 	}catch(CLidarLiteException &e)

src/lidar_lite.cpp

 #include "lidar_lite.h"
 
-CLidarLite::CLidarLite(const unsigned char &dev_id, const std::string &serial)
+CLidarLite::CLidarLite(const std::string &serial)
 {
-  this->dev_id_ = dev_id;
   this->serial_ = serial;
   this->status_ = IDLE;
+
+  // 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);
 }
  
 CLidarLite::~CLidarLite(void)
@@ -13,36 +17,6 @@ CLidarLite::~CLidarLite(void)
     close();
 }
 
-void CLidarLite::write(unsigned char addr, unsigned char cmd)
-{
-  this->adapter_->write_reg(this->dev_id_, addr, &cmd, 1);
-}
-
-int CLidarLite::read(unsigned char addr, int len)
-{
-  if (len!=1 && len!=2)
-    throw CLidarLiteException(_HERE_, "Expected read data with invalid length. This function only allows to read 1 or 2 bytes");
-
-  if (len == 2)
-    addr = addr_to_read_2bytes(addr); // See header file for a description of this functionality.
-  
-  unsigned char data[len];
-  this->adapter_->write(this->dev_id_,&addr,1);
-  this->adapter_->read(this->dev_id_,data,len);
-
-  int val;
-  if (len==1)
-    val = data[0];
-  else if (len==2)
-  {
-    unsigned short int read_val = data[0]*256+data[1];
-    val = (int)read_val;
-  }
-
-  return val;
-}
-
-
 void CLidarLite::open(void)
 {
   if (this->status_ != IDLE)
@@ -53,11 +27,10 @@ void CLidarLite::open(void)
   	this->adapter_ = new CUSBI2C("adapter");
 
 		// open serial port
-    std::cout << "[CLidarLite] Opening Lidar Lite. " << std::endl; 
+    std::cout << "[CLidarLite]: Opening Lidar Lite. " << std::endl; 
     this->adapter_->open(this->serial_);
-    usleep(10000); // Avoid initialization errors
     std::cout << "  - Serial number: " << this->serial_ << std::endl;
-    std::cout << "  - Firmware revision: " << (int)this->adapter_->get_revision() << std::endl;
+    std::cout << "  - Firmware revision of USB_I2C adapter: " << (int)this->adapter_->get_revision() << std::endl;
 
 		// Set gpio 2 and 3 as i2c    
     this->adapter_->config_gpio(gpio2,i2c);
@@ -78,7 +51,7 @@ void CLidarLite::open(void)
     // std::cout << "Register " << std::hex << ACQ_CONFIG_REG << " with value: " << read(ACQ_CONFIG_REG,1) << std::dec << std::endl;
 
     // read two bytes
-    std::cout << "Lidar-Lite measurement: " << read(FULL_DELAY_HIGH,2) << " cm" << std::endl;
+    std::cout << "[CLidarLite]: Lidar-Lite measurement: " << read(FULL_DELAY_HIGH,2) << " [cm]" << std::endl;
     
     // ************************************************
 
@@ -108,16 +81,7 @@ void CLidarLite::close(void)
   }
 }
 
-void CLidarLite::reset(void)
-{
-  if (this->status_ != RUNNING)
-    throw CLidarLiteException(_HERE_, "Device cannot be RESET because it is not running.");
-
-  write(ACQ_COMMAND,0x00);
-}
-
-
-void CLidarLite::blink_led(void)
+void CLidarLite::blink_led_usb_adapter(void)
 {
   if (this->status_ != RUNNING)
     throw CLidarLiteException(_HERE_, "Usb adapter LED will not BLINK because Lidar-Lite is not running.");
@@ -137,8 +101,96 @@ void CLidarLite::blink_led(void)
   }
 }
 
+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();
+}
+
+//******************
+// DEVICE SPECIFIC *
+//******************
+
+int CLidarLite::read(unsigned char addr, int len)
+{
+  if (len!=1 && len!=2)
+    throw CLidarLiteException(_HERE_, "Expected read data with invalid length. This function only allows to read 1 or 2 bytes");
+
+  if (len == 2)
+    addr = addr_to_read_2bytes(addr); // See header file for a description of this functionality.
+  
+  unsigned char data[len];
+  try { 
+    this->adapter_->write(DEVICE_ID,&addr,1);
+    this->adapter_->read(DEVICE_ID,data,len);
+  }
+  catch(CException &e)
+  {
+    throw CLidarLiteException(_HERE_, e.what());
+  }
+
+  int val;
+  if (len==1)
+    val = data[0];
+  else if (len==2)
+  {
+    unsigned short int read_val = data[0]*256+data[1];
+    val = (int)read_val;
+  }
+
+  return val;
+}
+
 unsigned short int CLidarLite::addr_to_read_2bytes(unsigned char first_byte_addr)
 {
   unsigned short int val = (0x01<<7) | first_byte_addr;
   return val;
-}
+}
\ No newline at end of file
+
+void CLidarLite::write(unsigned char addr, unsigned char cmd)
+{
+  try { 
+    this->adapter_->write_reg(DEVICE_ID, addr, &cmd, 1);
+  }
+  catch(CException &e)
+  {
+    throw CLidarLiteException(_HERE_, e.what());
+  }
+}
+
+void CLidarLite::reset(void)
+{
+  if (this->status_ != RUNNING)
+    throw CLidarLiteException(_HERE_, "Device cannot be RESET because it is not running.");
+
+  try { 
+    write(ACQ_COMMAND,0x00);
+  }
+  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)
+{
+  if (this->status_ != RUNNING)
+    throw CLidarLiteException(_HERE_, "MAXIMUM ACQUISITION COUNT cannot be set because the device 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)
+  {
+    throw CLidarLiteException(_HERE_, e.what());
+  }
+}
+
+

src/lidar_lite.h

@@ -2,39 +2,45 @@
 #define _LIDAR_LITE_H
 
 #include <iostream>
-#include "usb_i2c.h"
+#include <sstream>  
+#include <string>    
+#include <iomanip>
+#include <usb_i2c.h>
 #include "exceptions/lidar_lite_exceptions.h"
 
+// The device has a 7-bit slave address with a default value of 0x62.
+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,
-	SIGNAL_STRENGTH = 	0x0e,
-	FULL_DELAY_HIGH = 	0x0f,
-	FULL_DELAY_LOW = 	0x10,
-	OUTER_LOOP_COUNT = 	0x11,
-	REF_COUNT_VAL = 	0x12,
-	LAST_DELAY_HIGH = 	0x14,
-	LAST_DELAY_LOW = 	0x15,
-	UNIT_ID_HIGH = 		0x16,
-	UNIT_ID_LOW = 		0x17,
-	I2C_ID_HIGH = 		0x18,
-	I2C_ID_LOW = 		0x19,
-	I2C_SEC_ADDR = 		0x1a,
-	THRESHOLD_BYPASS = 	0x1c,
-	I2C_CONFIG = 		0x1e,
-	COMMAND = 			0x40,
-	MEASURE_DELAY = 	0x45,
-	PEAK_BCK = 			0x4c,
-	CORR_DATA = 		0x52,
-	CORR_DATA_SIGN = 	0x53,
-	ACQ_SETTINGS =		0x5d,
-	POWER_CONTROL =		0x65, 
+	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.
+	OUTER_LOOP_COUNT = 	0x11,	// Burst measurement count control.
+	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.
+	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.
 };
 
 enum DEV_STATUS 
@@ -45,35 +51,49 @@ enum DEV_STATUS
 
 class CLidarLite
 {
-	private:
-		std::string serial_;	// Serial port (e.g., run dmesg).
-		unsigned char dev_id_;	// Device ID (from datasheet).
-		CUSBI2C* adapter_;		// Device object.
-		int status_;			// Device status.
+  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);
 
-		void blink_led(void);
+	// Converts an HEX number (int object) to string. Print purposes.
+	std::string hex_to_str(const int &val);
 
+	// Writes a command (cmd) to the register of the I2C device (addr).
+	void write(unsigned char addr, unsigned char cmd);
 
-		void write(unsigned char addr, unsigned char cmd);
-		int read(unsigned char addr, int len);
+	// 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 
-	    // and then take succesive readings from that register, then LIDAR-Lite automatically 
-	    // increments the register one for each read. 
-	    // An example: If we want to read the high and low bytes for the distance, we could 
-	    // 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);
+    // 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 
+    // and then take succesive readings from that register, then LIDAR-Lite automatically 
+    // increments the register one for each read. 
+    // An example: If we want to read the high and low bytes for the distance, we could 
+    // 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);
 
   public:
-    CLidarLite(const unsigned char &dev_id, const std::string &serial);
+    CLidarLite(const std::string &serial);
     ~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);
+
 };
 
 #endif