v3HP Library Updates

Update read function for better repeated start usage. Reduce 16's to 8's where it makes sense. Change-Id: Id33a4a97fbb69c0b5ef3b07e5aefdcd958bd4b20
garmin · Oct 19, 2020 · c2c51c4 · c2c51c4
1 parent 32736d9
commit c2c51c4
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 34 deletions.
diff --git a/src/LIDARLite_v3HP.cpp b/src/LIDARLite_v3HP.cpp
@@ -336,12 +336,12 @@ void LIDARLite_v3HP::resetReferenceFilter(uint8_t lidarliteAddress)
   lidarliteAddress: Default 0x62. Fill in new address here if changed. See
     operating manual for instructions.
 ------------------------------------------------------------------------------*/
-void LIDARLite_v3HP::write(uint8_t regAddr, uint8_t * dataBytes,
-                           uint16_t numBytes, uint8_t lidarliteAddress)
+void LIDARLite_v3HP::write(uint8_t regAddr,  uint8_t * dataBytes,
+                           uint8_t numBytes, uint8_t lidarliteAddress)
 {
-    int nackCatcher;
+    uint8_t nackCatcher;
 
-    Wire.beginTransmission((int) lidarliteAddress);
+    Wire.beginTransmission(lidarliteAddress);
 
     // Wire.write Syntax
     // -----------------------------------------------------------------
@@ -350,14 +350,13 @@ void LIDARLite_v3HP::write(uint8_t regAddr, uint8_t * dataBytes,
     // Wire.write(data, length)  - an array of data to send as bytes
 
     // First byte of every write sets the LidarLite's internal register address pointer
-    Wire.write((int) regAddr);
+    Wire.write(regAddr);
 
     // Subsequent bytes are data writes
-    Wire.write(dataBytes, (int) numBytes);
+    Wire.write(dataBytes, numBytes);
 
     // A nack means the device is not responding. Report the error over serial.
-    nackCatcher = Wire.endTransmission();
-    if (nackCatcher != 0)
+    if ( Wire.endTransmission() )
     {
         Serial.println("> nack");
     }
@@ -384,34 +383,34 @@ void LIDARLite_v3HP::write(uint8_t regAddr, uint8_t * dataBytes,
   lidarliteAddress: Default 0x62. Fill in new address here if changed. See
     operating manual for instructions.
 ------------------------------------------------------------------------------*/
-void LIDARLite_v3HP::read(uint8_t regAddr, uint8_t * dataBytes,
-                          uint16_t numBytes, uint8_t lidarliteAddress)
+void LIDARLite_v3HP::read(uint8_t regAddr,  uint8_t * dataBytes,
+                          uint8_t numBytes, uint8_t lidarliteAddress)
 {
-    uint16_t i = 0;
-    int nackCatcher = 0;
-
-    // Set the internal register address pointer in the Lidar Lite
-    Wire.beginTransmission((int) lidarliteAddress);
-    Wire.write((int) regAddr); // Set the register to be read
-
-    // A nack means the device is not responding, report the error over serial
-    nackCatcher = Wire.endTransmission(false); // false means perform repeated start
-    if (nackCatcher != 0)
+    // This single function performs the following actions -
+    //     1) I2C START
+    //     2) I2C write to set the address
+    //     3) I2C REPEATED START
+    //     4) I2C read to fetch the required data
+    //     5) I2C STOP
+    Wire.requestFrom
+    (
+        lidarliteAddress, // Slave address
+        numBytes,         // number of consecutive bytes to read
+        regAddr,          // address of first register to read
+        1,                // number of bytes in regAddr
+        true              // true = set STOP condition following I2C read
+    );
+
+    uint8_t  numHere = Wire.available();
+    uint8_t  i       = 0;
+
+    while (i < numHere)
     {
-        Serial.println("> nack");
-    }
-
-    // Perform read, save in dataBytes array
-    Wire.requestFrom((int)lidarliteAddress, (int) numBytes);
-    if ((int) numBytes <= Wire.available())
-    {
-        while (i < numBytes)
-        {
-            dataBytes[i] = (uint8_t) Wire.read();
-            i++;
-        }
+        dataBytes[i] = Wire.read();
+        i++;
     }
 
+    delayMicroseconds(100); // 100 us delay for robustness with successive reads and writes
 } /* LIDARLite_v3HP::read */
 
 /*------------------------------------------------------------------------------

diff --git a/src/LIDARLite_v3HP.h b/src/LIDARLite_v3HP.h
@@ -42,8 +42,8 @@ class LIDARLite_v3HP
       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);
+      void      write (uint8_t regAddr, uint8_t * dataBytes, uint8_t numBytes, uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
+      void      read  (uint8_t regAddr, uint8_t * dataBytes, uint8_t numBytes, uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
 
       void      correlationRecordToSerial (uint16_t numberOfReadings = 1024, uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT);
       void      peakStackRead(int16_t * peakArray, int16_t * distArray, uint8_t lidarliteAddress = LIDARLITE_ADDR_DEFAULT);