diff --git a/src/examples/lidar_lite_test.cpp b/src/examples/lidar_lite_test.cpp
index 09e263c6c8726981655b3f5ed0da3c2e07ccb81b..334f042b35966864043a9d06756e06e58b668941 100644
--- a/src/examples/lidar_lite_test.cpp
+++ b/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)
diff --git a/src/exceptions/lidar_lite_exceptions.cpp b/src/exceptions/lidar_lite_exceptions.cpp
index 19aee6e1965bae65522113fe50f135521589fef4..c288ce5e0845ec019fa077ef4330fe650c4c3d6b 100644
--- a/src/exceptions/lidar_lite_exceptions.cpp
+++ b/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)
{
diff --git a/src/lidar_lite.cpp b/src/lidar_lite.cpp
index 3a21b51414c9543360e1606d3660a5d7c47bd307..4f5968d1e8363b0271988ec61b93b560e40d3eb4 100644
--- a/src/lidar_lite.cpp
+++ b/src/lidar_lite.cpp
@@ -1,9 +1,15 @@
#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();
+}
diff --git a/src/lidar_lite.h b/src/lidar_lite.h
index d1e464b1010f2323d2d33bc5581229c4cbb8db28..465f5c6f58a87b2ee458b9a18e624e510ec7af59 100644
--- a/src/lidar_lite.h
+++ b/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