From 4a9ac784b7b12c1763e7cdee57abfa3d4921200f Mon Sep 17 00:00:00 2001
From: jmseitz <35376116+jmseitz@users.noreply.github.com>
Date: Wed, 10 Jul 2019 09:02:15 -0500
Subject: [PATCH] Add v3HP function to 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 added function illustrates how to disable the internal reference
filter, trigger a measurement (forcing re-init of the reference
filter), and then re-enable the reference filter.

Change-Id: I101a1fb153723bc3ab9842845b7fc3b4aaabaf56
---
 src/LIDARLite_v3HP.cpp | 57 ++++++++++++++++++++++++++++++++++++++++++
 src/LIDARLite_v3HP.h   |  1 +
 2 files changed, 58 insertions(+)

diff --git a/src/LIDARLite_v3HP.cpp b/src/LIDARLite_v3HP.cpp
index b4c81b5..4e05882 100644
--- a/src/LIDARLite_v3HP.cpp
+++ b/src/LIDARLite_v3HP.cpp
@@ -262,6 +262,63 @@ uint16_t LIDARLite_v3HP::readDistance(uint8_t lidarliteAddress)
     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(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
 
diff --git a/src/LIDARLite_v3HP.h b/src/LIDARLite_v3HP.h
index 0dc635d..4982e18 100644
--- a/src/LIDARLite_v3HP.h
+++ b/src/LIDARLite_v3HP.h
@@ -40,6 +40,7 @@ class LIDARLite_v3HP
       void      waitForBusy (uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
       uint8_t   getBusyFlag (uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
       void      takeRange   (uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+      void      resetReferenceFilter (uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
 
       void      write (uint8_t regAddr, uint8_t * dataBytes, uint16_t numBytes, uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
       void      read  (uint8_t regAddr, uint8_t * dataBytes, uint16_t numBytes, uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT);