Cleaned 2 bytes reading function

src/examples/lidar_lite_test.cpp

@@ -7,14 +7,13 @@ int main(int argc, char *argv[])
 	unsigned char device_id=0x62;
 
 	CLidarLite* laser_ptr = new CLidarLite(device_id,serial);
-
 	
 	try
 	{
-		laser_ptr->open();
 		laser_ptr->open();
 		laser_ptr->reset();
 		laser_ptr->close();
+
 	}catch(CLidarLiteException &e)
 	{
 		std::cout << e.what() << std::endl;

src/lidar_lite.cpp

@@ -9,7 +9,8 @@ CLidarLite::CLidarLite(const unsigned char &dev_id, const std::string &serial)
  
 CLidarLite::~CLidarLite(void)
 {
-	close();
+	if (this->status_ == RUNNING)
+    close();
 }
 
 void CLidarLite::write(unsigned char addr, unsigned char cmd)
@@ -22,6 +23,9 @@ 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);
@@ -42,7 +46,7 @@ int CLidarLite::read(unsigned char addr, int len)
 void CLidarLite::open(void)
 {
   if (this->status_ != IDLE)
-    throw CLidarLiteException(_HERE_, "Device cannot be OPENNED because it is already running.");
+    throw CLidarLiteException(_HERE_, "Device cannot be OPENED because it is already running.");
 
   try
   {
@@ -55,56 +59,26 @@ void CLidarLite::open(void)
     std::cout << "  - Serial number: " << this->serial_ << std::endl;
     std::cout << "  - Firmware revision: " << (int)this->adapter_->get_revision() << std::endl;
 
-		// Setup gpio 2 and 3 as i2c    
+		// Set gpio 2 and 3 as i2c    
     this->adapter_->config_gpio(gpio2,i2c);
     this->adapter_->config_gpio(gpio3,i2c);
 
 
-    // *********************************************
+    // TEST WORK IN PROGRESS **************************
 
-    // unsigned char cmd = 0x04;
-    // this->adapter_->write_reg(this->dev_id_, 0x00, &cmd, 1);
     write(ACQ_COMMAND,0x04);
 
-    // unsigned char add = 0x01;
-
-    // int data_read;
-
-    //Read register 0x01. Repeat until bit 0 (LSB) goes low.
+    //Read STATUS. Repeat until bit 0 (LSB) goes low.
     unsigned char busy;
     do 
-      busy = read(0x01,1);
+      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;
 
-    // regular read
-    // unsigned short int read_val;
-
-    // int len = 1;
-    // add = 0x04;
-    // unsigned char data_short[len];
-    // this->adapter_->write(this->dev_id_,&add,1);
-    // this->adapter_->read(this->dev_id_,data_short,len);
-
-    // read_val = data_short[0];
-
-    // std::cout << read_val << std::endl;
-
-    std::cout << read(0x04,1) << std::endl;
-
-    // double read
-
-    // len = 2;
-    // unsigned char data[len];
-    // add = 0x8f;
-    // this->adapter_->write(this->dev_id_,&add,1);
-    // this->adapter_->read(this->dev_id_,data,len);
-
-    // read_val = data[0]*256+data[1];
-
-    // std::cout << read_val << " cm" << std::endl;
-
-    std::cout << read(0x8f,2) << " cm" << std::endl;
+    // read two bytes
+    std::cout << "Lidar-Lite measurement: " << read(FULL_DELAY_HIGH,2) << " cm" << std::endl;
     
     // ************************************************
 
@@ -145,6 +119,9 @@ void CLidarLite::reset(void)
 
 void CLidarLite::blink_led(void)
 {
+  if (this->status_ != RUNNING)
+    throw CLidarLiteException(_HERE_, "Usb adapter LED will not BLINK because Lidar-Lite is not running.");
+
   try
   {
     for (int ii = 0; ii < 2; ++ii)
@@ -159,3 +136,9 @@ void CLidarLite::blink_led(void)
     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;
+  return val;
+}
\ No newline at end of file

src/lidar_lite.h

@@ -57,6 +57,16 @@ class CLidarLite
 		void write(unsigned char addr, unsigned char cmd);
 		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);
+
   public:
     CLidarLite(const unsigned char &dev_id, const std::string &serial);
     ~CLidarLite(void);