diff --git a/ReadMe.md b/ReadMe.md
index 9f569b2f996598cb12be21f8672bc6ba4bca9b41..a5a3faca6277f02d9dd91166eb90b67d99f73e56 100644
--- a/ReadMe.md
+++ b/ReadMe.md
@@ -13,10 +13,20 @@ For detailed specifications, pinout, and connection diagrams, please refer to th
### Software dependences
-- [usb_i2c_adapter](https://devel.iri.upc.edu/pub/labrobotica/drivers/usb_i2c_adapter/trunk) C++ library
+- [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`
+
+- [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`
+
+- [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 .. && make && sudo make install`
### Example of usage
diff --git a/src/examples/lidar_lite_test.cpp b/src/examples/lidar_lite_test.cpp
index e7f6b1f245545f6b85ba5ba259e3ab63ec716eb4..75284beb136c5e52a0bbde336d7f2895d7a4b9e1 100644
--- a/src/examples/lidar_lite_test.cpp
+++ b/src/examples/lidar_lite_test.cpp
@@ -8,9 +8,12 @@ int main(int argc, char *argv[])
CLidarLite* laser_ptr = new CLidarLite(device_id,serial);
+
try
{
laser_ptr->open();
+ laser_ptr->open();
+ laser_ptr->reset();
laser_ptr->close();
}catch(CLidarLiteException &e)
{
diff --git a/src/lidar_lite.cpp b/src/lidar_lite.cpp
index 889e3540ed6eb097eccbbab195f8250ca29b6cbb..2954287f0e8ff08ce747a800f76c43b5cf719f5e 100644
--- a/src/lidar_lite.cpp
+++ b/src/lidar_lite.cpp
@@ -4,6 +4,7 @@ CLidarLite::CLidarLite(const unsigned char &dev_id, const std::string &serial)
{
this->dev_id_ = dev_id;
this->serial_ = serial;
+ this->status_ = IDLE;
}
CLidarLite::~CLidarLite(void)
@@ -11,35 +12,23 @@ CLidarLite::~CLidarLite(void)
close();
}
-void CLidarLite::blink_led(void)
+void CLidarLite::write(unsigned char addr, unsigned char cmd)
{
- 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());
- }
+ this->adapter_->write_reg(this->dev_id_, addr, &cmd, 1);
}
-/*int CLidarLite::read(unsigned char &addr, int &len)
+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");
unsigned char data[len];
- this->adapter_->write(this->dev_id_,&addr,len);
- this->adapter_->read(this->dev_id_,&data,len);
+ this->adapter_->write(this->dev_id_,&addr,1);
+ this->adapter_->read(this->dev_id_,data,len);
int val;
if (len==1)
- val = (int)data;
+ val = data[0];
else if (len==2)
{
unsigned short int read_val = data[0]*256+data[1];
@@ -48,13 +37,17 @@ void CLidarLite::blink_led(void)
return val;
}
-*/
+
void CLidarLite::open(void)
{
- this->adapter_ = new CUSBI2C("adapter");
- try
- {
+ if (this->status_ != IDLE)
+ throw CLidarLiteException(_HERE_, "Device cannot be OPENNED because it is already running.");
+
+ try
+ {
+ this->adapter_ = new CUSBI2C("adapter");
+
// open serial port
std::cout << "[CLidarLite] Opening Lidar Lite. " << std::endl;
this->adapter_->open(this->serial_);
@@ -66,38 +59,68 @@ void CLidarLite::open(void)
this->adapter_->config_gpio(gpio2,i2c);
this->adapter_->config_gpio(gpio3,i2c);
+
// *********************************************
- unsigned char cmd = 0x04;
- this->adapter_->write_reg(this->dev_id_, 0x00, &cmd, 1);
+ // 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.
unsigned char busy;
- unsigned char add = 0x01;
- do {
- this->adapter_->write(this->dev_id_,&add,1);
- this->adapter_->read(this->dev_id_,&busy,1);
- }while( busy & 0x01 );
-
- // unsigned short int data;
- unsigned char data[2];
- unsigned short int read_val;
- add = 0x8f;
- this->adapter_->write(this->dev_id_,&add,1);
- this->adapter_->read(this->dev_id_,data,2);
-
- read_val = data[0]*256+data[1];
-
- std::cout << read_val << " cm" << std::endl;
+ do
+ busy = read(0x01,1);
+ while( busy & 0x01 );
+
+
+ // 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;
// ************************************************
+
}catch(CException &e)
{
throw CLidarLiteException(_HERE_, e.what());
}
+
+ this->status_ = RUNNING;
}
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)
@@ -110,3 +133,29 @@ void CLidarLite::close(void)
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.");
+
+ write(ACQ_COMMAND,0x00);
+}
+
+
+void CLidarLite::blink_led(void)
+{
+ 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());
+ }
+}
diff --git a/src/lidar_lite.h b/src/lidar_lite.h
index 14500c1db6b4cfaea4ac7abefb250a95491607a3..5b0b0d336553b49fb642335940eb9c575615d118 100644
--- a/src/lidar_lite.h
+++ b/src/lidar_lite.h
@@ -5,16 +5,57 @@
#include "usb_i2c.h"
#include "exceptions/lidar_lite_exceptions.h"
+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,
+};
+
+enum DEV_STATUS
+{
+ IDLE,
+ RUNNING
+};
+
class CLidarLite
{
private:
- std::string serial_; // Serial port (e.g., run dmesg)
- unsigned char dev_id_; // Device ID (from datasheet)
- CUSBI2C* adapter_; // Device object
+ 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.
void blink_led(void);
- // int read(unsigned char &addr, int &len);
+
+ void write(unsigned char addr, unsigned char cmd);
+ int read(unsigned char addr, int len);
public:
CLidarLite(const unsigned char &dev_id, const std::string &serial);
@@ -22,6 +63,7 @@ class CLidarLite
void open(void);
void close(void);
+ void reset(void);
};
#endif