diff --git a/src/examples/lidar_lite_test.cpp b/src/examples/lidar_lite_test.cpp
index 334f042b35966864043a9d06756e06e58b668941..936bed8d869fed2bc910b051a440a7c25e6e79c5 100644
--- a/src/examples/lidar_lite_test.cpp
+++ b/src/examples/lidar_lite_test.cpp
@@ -14,22 +14,42 @@ int main(int argc, char *argv[])
{
laser_ptr->open();
- // Test 1: Get 10 measurements with bias correction
+ // 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;
- // New configuration mode
+ // E2: 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
+ // 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;
+ // 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;
diff --git a/src/lidar_lite.cpp b/src/lidar_lite.cpp
index 4f5968d1e8363b0271988ec61b93b560e40d3eb4..f9581fb1e8e000ded4d52ad1d9d1d198877c962b 100644
--- a/src/lidar_lite.cpp
+++ b/src/lidar_lite.cpp
@@ -127,6 +127,52 @@ int CLidarLite::get_range(bool biasCorrection)
return range;
}
+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.");
+
+ 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].");
+
+ // Object to return
+ std::vector<int> correlationValues;
+ correlationValues.resize(numberOfReadings);
+
+ // Select memory bank
+ write(ACQ_SETTINGS,0xc0);
+
+ // Test mode enable
+ write(COMMAND, 0x07);
+
+ 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 data[2];
+ 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)
+ correlationValues.at(ii) = -correlationValues.at(ii);
+ }
+
+ // test mode disable
+ write(COMMAND,0x00);
+
+ return correlationValues;
+}
+
//******************************************************************************
// PRIVATE: DEVICE SPECIFIC FUNCTIONS
//******************************************************************************
diff --git a/src/lidar_lite.h b/src/lidar_lite.h
index 465f5c6f58a87b2ee458b9a18e624e510ec7af59..00e4c287a94994f6ae35a5a29162d26be62dc0b6 100644
--- a/src/lidar_lite.h
+++ b/src/lidar_lite.h
@@ -62,64 +62,165 @@ class CLidarLite
int status_; // Device status.
int max_unbiases_meas_; // Maximum of unbiased readings (not more than 100).
- // Blinks the usb_i2c adapter led.
+ /**
+ * \brief Blink LED of USB_I2C adapter
+ *
+ * Blinks the usb_i2c adapter led.
+ */
void blink_led_usb_adapter(void);
- // Converts an HEX number (int object) to string. Print purposes.
+ /**
+ * \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);
- // Writes a command (cmd) to the register of the I2C device (addr).
+ /**
+ * \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);
- // Read byte/s (len=[1,2]) from the register of the I2C device (addr).
+ /**
+ * \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);
- // 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.
+ /**
+ * \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);
- // Configure Lidar Lite device with parameters from Class constructor
+ /**
+ * \brief Configure I2C device
+ *
+ * Configure Lidar Lite device with parameters from Class constructor.
+ */
void config_dev(void);
public:
- // - 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.
+ /**
+ * \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);
- // Set new device configuration (see constructor for details on modes)
+ /**
+ * \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);
- // 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).
+ /**
+ * \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