Revision 0:417c1bd45a3c, committed 2019-09-20

Comitter:

hubercam

Date:

Fri Sep 20 14:15:12 2019 +0000

Commit message:

+ LIDARLite_v3HP.cpp; + LIDARLite_v3HP.h

Changed in this revision

--- /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 &regAddr, 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 &regAddr, 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, &regAddrC, 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 */

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LIDARLite_v3HP.h	Fri Sep 20 14:15:12 2019 +0000
@@ -0,0 +1,63 @@
+/*------------------------------------------------------------------------------
+
+  LIDARLite_v3HP Arduino Library
+  LIDARLite_v3HP_v3HP.h
+
+  This library provides quick access to all the basic functions of LIDAR-Lite
+  via the Arduino 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.
+
+------------------------------------------------------------------------------*/
+#ifndef LIDARLite_v3HP_h
+#define LIDARLite_v3HP_h
+
+#define LIDARLITE_ADDR_DEFAULT 0x62
+
+#include <cstdint>
+#include "mbed.h"
+
+class LIDARLite_v3HP
+{
+
+    public:
+
+        uint8_t addr_;
+        I2C *i2c_;
+
+        LIDARLite_v3HP(I2C *i2c);
+        LIDARLite_v3HP(I2C *i2c, uint8_t &addr);
+
+        //virtual ~LIDARLite_v3HP();
+
+        void      configure   (const uint8_t &configuration = 0, const uint8_t &lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+        void      setI2Caddr  (const uint8_t &newAddress, uint8_t &disableDefault, const uint8_t &lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+        uint16_t  readDistance(const uint8_t &lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+        void      waitForBusy (const uint8_t &lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+        uint8_t   getBusyFlag (const uint8_t &lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+        void      takeRange   (const uint8_t &lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+        void      resetReferenceFilter (const uint8_t &lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+
+        void      write (const uint8_t &regAddr, uint8_t * dataBytes,const uint16_t &numBytes, const uint8_t &lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+        void      read  (const uint8_t &regAddr, uint8_t * dataBytes,const uint16_t &numBytes, const uint8_t &lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+
+        void      correlationRecordToSerial (const uint16_t &numberOfReadings = 1024,const uint8_t &lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+
+    private:
+        
+};
+
+#endif