christophe vermaelen
update LIDARLite_v3HP just to be functionnal. IUT GEII NICE
Diff: LIDARLite_v3HP.cpp
- Revision:
- 0:417c1bd45a3c
- Child:
- 1:bd9e4d198947
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LIDARLite_v3HP.cpp Fri Sep 20 14:15:12 2019 +0000
@@ -0,0 +1,465 @@
+/*------------------------------------------------------------------------------
+
+ LIDARLite_v3HP Arduino Library
+ LIDARLite_v3HP.cpp
+
+ This library provides quick access to the basic functions of LIDAR-Lite
+ via the Nucleo interface. Additionally, it can provide a user of any
+ platform with a template for their own application code.
+
+ Copyright (c) 2018 Garmin Ltd. or its subsidiaries.
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+
+------------------------------------------------------------------------------*/
+
+#include <cstdarg>
+#include <cstdint>
+#include "LIDARLite_v3HP.h"
+#include "mbed.h"
+
+/*------------------------------------------------------------------------------
+ Configure
+
+ Selects one of several preset configurations.
+
+ Parameters
+ ------------------------------------------------------------------------------
+ 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.
+ lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+ operating manual for instructions.
+------------------------------------------------------------------------------*/
+LIDARLite_v3HP::LIDARLite_v3HP(I2C *i2c):i2c_(i2c)
+{
+ addr_ = LIDARLITE_ADDR_DEFAULT;
+
+} /* LIDARLite_v3HP::LIDARLite_v3HP */
+
+LIDARLite_v3HP::LIDARLite_v3HP(I2C *i2c, uint8_t &addr):i2c_(i2c)
+{
+ addr_ = addr;
+
+} /* LIDARLite_v3HP::LIDARLite_v3HP */
+
+void LIDARLite_v3HP::configure(const uint8_t &configuration, const uint8_t &lidarliteAddress)
+{
+ uint8_t sigCountMax;
+ uint8_t acqConfigReg;
+ uint8_t refCountMax;
+ uint8_t thresholdBypass;
+
+ switch (configuration)
+ {
+ case 0: // Default mode, balanced performance
+ sigCountMax = 0x80; // Default
+ acqConfigReg = 0x08; // Default
+ refCountMax = 0x05; // Default
+ thresholdBypass = 0x00; // Default
+ break;
+
+ case 1: // Short range, high speed
+ sigCountMax = 0x1d;
+ acqConfigReg = 0x08; // Default
+ refCountMax = 0x03;
+ thresholdBypass = 0x00; // Default
+ break;
+
+ case 2: // Default range, higher speed short range
+ sigCountMax = 0x80; // Default
+ acqConfigReg = 0x00;
+ refCountMax = 0x03;
+ thresholdBypass = 0x00; // Default
+ break;
+
+ case 3: // Maximum range
+ sigCountMax = 0xff;
+ acqConfigReg = 0x08; // Default
+ refCountMax = 0x05; // Default
+ thresholdBypass = 0x00; // Default
+ break;
+
+ case 4: // High sensitivity detection, high erroneous measurements
+ sigCountMax = 0x80; // Default
+ acqConfigReg = 0x08; // Default
+ refCountMax = 0x05; // Default
+ thresholdBypass = 0x80;
+ break;
+
+ case 5: // Low sensitivity detection, low erroneous measurements
+ sigCountMax = 0x80; // Default
+ acqConfigReg = 0x08; // Default
+ refCountMax = 0x05; // Default
+ thresholdBypass = 0xb0;
+ break;
+
+ case 6: // Short range, high speed, higher error
+ sigCountMax = 0x04;
+ acqConfigReg = 0x01; // turn off short_sig, mode pin = status output mode
+ refCountMax = 0x03;
+ thresholdBypass = 0x00;
+ break;
+ }
+
+ write(0x02, &sigCountMax , 1, lidarliteAddress);
+ write(0x04, &acqConfigReg , 1, lidarliteAddress);
+ write(0x12, &refCountMax , 1, lidarliteAddress);
+ write(0x1c, &thresholdBypass, 1, lidarliteAddress);
+} /* LIDARLite_v3HP::configure */
+
+/*------------------------------------------------------------------------------
+ Set I2C Address
+
+ Set Alternate I2C Device Address. See Operation Manual for additional info.
+
+ Parameters
+ ------------------------------------------------------------------------------
+ newAddress: desired secondary I2C device address
+ disableDefault: a non-zero value here means the default 0x62 I2C device
+ address will be disabled.
+ lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+ operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite_v3HP::setI2Caddr (const uint8_t &newAddress, uint8_t &disableDefault,
+ const uint8_t &lidarliteAddress)
+{
+ uint8_t dataBytes[2];
+
+ // Read UNIT_ID serial number bytes and write them into I2C_ID byte locations
+ read (0x16, dataBytes, 2, lidarliteAddress);
+ write(0x18, dataBytes, 2, lidarliteAddress);
+
+ // Write the new I2C device address to registers
+ // left shift by one to work around data alignment issue in v3HP
+ dataBytes[0] = (newAddress << 1);
+ write(0x1a, dataBytes, 1, lidarliteAddress);
+
+ // Enable the new I2C device address using the default I2C device address
+ read (0x1e, dataBytes, 1, lidarliteAddress);
+ dataBytes[0] = dataBytes[0] | (1 << 4); // set bit to enable the new address
+ write(0x1e, dataBytes, 1, lidarliteAddress);
+
+ // If desired, disable default I2C device address (using the new I2C device address)
+ if (disableDefault)
+ {
+ read (0x1e, dataBytes, 1, newAddress);
+ dataBytes[0] = dataBytes[0] | (1 << 3); // set bit to disable default address
+ write(0x1e, dataBytes, 1, newAddress);
+ }
+} /* LIDARLite_v3HP::setI2Caddr */
+
+/*------------------------------------------------------------------------------
+ Take Range
+
+ Initiate a distance measurement by writing to register 0x00.
+
+ Parameters
+ ------------------------------------------------------------------------------
+ lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+ operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite_v3HP::takeRange(const uint8_t &lidarliteAddress)
+{
+ uint8_t dataByte = 0x01;
+
+ write(0x00, &dataByte, 1, lidarliteAddress);
+} /* LIDARLite_v3HP::takeRange */
+
+/*------------------------------------------------------------------------------
+ Wait for Busy Flag
+
+ Blocking function to wait until the Lidar Lite's internal busy flag goes low
+
+ Parameters
+ ------------------------------------------------------------------------------
+ lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+ operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite_v3HP::waitForBusy(const uint8_t &lidarliteAddress)
+{
+ uint16_t busyCounter = 0; // busyCounter counts number of times busy flag is checked, for timeout
+ uint8_t busyFlag = 1; // busyFlag monitors when the device is done with a measurement
+
+ while (busyFlag) // Loop until device is not busy
+ {
+ // Handle timeout condition, exit while loop and goto bailout
+ if (busyCounter > 9999)
+ {
+ break;
+ }
+
+ busyFlag = getBusyFlag(lidarliteAddress);
+
+ // Increment busyCounter for timeout
+ busyCounter++;
+ }
+
+ // bailout reports error over serial
+ if (busyCounter > 9999)
+ {
+ }
+} /* LIDARLite_v3HP::waitForBusy */
+
+/*------------------------------------------------------------------------------
+ Get Busy Flag
+
+ Read BUSY flag from device registers. Function will return 0x00 if not busy.
+
+ Parameters
+ ------------------------------------------------------------------------------
+ lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+ operating manual for instructions.
+------------------------------------------------------------------------------*/
+uint8_t LIDARLite_v3HP::getBusyFlag(const uint8_t &lidarliteAddress)
+{
+ uint8_t busyFlag; // busyFlag monitors when the device is done with a measurement
+
+ // Read status register to check busy flag
+ read(0x01, &busyFlag, 1, lidarliteAddress);
+
+ // STATUS bit 0 is busyFlag
+ busyFlag &= 0x01;
+
+ return busyFlag;
+} /* LIDARLite_v3HP::getBusyFlag */
+
+/*------------------------------------------------------------------------------
+ Read Distance
+
+ Read and return result of distance measurement.
+
+ Process
+ ------------------------------------------------------------------------------
+ 1. Read two bytes from register 0x8f and save
+ 2. Shift the first value from 0x8f << 8 and add to second value from 0x8f.
+ The result is the measured distance in centimeters.
+
+ Parameters
+ ------------------------------------------------------------------------------
+ lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+ operating manual for instructions.
+------------------------------------------------------------------------------*/
+uint16_t LIDARLite_v3HP::readDistance(const uint8_t &lidarliteAddress)
+{
+ uint16_t distance;
+ uint8_t dataBytes[2];
+
+ // Read two bytes from register 0x0f and 0x10 (autoincrement)
+ read(0x0f, dataBytes, 2, lidarliteAddress);
+
+ // Shift high byte and add to low byte
+ distance = (dataBytes[0] << 8) | dataBytes[1];
+
+ return (distance);
+} /* LIDARLite_v3HP::readDistance */
+
+/*------------------------------------------------------------------------------
+ Reset Reference Filter
+
+ In some scenarios, power-on transients in the LIDAR-Lite v3HP can result in
+ initial measurements that may be a few centimeters short of actual distance.
+ This symptom will eventually rectify itself after a few hundred measurements.
+ This symptom can also be rectified more quickly by resetting the unit's internal
+ reference filter. The process here illustrates how to disable the internal
+ reference filter, trigger a measurement (forcing re-init of the reference
+ filter), and then re-enable the filter.
+
+ Process
+ ------------------------------------------------------------------------------
+ 1. Disable the LIDAR-Lite reference filter
+ 2. Set reference integration count to max
+ 3. Trigger a measurement
+ 4. Restore reference integration count
+ 5. Re-enable reference filter
+
+ Parameters
+ ------------------------------------------------------------------------------
+ lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+ operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite_v3HP::resetReferenceFilter(const uint8_t &lidarliteAddress)
+{
+ uint8_t dataBytes[2];
+ uint8_t acqConfigReg;
+ uint8_t refCountMax;
+
+ // Set bit 4 of the acquisition configuration register (disable reference filter)
+ read(0x04, dataBytes, 1, lidarliteAddress); // Read address 0x04 (acquisition config register)
+ acqConfigReg = dataBytes[0]; // store for later restoration
+ dataBytes[0] = dataBytes[0] | 0x10; // turn on disable of ref filter
+ write(0x04, dataBytes, 1, lidarliteAddress); // write it back
+
+ // Set reference integration count to max
+ read(0x12, dataBytes, 1, lidarliteAddress); // Read address 0x12 (ref integration count)
+ refCountMax = dataBytes[0]; // store for later restoration
+ dataBytes[0] = 0xff; // we want to reference to overflow quickly
+ write(0x12, dataBytes, 1, lidarliteAddress); // write ref integration count
+
+ // Trigger a measurement
+ waitForBusy(lidarliteAddress);
+ takeRange(lidarliteAddress);
+ waitForBusy(lidarliteAddress);
+ // ... no need to read the distance, it is immaterial
+
+ // Restore previous reference integration count
+ dataBytes[0] = refCountMax;
+ write(0x12, dataBytes, 1, lidarliteAddress);
+
+ // Restore previous acquisition configuration register (re-enabling reference filter)
+ dataBytes[0] = acqConfigReg;
+ write(0x04, dataBytes, 1, lidarliteAddress);
+} /* LIDARLite_v3HP::resetReferenceFilter */
+
+/*------------------------------------------------------------------------------
+ Write
+
+ Perform I2C write to device. The I2C peripheral in the LidarLite v3 HP
+ will receive multiple bytes in one I2C transmission. The first byte is
+ always the register address. The the bytes that follow will be written
+ into the specified register address first and then the internal address
+ in the Lidar Lite will be auto-incremented for all following bytes.
+
+ Parameters
+ ------------------------------------------------------------------------------
+ regAddr: register address to write to
+ dataBytes: pointer to array of bytes to write
+ numBytes: number of bytes in 'dataBytes' array to write
+ lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+ operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite_v3HP::write (const uint8_t ®Addr, uint8_t * dataBytes,
+ const uint16_t &numBytes, const uint8_t &lidarliteAddress)
+{
+ int nackCatcher;
+
+ // i2c Syntax
+ // -----------------------------------------------------------------
+ // I2C.write(i2cAddress, *data, length, repeated = false)
+
+
+ nackCatcher = i2c_->write(lidarliteAddress<<1, (char *)dataBytes, numBytes, false);
+
+ // A nack means the device is not responding. Report the error over serial.
+ if (nackCatcher != 0)
+ {
+ printf("> nack\n\r");
+ }
+
+ wait_us(100); // 100 us delay for robustness with successive reads and writes
+} /* LIDARLite_v3HP::write */
+
+/*------------------------------------------------------------------------------
+ Read
+
+ Perform I2C read from device. The I2C peripheral in the LidarLite v3 HP
+ will send multiple bytes in one I2C transmission. The register address must
+ be set up by a previous I2C write. The bytes that follow will be read
+ from the specified register address first and then the internal address
+ pointer in the Lidar Lite will be auto-incremented for following bytes.
+
+ Will detect an unresponsive device and report the error over serial.
+
+ Parameters
+ ------------------------------------------------------------------------------
+ regAddr: register address to write to
+ dataBytes: pointer to array of bytes to write
+ numBytes: number of bytes in 'dataBytes' array to write
+ lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+ operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite_v3HP::read (const uint8_t ®Addr, uint8_t * dataBytes,
+ const uint16_t &numBytes, const uint8_t &lidarliteAddress)
+{
+ int nackCatcher = 0;
+
+ char regAddrC = char(regAddr);
+
+ // A nack means the device is not responding, report the error over serial
+ nackCatcher = i2c_->write(lidarliteAddress<<1, ®AddrC, 1, false); // false means perform repeated start
+
+ if (nackCatcher != 0)
+ {
+ printf("> nack\n\r");
+ }
+
+ // Perform read, save in dataBytes array
+ i2c_->read(lidarliteAddress<<1, (char *)dataBytes, numBytes);
+
+} /* LIDARLite_v3HP::read */
+
+/*------------------------------------------------------------------------------
+ Correlation Record To Serial
+
+ 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.
+
+ Process
+ ------------------------------------------------------------------------------
+ 1. Take a distance reading (there is no correlation record without at least
+ one distance reading being taken)
+ 2. Set test mode select by writing 0x07 to register 0x40
+ 3. For as many readings as you want to take (max is 1024)
+ 1. Read two bytes from 0x52
+ 2. The Low byte is the value from the record
+ 3. The high byte is the sign from the record
+
+ Parameters
+ ------------------------------------------------------------------------------
+ separator: the separator between serial data words
+ numberOfReadings: Default: 1024. Maximum of 1024
+ lidarliteAddress: Default 0x62. Fill in new address here if changed. See
+ operating manual for instructions.
+------------------------------------------------------------------------------*/
+void LIDARLite_v3HP::correlationRecordToSerial(
+ const uint16_t &numberOfReadings, const uint8_t &lidarliteAddress)
+{
+ uint16_t i = 0;
+ uint8_t dataBytes[2]; // Array to store read / write data
+ int16_t correlationValue = 0; // Var to store value of correlation record
+
+ // Test mode enable
+ dataBytes[0] = 0x07;
+ write(0x40, dataBytes, 1, lidarliteAddress);
+
+ for (i=0 ; i<numberOfReadings ; i++)
+ {
+ read(0x52, dataBytes, 2, lidarliteAddress);
+
+ // Low byte is the value of the correlation record
+ correlationValue = (uint16_t) dataBytes[0];
+
+ // if upper byte lsb is one, the value is negative
+ // so here we test to artifically sign extend the data
+ if ( (int) dataBytes[1] == 1)
+ {
+ correlationValue |= 0xff00;
+ }
+ printf("%d\n\r", correlationValue);
+
+ }
+
+ // test mode disable
+ dataBytes[0] = 0;
+ write(0x40, dataBytes, 1, lidarliteAddress);
+} /* LIDARLite_v3HP::correlationRecordToSerial */