diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 291516868556be3f2d352a89cccd40a33a4af623..bb01d297211cebe67ce639bde8734260ce33ff25 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -38,6 +38,6 @@ INSTALL(FILES ../Findlidar_lite.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
INSTALL(FILES ../99-lidar_lite.rules DESTINATION /etc/udev/rules.d/)
# Restart UDEV to account for new rules.d
-execute_process(COMMAND sudo service udev restart)
+INSTALL(CODE "execute_process(COMMAND sudo service udev restart)")
ADD_SUBDIRECTORY(examples)
diff --git a/src/examples/lidar_lite_test.cpp b/src/examples/lidar_lite_test.cpp
index ecd2cc3bf57808c8c7691defccde2093edf9c84a..a3eb3144c260d13d5fe814e9c3c51b4928b4a4ee 100644
--- a/src/examples/lidar_lite_test.cpp
+++ b/src/examples/lidar_lite_test.cpp
@@ -1,64 +1,78 @@
#include "lidar_lite.h"
#include "exceptions/lidar_lite_exceptions.h"
-int main(int argc, char *argv[])
+int
+main(int argc, char* argv[])
{
- std::cout << "***************************************************" << std::endl;
- std::cout << "* LIDAR LITE TEST APPLICATION *" << std::endl;
- std::cout << "***************************************************" << std::endl;
+ std::cout << "***************************************************"
+ << std::endl;
+ std::cout << "* LIDAR LITE TEST APPLICATION *"
+ << std::endl;
+ std::cout << "***************************************************"
+ << std::endl;
- std::string serial = "A700evSl";
+ std::string serial = "A700evSl";
- CLidarLite* laser_ptr = new CLidarLite(serial,0);
-
- try
- {
- laser_ptr->open();
-
- // E1: 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;
-
- // E2: New configuration mode
- std::cout << std::endl << "***** Example 2: Set new configuration mode. **************" << std::endl;
- laser_ptr->set_new_config(5);
+ CLidarLite* laser_ptr = new CLidarLite(serial, 0);
- // E3: 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;
+ try {
+ laser_ptr->open();
- // E4: Get correlation values
- std::cout << std::endl << "***** Example 4: Get correlation values. ******************" << std::endl;
- int num_meas = 50;
- std::cout << " - Number of measurements to analize: " << num_meas << std::endl;
- std::vector<int> corr_val = laser_ptr->get_correlation(num_meas);
- std::cout << " - Correlation values: [";
- for (unsigned int ii = 0; ii < corr_val.size(); ++ii)
- {
- if (ii < corr_val.size()-1)
- std::cout << corr_val.at(ii) << ",";
- else
- std::cout << corr_val.at(ii);
- std::cout << std::flush;
- }
- std::cout << "]" << std::endl;
- laser_ptr->close();
+ // E1: 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;
- std::cout << std::endl;
- std::cout << "***************************************************" << std::endl;
- std::cout << "* LIDAR LITE TEST APPLICATION *" << std::endl;
- std::cout << "***************************************************" << std::endl;
+ // E2: New configuration mode
+ std::cout << std::endl
+ << "***** Example 2: Set new configuration mode. **************"
+ << std::endl;
+ laser_ptr->set_new_config(5);
- }catch(CLidarLiteException &e)
- {
- std::cout << e.what() << std::endl;
- }
+ // E3: 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;
- if(laser_ptr != NULL)
- {
- laser_ptr = NULL;
- delete laser_ptr;
- }
+ // E4: Get correlation values
+ std::cout << std::endl
+ << "***** Example 4: Get correlation values. ******************"
+ << std::endl;
+ int num_meas = 50;
+ std::cout << " - Number of measurements to analize: " << num_meas
+ << std::endl;
+ std::vector<int> corr_val = laser_ptr->get_correlation(num_meas);
+ std::cout << " - Correlation values: [";
+ for (unsigned int ii = 0; ii < corr_val.size(); ++ii) {
+ if (ii < corr_val.size() - 1)
+ std::cout << corr_val.at(ii) << ",";
+ else
+ std::cout << corr_val.at(ii);
+ std::cout << std::flush;
+ }
+ std::cout << "]" << std::endl;
+ laser_ptr->close();
+
+ std::cout << std::endl;
+ std::cout << "***************************************************"
+ << std::endl;
+ std::cout << "* LIDAR LITE TEST APPLICATION *"
+ << std::endl;
+ std::cout << "***************************************************"
+ << std::endl;
+
+ } catch (CLidarLiteException& e) {
+ std::cout << e.what() << std::endl;
+ }
+
+ if (laser_ptr != NULL) {
+ laser_ptr = NULL;
+ delete laser_ptr;
+ }
}
diff --git a/src/exceptions/exceptions.cpp b/src/exceptions/exceptions.cpp
index f9cc6ba3ef5b5c42684bc5c135c17ff5f9f47136..a2636a3778d966f9c794e5a94ed4923571f148c9 100644
--- a/src/exceptions/exceptions.cpp
+++ b/src/exceptions/exceptions.cpp
@@ -1,4 +1,5 @@
-// Copyright (C) 2009-2010 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Copyright (C) 2009-2010 Institut de Robòtica i Informà tica Industrial,
+// CSIC-UPC.
// Author Sergi Hernandez (shernand@iri.upc.edu)
// All rights reserved.
//
@@ -17,20 +18,21 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "exceptions.h"
-#include <string.h>
#include <stdio.h>
+#include <string.h>
-const std::string exception_msg="[Exception caught] - ";
+const std::string exception_msg = "[Exception caught] - ";
CException::CException(const std::string& where, const std::string& error_msg)
{
- this->error_msg=exception_msg;
- this->error_msg+=where;
- this->error_msg+="\nError: ";
- this->error_msg+=error_msg;
+ this->error_msg = exception_msg;
+ this->error_msg += where;
+ this->error_msg += "\nError: ";
+ this->error_msg += error_msg;
}
-const std::string& CException::what(void)
+const std::string&
+CException::what(void)
{
return this->error_msg;
}
diff --git a/src/exceptions/exceptions.h b/src/exceptions/exceptions.h
index 0b75285e1724008dae43cdcc9ba7dd87cfd070d8..9a27cd7bb20486b142ee0a5c36304f9a7e8fc5f7 100644
--- a/src/exceptions/exceptions.h
+++ b/src/exceptions/exceptions.h
@@ -1,4 +1,5 @@
-// Copyright (C) 2009-2010 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Copyright (C) 2009-2010 Institut de Robòtica i Informà tica Industrial,
+// CSIC-UPC.
// Author Sergi Hernandez (shernand@iri.upc.edu)
// All rights reserved.
//
@@ -23,8 +24,9 @@
#include <sstream>
#include <string>
-template <typename T_>
-inline std::string tostring (const T_ & src)
+template <typename T_>
+inline std::string
+tostring(const T_& src)
{
std::ostringstream out;
@@ -32,24 +34,25 @@ inline std::string tostring (const T_ & src)
return out.str();
}
-#define _HERE_ (std::string(__PRETTY_FUNCTION__) + " at " + \
- std::string(__FILE__) + ":" +tostring(__LINE__))
+#define _HERE_ \
+ (std::string(__PRETTY_FUNCTION__) + " at " + std::string(__FILE__) + ":" + \
+ tostring(__LINE__))
-/**
+/**
* \brief Generic exception
- *
- * This class inherits from the std::exception class to provide generic
+ *
+ * This class inherits from the std::exception class to provide generic
* exceptions for for all classes. The error message is provided at
* construction time with all the necessary information about the error
- * that has generated the exception. When the exception is caught, the
- * what() function may be used to get the error message.
+ * that has generated the exception. When the exception is caught, the
+ * what() function may be used to get the error message.
*
* This class should be used as a base class for all other exception
- * classes in any program. This way it will be simple to distinguish
+ * classes in any program. This way it will be simple to distinguish
* between system exceptions and application exceptions.
*
- * When throwing exceptions, it is important to free all allocated
- * resources and also to set all the class attributes and variables to a
+ * When throwing exceptions, it is important to free all allocated
+ * resources and also to set all the class attributes and variables to a
* known state. Otherwise the behavior of the program after an exception
* is thrown could be unpredictible.
*
@@ -69,62 +72,63 @@ inline std::string tostring (const T_ & src)
* of the original one.
*
* This code will catch any exception of this class or any inherited class.
- * Several catch statements can be used to catch exceptions of different
+ * Several catch statements can be used to catch exceptions of different
* classes separatelly and handle them in different ways.
*
*/
class CException : public std::exception
{
- protected:
- /**
- * \brief Exception error message.
- *
- * This attribute is a string which contains the error message with the
- * information about the error that has generated the exception. This
- * message is allocated and initialized at construction time and can not
- * be modified afterwards.
- *
- * When the exception is caught, the what() function must be used to get
- * the contents of this error message.
- */
- std::string error_msg;
- public:
- /**
- * \brief Class constructor
- *
- * The constructor of this class allocates the necessary memory to store
- * the error message. In addition to the error message provided to the
- * constructor, the name of the function, filename and line where the error
- * has ocurred, as well as the text "[Exception caught] - " is pre-appended
- * to label the message as an exception.
- *
- * \param where a null terminated string with the information about the name
- * of the function, the source code filename and the line where
- * the exception was generated. This string must be generated
- * by the _HERE_ macro.
- *
- * \param error_msg a null terminated string that contains the error message.
- * This string may have any valid character and there is no
- * limit on its length.
- */
- CException(const std::string& where, const std::string& error_msg);
- /**
- * \brief Function to get the error message
- *
- * This function is used to get the error message with the information about
- * the error that has generated the exception.
- *
- * \return the error message as a constant string to prevent external
- * modifications of the private attributes of the class.
- */
- virtual const std::string& what(void);
- /**
- * \brief Class destructor
- *
- * This destructor just frees all the memory allocated to store the error
- * message.
- */
- virtual ~CException() throw();
+protected:
+ /**
+ * \brief Exception error message.
+ *
+ * This attribute is a string which contains the error message with the
+ * information about the error that has generated the exception. This
+ * message is allocated and initialized at construction time and can not
+ * be modified afterwards.
+ *
+ * When the exception is caught, the what() function must be used to get
+ * the contents of this error message.
+ */
+ std::string error_msg;
+
+public:
+ /**
+ * \brief Class constructor
+ *
+ * The constructor of this class allocates the necessary memory to store
+ * the error message. In addition to the error message provided to the
+ * constructor, the name of the function, filename and line where the error
+ * has ocurred, as well as the text "[Exception caught] - " is pre-appended
+ * to label the message as an exception.
+ *
+ * \param where a null terminated string with the information about the name
+ * of the function, the source code filename and the line where
+ * the exception was generated. This string must be generated
+ * by the _HERE_ macro.
+ *
+ * \param error_msg a null terminated string that contains the error message.
+ * This string may have any valid character and there is no
+ * limit on its length.
+ */
+ CException(const std::string& where, const std::string& error_msg);
+ /**
+ * \brief Function to get the error message
+ *
+ * This function is used to get the error message with the information about
+ * the error that has generated the exception.
+ *
+ * \return the error message as a constant string to prevent external
+ * modifications of the private attributes of the class.
+ */
+ virtual const std::string& what(void);
+ /**
+ * \brief Class destructor
+ *
+ * This destructor just frees all the memory allocated to store the error
+ * message.
+ */
+ virtual ~CException() throw();
};
#endif
diff --git a/src/exceptions/lidar_lite_exceptions.cpp b/src/exceptions/lidar_lite_exceptions.cpp
index 6c7199e418da987658f0a2b97d49722959cd55fd..375f9a298af606c38d796a0a002cacba5ff4f508 100644
--- a/src/exceptions/lidar_lite_exceptions.cpp
+++ b/src/exceptions/lidar_lite_exceptions.cpp
@@ -1,13 +1,15 @@
#include "lidar_lite_exceptions.h"
#include <sstream>
-#include <string.h>
#include <stdio.h>
+#include <string.h>
-const std::string lidar_lite_error_message="LidarLite - ";
+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)
+CLidarLiteException::CLidarLiteException(const std::string& where,
+ const std::string& error_msg)
+ : CException(where, lidar_lite_error_message)
{
std::stringstream text;
-
- this->error_msg+=error_msg;
+
+ this->error_msg += error_msg;
}
diff --git a/src/exceptions/lidar_lite_exceptions.h b/src/exceptions/lidar_lite_exceptions.h
index 67bfa2af783dd9160614b97b2c70b35a5ab7deee..5b7dc8345b1c317b33c239f264e71bb3c4bf00a9 100644
--- a/src/exceptions/lidar_lite_exceptions.h
+++ b/src/exceptions/lidar_lite_exceptions.h
@@ -5,19 +5,19 @@
/**
* \brief Lidar Lite exception
- *
- * This class is used to report general errors of the Lidar Lite devices. These
+ *
+ * This class is used to report general errors of the Lidar Lite devices. These
* errors are normally caused by invalid parameters passed to the functions.
* In this case the error message is used to identify which error has ocuurred.
*
*/
class CLidarLiteException : public CException
{
- public:
- /**
- * \brief
- */
- CLidarLiteException(const std::string& where,const std::string &error_msg);
+public:
+ /**
+ * \brief
+ */
+ CLidarLiteException(const std::string& where, const std::string& error_msg);
};
#endif
diff --git a/src/lidar_lite.cpp b/src/lidar_lite.cpp
index ca4cb411ce41434ee767b751eea20fc2b0d18d7c..3fa7b3b4864f5c898594228f1e26ea03ecf5af41 100644
--- a/src/lidar_lite.cpp
+++ b/src/lidar_lite.cpp
@@ -4,61 +4,64 @@
#include <ctime.h>
//******************************************************************************
-// PUBLIC API
+// PUBLIC API
//******************************************************************************
// The device has a 7-bit slave address with a default value of 0x62.
-// Fill in new address here if changed.
+// Fill in new address here if changed.
// See operating manual for instructions.
unsigned char DEVICE_ID = 0x62;
-CLidarLite::CLidarLite(const std::string &serial, const int config)
+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
+ // 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)
{
- if (this->status_ == RUNNING)
- {
+ if (this->status_ == RUNNING) {
reset();
close();
}
}
-void CLidarLite::open(void)
+void
+CLidarLite::open(void)
{
if (this->status_ != IDLE)
- throw CLidarLiteException(_HERE_, "[CLidarLite]: Device cannot be OPENED because it is already running.");
+ throw CLidarLiteException(
+ _HERE_,
+ "[CLidarLite]: Device cannot be OPENED because it is already running.");
- try
- {
- this->adapter_ = new CUSBI2C("adapter");
+ try {
+ this->adapter_ = new CUSBI2C("adapter");
- // open serial port
- std::cout << "[CLidarLite]: Opening Lidar Lite. " << std::endl;
+ // open serial port
+ std::cout << "[CLidarLite]: Opening Lidar Lite. " << std::endl;
this->adapter_->open(this->serial_);
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;
+ 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);
+ // Set gpio 2 and 3 as i2c
+ this->adapter_->config_gpio(gpio2, i2c);
+ this->adapter_->config_gpio(gpio3, i2c);
- }catch(CException &e)
- {
+ } catch (CException& e) {
throw CLidarLiteException(_HERE_, e.what());
- }
+ }
// Configure device
config_dev();
@@ -67,29 +70,34 @@ void CLidarLite::open(void)
this->status_ = RUNNING;
}
-void CLidarLite::close(void)
+void
+CLidarLite::close(void)
{
if (this->status_ != RUNNING)
- throw CLidarLiteException(_HERE_, "Device cannot be CLOSED because it is not running.");
+ throw CLidarLiteException(
+ _HERE_, "Device cannot be CLOSED because it is not running.");
- if(this->adapter_ != NULL)
- {
+ if (this->adapter_ != NULL) {
this->adapter_ = NULL;
- delete this->adapter_;
+ delete this->adapter_;
}
}
-void CLidarLite::reset(void)
+void
+CLidarLite::reset(void)
{
if (this->status_ != RUNNING)
- throw CLidarLiteException(_HERE_, "Device cannot be RESET because it is not running.");
+ throw CLidarLiteException(
+ _HERE_, "Device cannot be RESET because it is not running.");
- // reset operation
- write(ACQ_COMMAND,0x00);
+ // 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
+ // 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);
@@ -97,82 +105,91 @@ void CLidarLite::reset(void)
config_dev();
}
-void CLidarLite::set_new_config(const int config)
+void
+CLidarLite::set_new_config(const int config)
{
this->config_ = config;
-
+
// Configure device
config_dev();
}
-int CLidarLite::get_range(bool biasCorrection)
+int
+CLidarLite::get_range(bool biasCorrection)
{
if (this->status_ != RUNNING)
- throw CLidarLiteException(_HERE_, "RANGE MEASUREMENT cannot be obtained because the device is not running.");
+ throw CLidarLiteException(_HERE_, "RANGE MEASUREMENT cannot be obtained "
+ "because the device is not running.");
// Initiate an aquisition.
- if(biasCorrection || this->max_unbiases_meas_ > 99)
- {
+ if (biasCorrection || this->max_unbiases_meas_ > 99) {
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);
+ 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
- {
- write(ACQ_COMMAND,0x03);
+ } else {
+ write(ACQ_COMMAND, 0x03);
++this->max_unbiases_meas_;
}
// Read two bytes from register 0x8f and save
int range = NAN;
- range = read(FULL_DELAY_HIGH,2);
+ range = read(FULL_DELAY_HIGH, 2);
return range;
}
-std::vector<int> CLidarLite::get_correlation(const int numberOfReadings)
+std::vector<int>
+CLidarLite::get_correlation(const int numberOfReadings)
{
if (this->status_ != RUNNING)
- throw CLidarLiteException(_HERE_, "CORRELATION between measurements cannot be obtained because the device is not running.");
+ throw CLidarLiteException(_HERE_, "CORRELATION between measurements cannot "
+ "be obtained because the device is not "
+ "running.");
if (numberOfReadings < 2 || numberOfReadings > 1024)
- throw CLidarLiteException(_HERE_, "CORRELATION between measurements cannot be obtained. Incorrect selecetd NUMBER OF READINGS, which must be between [2~1024].");
+ throw CLidarLiteException(
+ _HERE_, "CORRELATION between measurements cannot be obtained. Incorrect "
+ "selecetd NUMBER OF READINGS, which must be between [2~1024].");
// Object to return
std::vector<int> correlationValues;
correlationValues.resize(numberOfReadings);
// Select memory bank
- write(ACQ_SETTINGS,0xc0);
+ write(ACQ_SETTINGS, 0xc0);
// Test mode enable
write(COMMAND, 0x07);
- for(int ii = 0; ii<numberOfReadings; ii++)
- {
+ for (int ii = 0; ii < numberOfReadings; ii++) {
// Read two bytes from 0xd2
- unsigned char addr = addr_to_read_2bytes(CORR_DATA); // See header file for a description of this functionality.
+ unsigned char addr = addr_to_read_2bytes(
+ CORR_DATA); // See header file for a description of this functionality.
unsigned char data[2];
- try {
- this->adapter_->write(DEVICE_ID,&addr,1);
- this->adapter_->read(DEVICE_ID,data,2);
- }
- catch(CException &e)
- {
+ try {
+ this->adapter_->write(DEVICE_ID, &addr, 1);
+ this->adapter_->read(DEVICE_ID, data, 2);
+ } catch (CException& e) {
throw CLidarLiteException(_HERE_, e.what());
}
-
+
// Low byte is the value of the correlation record
correlationValues.at(ii) = (int)data[0];
- // High byte is the sign from the record. If upper byte lsb is set, the value is negative.
- if((int)data[1] == 1)
+ // High byte is the sign from the record. If upper byte lsb is set, the
+ // value is negative.
+ if ((int)data[1] == 1)
correlationValues.at(ii) = -correlationValues.at(ii);
}
// test mode disable
- write(COMMAND,0x00);
+ write(COMMAND, 0x00);
return correlationValues;
}
@@ -181,165 +198,165 @@ std::vector<int> CLidarLite::get_correlation(const int numberOfReadings)
// PRIVATE: DEVICE SPECIFIC FUNCTIONS
//******************************************************************************
-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");
+ 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.
-
+ addr = addr_to_read_2bytes(
+ addr); // See header file for a description of this functionality.
+
// Read STATUS. Repeat until bit 0 (LSB) goes low.
// wait_busy();
- // NOTE: Commented because using this USB adapter the following write actions are slower than usual busy time.
+ // NOTE: Commented because using this USB adapter the following write actions
+ // are slower than usual busy time.
unsigned char data[len];
- try {
- this->adapter_->write(DEVICE_ID,&addr,1);
- this->adapter_->read(DEVICE_ID,data,len);
- }
- catch(CException &e)
- {
+ 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)
+ if (len == 1)
val = data[0];
- else if (len==2)
- {
- unsigned short int read_val = data[0]*256+data[1];
+ else if (len == 2) {
+ unsigned short int read_val = data[0] * 256 + data[1];
val = (int)read_val;
}
return val;
}
-void CLidarLite::wait_busy(void)
+void
+CLidarLite::wait_busy(void)
{
- try {
+ try {
unsigned char addr = STATUS;
- this->adapter_->write(DEVICE_ID,&addr,1);
- }
- catch(CException &e)
- {
+ this->adapter_->write(DEVICE_ID, &addr, 1);
+ } catch (CException& e) {
throw CLidarLiteException(_HERE_, e.what());
}
bool busy = true;
int busyCounter = 0;
- try {
- while (busy)
- {
+ try {
+ while (busy) {
unsigned char data;
- this->adapter_->read(DEVICE_ID,&data,1);
+ this->adapter_->read(DEVICE_ID, &data, 1);
busy = data & 0x01;
busyCounter++; // Increment busyCounter for timeout
// Handle timeout condition, exit while loop and goto bailout
- if(busyCounter > 99)
+ if (busyCounter > 99)
break;
}
- }
- catch(CException &e)
- {
+ } catch (CException& e) {
throw CLidarLiteException(_HERE_, e.what());
}
}
-unsigned short int CLidarLite::addr_to_read_2bytes(unsigned char first_byte_addr)
+unsigned short int
+CLidarLite::addr_to_read_2bytes(unsigned char first_byte_addr)
{
- unsigned short int val = (0x01<<7) | first_byte_addr;
+ unsigned short int val = (0x01 << 7) | first_byte_addr;
return val;
}
-void CLidarLite::write(unsigned char addr, unsigned char cmd)
+void
+CLidarLite::write(unsigned char addr, unsigned char cmd)
{
- try {
+ try {
this->adapter_->write_reg(DEVICE_ID, addr, &cmd, 1);
- }
- catch(CException &e)
- {
+ } catch (CException& e) {
throw CLidarLiteException(_HERE_, e.what());
}
}
-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 {
- 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;
-
+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 {
+ 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;
-
+ 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;
-
+ 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;
-
+ 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;
+ 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)
- {
+ std::cout << "[CLidarLite]: Configuration mode set to: " << this->config_
+ << std::endl;
+ } catch (CException& e) {
throw CLidarLiteException(_HERE_, e.what());
}
}
//******************************************************************************
-// PRIVATE: MISC FUNCTIONS
+// PRIVATE: MISC FUNCTIONS
//******************************************************************************
-void CLidarLite::blink_led_usb_adapter(void)
+void
+CLidarLite::blink_led_usb_adapter(void)
{
if (this->status_ != RUNNING)
- throw CLidarLiteException(_HERE_, "USB_I2C adapter LED will not BLINK because Lidar-Lite is not running.");
+ throw CLidarLiteException(
+ _HERE_,
+ "USB_I2C adapter LED will not BLINK because Lidar-Lite is not running.");
- try
- {
- for (int ii = 0; ii < 2; ++ii)
- {
+ 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)
- {
+ } catch (CException& e) {
throw CLidarLiteException(_HERE_, e.what());
}
}
-std::string CLidarLite::hex_to_str(const int &val)
+std::string
+CLidarLite::hex_to_str(const int& val)
{
std::ostringstream ret;
ret << "0x" << std::setfill('0') << std::setw(2) << std::hex << val;
diff --git a/src/lidar_lite.h b/src/lidar_lite.h
index 7eba38bef767b29dce3e5196ab1f5c3287e10c98..54d9d4028a14969b8943f2dfe05dfde1a10433d1 100644
--- a/src/lidar_lite.h
+++ b/src/lidar_lite.h
@@ -1,222 +1,258 @@
#ifndef _LIDAR_LITE_H
#define _LIDAR_LITE_H
-#include <iostream>
-#include <sstream>
-#include <string>
-#include <iomanip>
#include <cmath>
+#include <iomanip>
+#include <iostream>
+#include <sstream>
+#include <string>
#include <usb_i2c.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, // 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.
+{
+ 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
-{
- IDLE,
- RUNNING
+enum DEV_STATUS
+{
+ IDLE,
+ RUNNING
};
class CLidarLite
{
- private:
-
- 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).
-
- /**
- * \brief Blink LED of USB_I2C adapter
- *
- * Blinks the usb_i2c adapter led.
- */
- void blink_led_usb_adapter(void);
-
- /**
- * \brief Convert number from HEX to STR
- *
- * Converts an HEX number (int object) to string. Print purposes.
- *
- * - Inpunts:
- * val: HEX value to be converted.
- */
- std::string hex_to_str(const int &val);
-
- /**
- * \brief Write command to the register of the I2C device
- *
- * Writes a command (cmd) to the register of the I2C device (addr).
- *
- * - Inpunts:
- * addr: HEX address of the I2C device.
- * cmd: Command to be written.
- */
- void write(unsigned char addr, unsigned char cmd);
-
- /**
- * \brief Read command from the register of the I2C device
- *
- * Read byte/s from the register of the I2C device.
- *
- * - Inpunts:
- * addr: HEX address of the I2C device.
- * len: Number of bytes to read (1 or 2 bytes supported).
- */
- int read(unsigned char addr, int len);
-
- /**
- * \brief HEX address convertion for successive 2-byte readings.
- *
- * 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.
- *
- * - Inpunts:
- * first_byte_addr: HEX address of the LSB.
- */
- unsigned short int addr_to_read_2bytes(unsigned char first_byte_addr);
-
- /**
- * \brief Configure I2C device
- *
- * Configure Lidar Lite device with parameters from Class constructor.
- */
- void config_dev(void);
-
- void wait_busy(void);
-
- public:
-
- /**
- * \brief Class constructor
- *
- * Class constructor.
- *
- * - 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);
-
- /**
- * \brief Class destructor
- *
- * Class destructor.
- */
- ~CLidarLite(void);
-
- /**
- * \brief Open I2C device
- *
- * Open I2C device.
- */
- void open(void);
-
- /**
- * \brief Close I2C device
- *
- * Close I2C device.
- */
- void close(void);
-
- /**
- * \brief Reset I2C device
- *
- * Reset I2C device. Configuration is set with initial constructor values.
- */
- void reset(void);
-
- /**
- * \brief Set new I2C device configuration
- *
- * Set new device configuration.
- *
- * -Inputs:
- * - 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.
- */
- void set_new_config(const int config);
-
- /**
- * \brief Get range measurement
- *
- * Take a distance measurement.
- *
- * - 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);
-
- /**
- * \brief Get correlation between measurements
- *
- * The correlation record used to calculate distance can be read from the device.
- * It has a bipolar wave shape, transitioning from a positive going portion to a
- * roughly symmetrical negative going pulse. The point where the signal crosses
- * zero represents the effective delay for the reference and return signals.
- *
- * - Inputs:
- * - numberOfReadings: Default: 256. Maximum of 1024.
- */
- std::vector<int> get_correlation(const int numberOfReadings = 256);
+private:
+ 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).
+
+ /**
+ * \brief Blink LED of USB_I2C adapter
+ *
+ * Blinks the usb_i2c adapter led.
+ */
+ void blink_led_usb_adapter(void);
+
+ /**
+* \brief Convert number from HEX to STR
+*
+* Converts an HEX number (int object) to string. Print purposes.
+*
+* - Inpunts:
+* val: HEX value to be converted.
+*/
+ std::string hex_to_str(const int& val);
+
+ /**
+* \brief Write command to the register of the I2C device
+*
+* Writes a command (cmd) to the register of the I2C device (addr).
+*
+* - Inpunts:
+* addr: HEX address of the I2C device.
+* cmd: Command to be written.
+*/
+ void write(unsigned char addr, unsigned char cmd);
+
+ /**
+* \brief Read command from the register of the I2C device
+*
+* Read byte/s from the register of the I2C device.
+*
+* - Inpunts:
+* addr: HEX address of the I2C device.
+* len: Number of bytes to read (1 or 2 bytes supported).
+*/
+ int read(unsigned char addr, int len);
+
+ /**
+* \brief HEX address convertion for successive 2-byte readings.
+*
+* 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.
+*
+* - Inpunts:
+* first_byte_addr: HEX address of the LSB.
+*/
+ unsigned short int addr_to_read_2bytes(unsigned char first_byte_addr);
+
+ /**
+ * \brief Configure I2C device
+ *
+ * Configure Lidar Lite device with parameters from Class
+*constructor.
+ */
+ void config_dev(void);
+
+ void wait_busy(void);
+
+public:
+ /**
+ * \brief Class constructor
+ *
+ * Class constructor.
+ *
+* - 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);
+
+ /**
+ * \brief Class destructor
+ *
+ * Class destructor.
+ */
+ ~CLidarLite(void);
+
+ /**
+ * \brief Open I2C device
+ *
+ * Open I2C device.
+ */
+ void open(void);
+
+ /**
+ * \brief Close I2C device
+ *
+ * Close I2C device.
+ */
+ void close(void);
+
+ /**
+ * \brief Reset I2C device
+ *
+ * Reset I2C device. Configuration is set with initial
+*constructor values.
+ */
+ void reset(void);
+
+ /**
+ * \brief Set new I2C device configuration
+ *
+ * Set new device configuration.
+ *
+* -Inputs:
+* - 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.
+ */
+ void set_new_config(const int config);
+
+ /**
+ * \brief Get range measurement
+ *
+ * Take a distance measurement.
+ *
+ * - 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);
+
+ /**
+ * \brief Get correlation between measurements
+ *
+ * The correlation record used to calculate distance can be read
+*from the device.
+ * It has a bipolar wave shape, transitioning from a positive going
+*portion to a
+ * roughly symmetrical negative going pulse. The point where the
+*signal crosses
+ * zero represents the effective delay for the reference and return
+*signals.
+ *
+ * - Inputs:
+ * - numberOfReadings: Default: 256. Maximum of 1024.
+ */
+ std::vector<int> get_correlation(const int numberOfReadings = 256);
};
#endif