diff --git a/src/examples/lidar_lite_test.cpp b/src/examples/lidar_lite_test.cpp
index 3dc2a7374c013865d794d3c90dfc1fe47ace0343..09e263c6c8726981655b3f5ed0da3c2e07ccb81b 100644
--- a/src/examples/lidar_lite_test.cpp
+++ b/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)
diff --git a/src/lidar_lite.cpp b/src/lidar_lite.cpp
index 8ee4b8af8b1c311bb45702366ecd0f0f0b0cd4b2..3a21b51414c9543360e1606d3660a5d7c47bd307 100644
--- a/src/lidar_lite.cpp
+++ b/src/lidar_lite.cpp
@@ -1,10 +1,14 @@
#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());
+ }
+}
+
+
diff --git a/src/lidar_lite.h b/src/lidar_lite.h
index 451dd17b08dc38c5b9d4d4f65e082c4ec3580b51..d1e464b1010f2323d2d33bc5581229c4cbb8db28 100644
--- a/src/lidar_lite.h
+++ b/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