Tag Archives: Garmin

Integrating Garmin LIDAR-Lite V4/LED into WallE3

23 February 2023 Update:

After pulling the 2nd deck off the robot and physically inspecting the inter-deck connector wiring, I was able to determine that there were three free lines available to replace (or act as) the VL53L0X Teensy reset line.

02 March 2023 Update:

After making the above hardware changes, here is the new system schematic

It was at this point that I discovered that the Garmin library for this device doesn’t have support for I2C ports other than ‘Wire’ (Wire0). After dicking around for a while, I looked again at the Sparkfun library and saw that it does have multiport support – yay! So, after dicking around some more, I got the Garmin device working again on I2C port 3 (Wire2) with the Sparkfun library using my little Teensy 3.5 plugboard setup as shown below:

Note the Garmin LIDAR connected to pins 3/4 (SCL2/SDA2)

However, somewhere in the change-over from Garmin to Sparkfun libraries, I got all screwed up, and couldn’t make sense of the results. So I decided to ‘go back to baseline’ with the ‘Fast’ example from Garmin. Unfortunately this turned out to be less-than-simple. Both the Garmin and Sparkfun libraries use the same .h/.cpp filenames so having both libraries installed posed a problem, so I ‘solved’ that problem by removing the Garmin libraries from the ‘libraries’ folder. So, when I decided to revert to the Garmin ‘Fast’ example (which references the Garmin library, of course), I got the Sparkfun libraries instead, and so down the rabbit hole I went – again.

I eventually figured all this out, but it took my entire evening to do it. I wound up

starting an entirely new VS/VM project called ‘Garmin Fast Example’
copy/pasted the Garmin ‘Fast’ example .ino file into the blank project file
copied the Garmin library .cpp/.h files into the local project folder, and changed their names to make sure they couldn’t be confused with the Sparkfun versions
‘add’ed the .cpp/.h files to the project in Solution Explorer
Edited the ‘fast’ example #include line to match the .cpp/.h filenames (I had changed them to make sure they wouldn’t conflict with the Sparkfun names)
made the required edits to the .ino file to get a more informative readout
Fixed the inevitable screwups.

At this point I had a brand-new ‘Fast’ example file, properly instrumented, which produced the following Excel chart:

So, after blowing the entire evening, I’m finally confident that I once again have a working ‘Fast’ example using the Garmin library as a baseline, so now I can continue the adventure by moving to the Sparkfun library so I can move the LIDAR from Wire0 (the default I2C port) to Wire2 where I need it to integrate with WallE3.

Before switching over to the Sparkfun library, I decided to make a couple more runs for a better understanding of the ‘HIGH ACCURACY’ mode. In the above example ‘0’ is written to the HIGH ACCURACY register (0xEB) to turn it off completely. However, intermediate values from 1 to 20 can also be used. Here’s a run at 0XEB = 10 (0x0A):

And another run with 0XEB = 0x05:

And one more with the register set for 2:

Looking at the above, it seems that a value of ‘5’ should work. This produces a measurement time of about 150-170mSec for a 7m distant target, which is about the most I can reasonably expect to encounter, and this would work fine with a 200mSec front distance cycle time. Or, I could go with a value of ‘2’, suffer a bit less repeatability (not really an issue for the robot) and keep the measurement times well under 100mSec. This would be nice, as then I could use a single measurement interval for all distance measurements.

Next I went back to the Sparkfun version of the library so I could run the Garmin LIDAR-Lite V4/LED unit from Teensy 3.5 Wire2 port (pins 3/4). This program is functionally identical to the Garmin ‘Fast’ example. Here’s the output using 0xEB = 0x02.

Garmin LIDAR on WallE3 I2C port2, 0XEB = 2

The above plot, taken with the Garmin-supplied LIDAR (without Qwiic breakout) connected to SCL2/SDA2 on WallE3’s main Teensy 3.5 processor is basically identical to the one taken on my separate Teensy 3.5 plugboard with the Sparkfun LIDAR (the one with Qwiic connectors).

Mounting the Garmin on WallE3 turned out to be non-trivial. The unit doesn’t have mounting flanges like the Pulsed Light unit, and my first attempt with double-sided tape lasted less than an hour – oops! So, I designed a mounting adaptor that picks up the threaded holes used by the Pulsed Light unit and provides a way to mount the Garmin unit with a plastic sta-strap, as shown in the following photos:

Now that I have the Garmin LIDAR working on WallE3, the next step is to integrate the hardware-specific code (the ‘drivers’) into WallE3’s current software.

05 March 2023 Update:

I ported the relevant setup() code from ‘Garmin_LIDARV4_Sparkfun_V1.ino’ to ‘WallE3_Complete_V2.ino’ and the actual driver code to ‘GetFrontDistCm()’. At this point ‘WallE3_Complete_V2.ino’ compiles properly, but I have not tested anything yet.

06 March 2023 Update:

After porting the code as above, I tried to run ‘WallE3_Complete_V2.ino’, but it hung up where it tries to connect to the Garmin LIDAR…. and this is where my painstaking effort to take things in small steps paid off. I am certifiably obsessed with always having a backup and/or a way to retreat to a known-good baseline. With this project it involved the following:

Before even touching my robot, I got the Sparkfun-supplied Garmin LIDAR (with the Qwiic connector breakout board) working with the Sparkfun library on a Teensy 3.5 on a plugboard – no extra hardware at all, using the default I2C port.
Then I did the same thing with the Garmin-supplied LIDAR (without the Sparkfun breakout board) on the same plugboard with Garmin library.
Then I moved the LIDAR to Wire2 on the plugboard mounted Teensy, and verified that the LIDAR and the Sparkfun library (the Garmin library isn’t multi-port capable) operated OK.
Then I moved the Garmin-supplied LIDAR to my WallE3 robot and connected it to the main Teensy 3.5’s Wire2 port, with just the minimalist test program loaded into the robot’s main Teensy 3.5, and confirmed I could get the same performance as before with the plugboard.
Finally, I ported the necessary code into my ‘WallE3_Complete_V2.ino’, and tried to run it, at which point it hung up on the check for the LIDAR, as noted above

At this point, I immediately backed up to my ‘last-good’ baseline, with the LIDAR test program loaded onto the Teensy 3.5, and confirmed that it still worked fine. Now I know that the hardware is OK, and the problem has to be something screwy with my robot program. Since the point at which the ‘complete’ program died was in setup(), I also know that the problem has to lie before the LIDAR connection check. I also know that since the connection check succeeded with my small test program but not with the ‘complete’ one, the problem has something to do with the way the I2C ports were set up or initialized. Looking backwards in setup() from the LIDAR connection check, I ran across the following lines:

#pragma region I2C_PORTS
  //I2C bus
  Wire.begin();

  //01/31/22 added to enable internal pullups on Wire - the I2C bus connected to 
  //the T3.2 IR Homing processor and the MPU6050 MPU (the MPU does have internal 4.7K pullups installed)
  CORE_PIN19_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);
  CORE_PIN18_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

  Wire1.begin();

  //01/31/22 added to enable internal pullups on Wire1 - the I2C buss connected to the T3.5 VL53L0X array processor 
  CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);
  CORE_PIN38_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

  Wire2.begin();

  //02/25/23 added to enable internal pullups on Wire2 - the I2C buss connected to the Garmin LIDAR-Lite V4/LED
  CORE_PIN3_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);
  CORE_PIN4_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);
#pragma endregion I2C_PORTS

#pragma region I2C_PORTS

//I2C bus

Wire.begin();

//01/31/22 added to enable internal pullups on Wire - the I2C bus connected to

//the T3.2 IR Homing processor and the MPU6050 MPU (the MPU does have internal 4.7K pullups installed)

CORE_PIN19_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

CORE_PIN18_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

Wire1.begin();

//01/31/22 added to enable internal pullups on Wire1 - the I2C buss connected to the T3.5 VL53L0X array processor

CORE_PIN37_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

CORE_PIN38_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

Wire2.begin();

//02/25/23 added to enable internal pullups on Wire2 - the I2C buss connected to the Garmin LIDAR-Lite V4/LED

CORE_PIN3_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

CORE_PIN4_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(2);

#pragma endregion I2C_PORTS

The first two sets of lines in the above snippets were necessary to enable the internal pullup resistors on the main Teensy 3.5’s primary and secondary I2C ports (see this post and this post for all the gory details). The third set of lines was intended to do the same thing (enable the internal ~33KΩ pullups) for Wire2 on pins 3/4), but shouldn’t be needed for the Garmin LIDAR because it provides internal 13KΩ pullups (see this doc, top of column 2 on page 2). I wasn’t sure why having a 13KΩ and 33KΩ pullups would be a problem, but I decided to comment those two lines out and see if it made a difference. Well, as it happened – it did, and now the Garmin LIDAR on I2C port2 (Wire2) responded properly, and with very little additional effort was fully integrated with the rest of the ‘complete’ program.

The reason I took so much time and space describing this relatively minor hiccup in the integration effort is because I wanted to demonstrate how important it is to proceed on a complex task by changing just one thing at a time, and to always have a fallback to a ‘known-good’ baseline. Otherwise you are just asking for an unguided tour through the infinitely turning tunnels down the rabbit-hole.

11 March 2023:

After getting the Garmin LIDAR integrated into WallE3, and cleaning up the normal amount of screwups, I finally got a good run on my office ‘test wall’, and was able to confirm that the Garmin LIDAR was performing well, as shown in the following Excel plot.

Wall run showing Garmin LIDAR ‘test wall’ run

Stay Tuned!

Frank

Garmin LIDAR-Lite V4 LED Study

Leave a reply

Posted 16 February 2023

A week or so ago, while finishing up my ‘WallE3_Complete Testing‘ post, I discovered that the reason I (or at least WallE3, my autonomous wall-following robot) was experiencing STUCK_AHEAD errors was because my current forward distance sensor, a Pulsed Light ‘Blue Label’ LIDAR system, couldn’t measure beyond about 4m. Anything beyond that just got reported as 4m (or as a zero, which I converted to 4m). What that meant was that if WallE3 was wall tracking with a lot of open space ahead, it’s sensed distance was a constant 4m, which eventually (eventually being about 5sec) caused my distance variance calculation to go below the ‘stuck’ threshold, and ‘voila!’ STUCK_AHEAD error. I thought about some work-arounds, but there weren’t any good ones that didn’t come with a lot of downside, so I started looking at the new (well, at least new to me) Garmin LIDAR-Lite V4 LED time-of-flight distance sensor. I ordered one from Sparkfun, and it just arrived today – yay! Now to find out if this will really solve my ‘STUCK’ problem. The Garmin LIDAR is significantly smaller and lighter than my current Pulsed Light ‘Blue Label’ LIDAR Lite. I’ve included some photos here for context/scaling:

The first thing I did was put together a small test setup using a plug board and a Teensy 3.5, as shown below:

I started off the study by using one of Sparkfun’s ‘Example1_GetDistance’ program, (modified for better print output) as shown below:

/*
    Name:       Garmin_LIDARV4_Study.ino
    Created:	2/16/2023 7:45:05 PM
    Author:     FRANKNEWXPS15\Frank
*/


/******************************************************************************
  Reads the distance something is in front of LIDAR and prints it to the Serial port

  Priyanka Makin @ SparkX Labs
  Original Creation Date: Sept 30, 2019

  This code is Lemonadeware; if you see me (or any other SparkFun employee) at the
  local, and you've found our code helpful, please buy us a round!

  Hardware Connections:
  Plug Qwiic LIDAR into Qwiic RedBoard using Qwiic cable.
  Set serial monitor to 115200 baud.

  Distributed as-is; no warranty is given.
******************************************************************************/
#include "LIDARLite_v4LED.h"

LIDARLite_v4LED myLIDAR; //Click here to get the library: http://librarymanager/All#SparkFun_LIDARLitev4 by SparkFun
  float newDistance;

void setup() 
{
  Serial.begin(115200);
  Serial.println("Qwiic LIDARLite_v4 examples");
  Wire.begin(); //Join I2C bus

  //check if LIDAR will acknowledge over I2C
  if (myLIDAR.begin() == false) {
    Serial.println("Device did not acknowledge! Freezing.");
    while (1);
  }
  Serial.println("LIDAR acknowledged!");

  //gfp added
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, LOW);

  Serial.printf("Msec\tDistCm\n");
}

void loop() {

  //getDistance() returns the distance reading in cm
  digitalToggle(LED_BUILTIN);
  newDistance = myLIDAR.getDistance();
  digitalToggle(LED_BUILTIN);

  //Print to Serial port
  //Serial.print("New distance: ");
  //Serial.print(newDistance / 100);
  //Serial.println(" m");

  Serial.printf("%lu\t%2.3f\n",millis(), newDistance);

  //delay(20);  //Don't hammer too hard on the I2C bus
  //delay(200);  //Don't hammer too hard on the I2C bus
}


//void setup()
//{
//  Serial.begin(115200);
//  Serial.printf("Hello World\n");
//}

//void loop()
//{}

Name: Garmin_LIDARV4_Study.ino

Created: 2/16/2023 7:45:05 PM

Author: FRANKNEWXPS15\Frank

/******************************************************************************

Reads the distance something is in front of LIDAR and prints it to the Serial port

Priyanka Makin @ SparkX Labs

Original Creation Date: Sept 30, 2019

This code is Lemonadeware; if you see me (or any other SparkFun employee) at the

local, and you've found our code helpful, please buy us a round!

Hardware Connections:

Plug Qwiic LIDAR into Qwiic RedBoard using Qwiic cable.

Set serial monitor to 115200 baud.

Distributed as-is; no warranty is given.

******************************************************************************/

#include "LIDARLite_v4LED.h"

LIDARLite_v4LED myLIDAR; //Click here to get the library: http://librarymanager/All#SparkFun_LIDARLitev4 by SparkFun

float newDistance;

void setup()

{

Serial.begin(115200);

Serial.println("Qwiic LIDARLite_v4 examples");

Wire.begin(); //Join I2C bus

//check if LIDAR will acknowledge over I2C

if (myLIDAR.begin() == false) {

Serial.println("Device did not acknowledge! Freezing.");

while (1);

}

Serial.println("LIDAR acknowledged!");

//gfp added

pinMode(LED_BUILTIN, OUTPUT);

digitalWrite(LED_BUILTIN, LOW);

Serial.printf("Msec\tDistCm\n");

}

void loop() {

//getDistance() returns the distance reading in cm

digitalToggle(LED_BUILTIN);

newDistance = myLIDAR.getDistance();

digitalToggle(LED_BUILTIN);

//Print to Serial port

//Serial.print("New distance: ");

//Serial.print(newDistance / 100);

//Serial.println(" m");

Serial.printf("%lu\t%2.3f\n",millis(), newDistance);

//delay(20); //Don't hammer too hard on the I2C bus

//delay(200); //Don't hammer too hard on the I2C bus

}

//void setup()

//{

// Serial.begin(115200);

// Serial.printf("Hello World\n");

//}

//void loop()

//{}

When I ran this program, I noticed that regardless of the delay value in loop(), the time stamps associated with successive distance measurement were a LOT further apart than I expected. So, I fiddled around with the experiment and soon discovered that the time between subsequent measurements varied dramatically depending on the actual distance being measured; short distances were measured very quickly, long distances not so much. In a way this makes a lot of sense (it is a ‘time-of-flight’ device after all), but since we are talking about the speed of light – i.e. 3*10^8 m/sec, it seems a little unreasonable for a measurement of something like 4 m to take significantly longer than for a measurement of 1m, don’t you think?

In any case, I set up an experiment where I pointed the unit at a distant (i.e. 4m) target, and then waved my hand in front of the LIDAR to simulate a close target, and had the program print out the time stamp and the distance measurement. I dumped this into Excel, and plotted the measured distance along with the time difference between subsequent measurements, and got the following plot:

As can be seen in the above plot the time required between two measurements at a distance of 450cm is approximately 350mSec – ouch! Conversely, the time required between two measurements at a distance of 20cm is approximately 70mSec – a factor of five – wow!

Looking at the code, it is clear that myLIDAR.getDistance() is a blocking function that first triggers a new measurement, and then waits for the LIDAR to finish before reporting the result. Reading through the Garmin documentation, I learned that Garmin developed an Arduino library for the V4 unit. I loaded this library and ran their ‘v4LED/v4LED_fast’ example. Here’s the code (slightly modified for better printout using a Teensy 3.5):

/*
    Name:       Garmin_LIDARV4_Study_V2.ino
    Created:	2/17/2023 4:34:38 PM
    Author:     FRANKNEWXPS15\Frank

    02/17/23 This version uses the 'v4LED/v4LED_fast' example program from the Garmin library 
*/


/*------------------------------------------------------------------------------

  LIDARLite Arduino Library
  v4LED/v4LED_fast

  This example shows how to take multiple distance measurements with a
  LIDAR-Lite v4 LED with a high rep rate using the optional GPIO pins
  or the I2C port to trigger measurements.

  *** NOTE ***
  The LIDAR-Lite v4 LED is strictly a 3.3V system. The Arduino Due is a
  3.3V system and is recommended for use with the LIDAR-Lite v4 LED.
  Care MUST be taken if connecting to a 5V system such as the Arduino Uno.
  See comment block in the setup() function for details on I2C connections.
  It is recommended to use a voltage level-shifter if connecting the GPIO
  pins to any 5V system I/O.

  Connections:
  LIDAR-Lite 5 VDC   (pin 1) to Arduino 5V
  LIDAR-Lite Ground  (pin 2) to Arduino GND
  LIDAR-Lite I2C SDA (pin 3) to Arduino SDA
  LIDAR-Lite I2C SCL (pin 4) to Arduino SCL

  Optional connections to utilize GPIO triggering:
  LIDAR-Lite GPIOA   (pin 5) to Arduino Digital 2
  LIDAR-Lite GPIOB   (pin 6) to Arduino Digital 3

  (Capacitor recommended to mitigate inrush current when device is enabled)
  680uF capacitor (+) to Arduino 5V
  680uF capacitor (-) to Arduino GND

  See the Operation Manual for wiring diagrams and more information

------------------------------------------------------------------------------*/

#include <stdint.h>
#include <Wire.h>
#include "LIDARLite_v4LED.h"

LIDARLite_v4LED myLidarLite;

#define FAST_I2C

#define MonitorPin    3
#define TriggerPin    2


uint32_t old_timeMSec;//02/17/23 gfp - added to print out differential measurement time

//---------------------------------------------------------------------
void setup()
//---------------------------------------------------------------------
{
  // Initialize Arduino serial port (for display of ASCII output to PC)
  Serial.begin(115200);

  // Initialize Arduino I2C (for communication to LidarLite)
  Wire.begin();
#ifdef FAST_I2C
#if ARDUINO >= 157
  Wire.setClock(400000UL); // Set I2C frequency to 400kHz (for Arduino Due)
#else
  TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
#endif

  // ----------------------------------------------------------------------
  // The LIDAR-Lite v4 LED is strictly a 3.3V system. The Arduino Due is a
  // 3.3V system and is recommended for use with the LIDAR-Lite v4 LED.
  // Care MUST be taken if connecting to a 5V system such as the Arduino Uno.
  //
  // I2C is a two wire communications bus that requires a pull-up resistor
  // on each signal. In the Arduino microcontrollers the Wire.begin()
  // function (called above) turns on pull-up resistors that pull the I2C
  // signals up to the system voltage rail. The Arduino Uno is a 5V system
  // and using the Uno's internal pull-ups risks damage to the LLv4.
  //
  // The two digitalWrite() functions (below) turn off the micro's internal
  // pull-up resistors. This protects the LLv4 from damage via overvoltage
  // but requires external pullups to 3.3V for the I2C signals.
  //
  // External pull-ups are NOT present on the Arduino Uno and must be added
  // manually to the I2C signals. 3.3V is available on pin 2 of the 6pin
  // "POWER" connector and can be used for this purpose. See the Uno
  // schematic for details:
  // https://www.arduino.cc/en/uploads/Main/arduino-uno-schematic.pdf
  //
  // External 1.5k ohm pull-ups to 3.3V are already present on the
  // Arduino Due. If using the Due no further action is required
  // ----------------------------------------------------------------------
  digitalWrite(SCL, LOW);
  digitalWrite(SDA, LOW);

  // ----------------------------------------------------------------------
  // Optional GPIO pin assignments for measurement triggering & monitoring
  // ----------------------------------------------------------------------
  pinMode(MonitorPin, INPUT);
  pinMode(TriggerPin, OUTPUT);
  digitalWrite(TriggerPin, LOW);

  // ----------------------------------------------------------------------
  // Optionally configure the LidarLite parameters to lend itself to
  // various modes of operation by altering 'configure' input integer.
  // See LIDARLite_v4LED.cpp for details.
  // ----------------------------------------------------------------------
  myLidarLite.configure(0);

  // ----------------------------------------------------------------------
  // ** One method of increasing the measurement speed of the
  //    LIDAR-Lite v4 LED is to adjust the number of acquisitions taken
  //    per measurement from the default of 20. For max speed we
  //    set this value to 0x00 in this example.
  // ** Note that when reducing the number of acquisitions taken per
  //    measurement from the default, repeatability of measurements is
  //    reduced.
  // ----------------------------------------------------------------------
  uint8_t dataByte = 0x00;
  myLidarLite.write(0xEB, &dataByte, 1, 0x62); // Turn off high accuracy mode

  //02/17/23 gfp
  old_timeMSec = millis();
  Serial.printf("MSec\tDist\tDt\n");
}


//---------------------------------------------------------------------
void loop()
//---------------------------------------------------------------------
{
  // Using GPIO triggering can increase the rep rate.
  // Remove comments from the line below to use I2C triggering.
   #define USE_I2C_TRIGGERING

  uint16_t distance;
  uint8_t  newDistance;
  //bool  newDistance; //02/17/23 gfp - used like a boolean, so make it one!

#ifdef USE_I2C_TRIGGERING
  newDistance = distanceContinuous(&distance);
#else
  newDistance = distanceContinuousGpio(&distance);
#endif

  if (newDistance)
  {
    uint32_t curtimeMsec = millis();
    //Serial.println(distance); // print measurement to serial terminal
    Serial.printf("%lu\t%d\t%d\n", curtimeMsec, distance, curtimeMsec -old_timeMSec); // print measurement to serial terminal
    old_timeMSec = curtimeMsec;
  }
}

//---------------------------------------------------------------------
// Read Continuous Distance Measurements
//
// The most recent distance measurement can always be read from
// device registers. Polling for the BUSY flag in the STATUS
// register can alert the user that the distance measurement is new
// and that the next measurement can be initiated. If the device is
// BUSY this function does nothing and returns 0. If the device is
// NOT BUSY this function triggers the next measurement, reads the
// distance data from the previous measurement, and returns 1.
//---------------------------------------------------------------------
uint8_t distanceContinuous(uint16_t* distance)
{
  uint8_t newDistance = 0;

  // Check on busyFlag to indicate if device is idle
  // (meaning = it finished the previously triggered measurement)
  if (myLidarLite.getBusyFlag() == 0)
  {
    // Trigger the next range measurement
    myLidarLite.takeRange();

    // Read new distance data from device registers
    *distance = myLidarLite.readDistance();

    // Report to calling function that we have new data
    newDistance = 1;
  }

  return newDistance;
}

//---------------------------------------------------------------------
// Read Continuous Distance Measurements using Trigger / Monitor Pins
//
// The most recent distance measurement can always be read from
// device registers. Polling for the BUSY flag using the Monitor Pin
// can alert the user that the distance measurement is new
// and that the next measurement can be initiated. If the device is
// BUSY this function does nothing and returns 0. If the device is
// NOT BUSY this function triggers the next measurement, reads the
// distance data from the previous measurement, and returns 1.
//---------------------------------------------------------------------
uint8_t distanceContinuousGpio(uint16_t* distance)
{
  uint8_t newDistance = 0;

  // Check on busyFlag to indicate if device is idle
  // (meaning = it finished the previously triggered measurement)
  if (myLidarLite.getBusyFlagGpio(MonitorPin) == 0)
  {
    // Trigger the next range measurement
    myLidarLite.takeRangeGpio(TriggerPin, MonitorPin);

    // Read new distance data from device registers
    *distance = myLidarLite.readDistance();

    // Report to calling function that we have new data
    newDistance = 1;
  }

  return newDistance;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

Name: Garmin_LIDARV4_Study_V2.ino

Created: 2/17/2023 4:34:38 PM

Author: FRANKNEWXPS15\Frank

02/17/23 This version uses the 'v4LED/v4LED_fast' example program from the Garmin library

/*------------------------------------------------------------------------------

LIDARLite Arduino Library

v4LED/v4LED_fast

This example shows how to take multiple distance measurements with a

LIDAR-Lite v4 LED with a high rep rate using the optional GPIO pins

or the I2C port to trigger measurements.

*** NOTE ***

The LIDAR-Lite v4 LED is strictly a 3.3V system. The Arduino Due is a

3.3V system and is recommended for use with the LIDAR-Lite v4 LED.

Care MUST be taken if connecting to a 5V system such as the Arduino Uno.

See comment block in the setup() function for details on I2C connections.

It is recommended to use a voltage level-shifter if connecting the GPIO

pins to any 5V system I/O.

Connections:

LIDAR-Lite 5 VDC (pin 1) to Arduino 5V

LIDAR-Lite Ground (pin 2) to Arduino GND

LIDAR-Lite I2C SDA (pin 3) to Arduino SDA

LIDAR-Lite I2C SCL (pin 4) to Arduino SCL

Optional connections to utilize GPIO triggering:

LIDAR-Lite GPIOA (pin 5) to Arduino Digital 2

LIDAR-Lite GPIOB (pin 6) to Arduino Digital 3

(Capacitor recommended to mitigate inrush current when device is enabled)

680uF capacitor (+) to Arduino 5V

680uF capacitor (-) to Arduino GND

See the Operation Manual for wiring diagrams and more information

------------------------------------------------------------------------------*/

#include <stdint.h>

#include <Wire.h>

#include "LIDARLite_v4LED.h"

LIDARLite_v4LED myLidarLite;

#define FAST_I2C

#define MonitorPin 3

#define TriggerPin 2

uint32_t old_timeMSec;//02/17/23 gfp - added to print out differential measurement time

//---------------------------------------------------------------------

void setup()

//---------------------------------------------------------------------

{

// Initialize Arduino serial port (for display of ASCII output to PC)

Serial.begin(115200);

// Initialize Arduino I2C (for communication to LidarLite)

Wire.begin();

#ifdef FAST_I2C

#if ARDUINO >= 157

Wire.setClock(400000UL); // Set I2C frequency to 400kHz (for Arduino Due)

#else

TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz

#endif

// ----------------------------------------------------------------------

// The LIDAR-Lite v4 LED is strictly a 3.3V system. The Arduino Due is a

// 3.3V system and is recommended for use with the LIDAR-Lite v4 LED.

// Care MUST be taken if connecting to a 5V system such as the Arduino Uno.

// I2C is a two wire communications bus that requires a pull-up resistor

// on each signal. In the Arduino microcontrollers the Wire.begin()

// function (called above) turns on pull-up resistors that pull the I2C

// signals up to the system voltage rail. The Arduino Uno is a 5V system

// and using the Uno's internal pull-ups risks damage to the LLv4.

// The two digitalWrite() functions (below) turn off the micro's internal

// pull-up resistors. This protects the LLv4 from damage via overvoltage

// but requires external pullups to 3.3V for the I2C signals.

// External pull-ups are NOT present on the Arduino Uno and must be added

// manually to the I2C signals. 3.3V is available on pin 2 of the 6pin

// "POWER" connector and can be used for this purpose. See the Uno

// schematic for details:

// https://www.arduino.cc/en/uploads/Main/arduino-uno-schematic.pdf

// External 1.5k ohm pull-ups to 3.3V are already present on the

// Arduino Due. If using the Due no further action is required

// ----------------------------------------------------------------------

digitalWrite(SCL, LOW);

digitalWrite(SDA, LOW);

// ----------------------------------------------------------------------

// Optional GPIO pin assignments for measurement triggering & monitoring

// ----------------------------------------------------------------------

pinMode(MonitorPin, INPUT);

pinMode(TriggerPin, OUTPUT);

digitalWrite(TriggerPin, LOW);

// ----------------------------------------------------------------------

// Optionally configure the LidarLite parameters to lend itself to

// various modes of operation by altering 'configure' input integer.

// See LIDARLite_v4LED.cpp for details.

// ----------------------------------------------------------------------

myLidarLite.configure(0);

// ----------------------------------------------------------------------

// ** One method of increasing the measurement speed of the

// LIDAR-Lite v4 LED is to adjust the number of acquisitions taken

// per measurement from the default of 20. For max speed we

// set this value to 0x00 in this example.

// ** Note that when reducing the number of acquisitions taken per

// measurement from the default, repeatability of measurements is

// reduced.

// ----------------------------------------------------------------------

uint8_t dataByte = 0x00;

myLidarLite.write(0xEB, &dataByte, 1, 0x62); // Turn off high accuracy mode

//02/17/23 gfp

old_timeMSec = millis();

Serial.printf("MSec\tDist\tDt\n");

}

//---------------------------------------------------------------------

void loop()

//---------------------------------------------------------------------

{

// Using GPIO triggering can increase the rep rate.

// Remove comments from the line below to use I2C triggering.

#define USE_I2C_TRIGGERING

uint16_t distance;

uint8_t newDistance;

//bool newDistance; //02/17/23 gfp - used like a boolean, so make it one!

#ifdef USE_I2C_TRIGGERING

newDistance = distanceContinuous(&distance);

#else

newDistance = distanceContinuousGpio(&distance);

#endif

if (newDistance)

{

uint32_t curtimeMsec = millis();

//Serial.println(distance); // print measurement to serial terminal

Serial.printf("%lu\t%d\t%d\n", curtimeMsec, distance, curtimeMsec -old_timeMSec); // print measurement to serial terminal

old_timeMSec = curtimeMsec;

}

//---------------------------------------------------------------------

// Read Continuous Distance Measurements

// The most recent distance measurement can always be read from

// device registers. Polling for the BUSY flag in the STATUS

// register can alert the user that the distance measurement is new

// and that the next measurement can be initiated. If the device is

// BUSY this function does nothing and returns 0. If the device is

// NOT BUSY this function triggers the next measurement, reads the

// distance data from the previous measurement, and returns 1.

//---------------------------------------------------------------------

uint8_t distanceContinuous(uint16_t* distance)

{

uint8_t newDistance = 0;

// Check on busyFlag to indicate if device is idle

// (meaning = it finished the previously triggered measurement)

if (myLidarLite.getBusyFlag() == 0)

{

// Trigger the next range measurement

myLidarLite.takeRange();

// Read new distance data from device registers

*distance = myLidarLite.readDistance();

// Report to calling function that we have new data

newDistance = 1;

}

return newDistance;

}

//---------------------------------------------------------------------

// Read Continuous Distance Measurements using Trigger / Monitor Pins

// The most recent distance measurement can always be read from

// device registers. Polling for the BUSY flag using the Monitor Pin

// can alert the user that the distance measurement is new

// and that the next measurement can be initiated. If the device is

// BUSY this function does nothing and returns 0. If the device is

// NOT BUSY this function triggers the next measurement, reads the

// distance data from the previous measurement, and returns 1.

//---------------------------------------------------------------------

uint8_t distanceContinuousGpio(uint16_t* distance)

{

uint8_t newDistance = 0;

// Check on busyFlag to indicate if device is idle

// (meaning = it finished the previously triggered measurement)

if (myLidarLite.getBusyFlagGpio(MonitorPin) == 0)

{

// Trigger the next range measurement

myLidarLite.takeRangeGpio(TriggerPin, MonitorPin);

// Read new distance data from device registers

*distance = myLidarLite.readDistance();

// Report to calling function that we have new data

newDistance = 1;

}

return newDistance;

}

NOTE: the ‘fast’ example sets the number of acquisitions per measurement from the default value of 20 to 0, as shown in the following code snippet from the bottom of setup():

  // ----------------------------------------------------------------------
  // ** One method of increasing the measurement speed of the
  //    LIDAR-Lite v4 LED is to adjust the number of acquisitions taken
  //    per measurement from the default of 20. For max speed we
  //    set this value to 0x00 in this example.
  // ** Note that when reducing the number of acquisitions taken per
  //    measurement from the default, repeatability of measurements is
  //    reduced.
  // ----------------------------------------------------------------------
  uint8_t dataByte = 0x00;
  myLidarLite.write(0xEB, &dataByte, 1, 0x62); // Turn off high accuracy mode

// ----------------------------------------------------------------------

// ** One method of increasing the measurement speed of the

// LIDAR-Lite v4 LED is to adjust the number of acquisitions taken

// per measurement from the default of 20. For max speed we

// set this value to 0x00 in this example.

// ** Note that when reducing the number of acquisitions taken per

// measurement from the default, repeatability of measurements is

// reduced.

// ----------------------------------------------------------------------

uint8_t dataByte = 0x00;

myLidarLite.write(0xEB, &dataByte, 1, 0x62); // Turn off high accuracy mode

Here’s an Excel plot of the output:

As the above plot shows, this example produces measurements much faster than the first one. Instead of 350mSec at 450cm is approximately, it is about 32Msec – a 10:1 ratio! And instead of approximately 70mSec at a distance of 20cm, it is more like 2Msec – again a 10:1 ratio.

To address the issue of accuracy and repeatability, I used a tape measure to set up a target 100cm away as shown in the following photo, and then moved the LIDAR in 10cm increments toward the target, taking 10 measurements at each stop.

Accuracy & Repeatability Plot, with ‘high accuracy’ mode disabled

As the above plot shows, each measurement is repeatable within +/- 2cm. However, the accuracy seemed to deteriorate as the distance decreased. The error started off at about +1 to +2cm at 100cm, and then increased more or less linearly to +8 to +9cm at 10cm.

Next, I re-ran the experiment after commenting out the line myLidarLite.write(0xEB, &dataByte, 1, 0x62); // Turn off high accuracy mode that disabled the ‘high accuracy’ mode. However, the results weren’t any better, and actually look worse than before.

Accuracy & Repeatability Plot, with ‘high accuracy’ mode enabled

Just for completeness, I ran the experiment again, with the same configuration as the above plot, except with 100 measurements/position instead of 10. As the following plot shows, repeatability is excellent – accuracy ‘not so much’.

Accuracy & Repeatability Plot, with ‘high accuracy’ mode enabled, 100 meas/position

18 February 2023 Update:

As I was drifting off to sleep last night, I was thinking about what I had learned from that day’s experiments with the Garmin LIDAR V4/LED unit. Something that popped into my head was an image of the last set of data I took, but the inter-measurement delay was about 65-67mSec – not the 1-2mSec I had been seeing – what caused that?

So, today I redid the distance progression measurement, with the ‘high accuracy’ mode enabled as before (except starting at 150cm rather than 100), with the following results:

Accuracy & Repeatability Plot, with ‘high accuracy’ mode enabled, 10 meas/position

This time the time delay between measurements was about 67mSec – the same as I got with that last measurement from yesterday (the image that got stuck in my head as I was going to sleep). In addition, the distance accuracy seemed to be better – the error at 10cm was about +5cm rather than the +8 – +9cm from yesterday, while the error values above 100cm (1m) were negligible.

Redid the above experiment with the ‘high accuracy’ mode disabled (as it was in the original ‘Fast’ measurement code), with the LIDAR at the last position (10cm) from the previous run, with the following results:

MSec	Dist	Dt
7200	15	0
7204	19	4
7207	18	3
7211	20	4
7215	19	4
7218	19	3
7222	20	4
7226	19	4
7230	20	4
7233	19	3

MSec Dist Dt

7200 15 0

7204 19 4

7207 18 3

7211 20 4

7215 19 4

7218 19 3

7222 20 4

7226 19 4

7230 20 4

7233 19 3

Without changing anything else, and with the LIDAR in the same position, I re-compiled the code to re-enable ‘high accuracy’ mode, and got the following result

MSec	Dist	Dt
4400	19	0
4404	17	4
4407	16	3
4411	17	4
4415	18	4
4418	18	3
4422	19	4
4426	17	4
4429	18	3
4433	18	4

MSec Dist Dt

4400 19 0

4404 17 4

4407 16 3

4411 17 4

4415 18 4

4418 18 3

4422 19 4

4426 17 4

4429 18 3

4433 18 4

Wait a minute! The above results are the same as the ‘fast’ results – what gives? I ran this same test several times, recompiling and re-uploading the code each time – no change! Then I cycled power to the unit and tried again, with the following results:

22900	0	0
22968	15	68
23036	15	68
23103	15	67
23171	14	68
23239	14	68
23307	15	68
23374	14	67
23442	15	68
23510	15	68

22900 0 0

22968 15 68

23036 15 68

23103 15 67

23171 14 68

23239 14 68

23307 15 68

23374 14 67

23442 15 68

23510 15 68

Aha! Recycling the power did the trick – now the results show the lower error (+5 vs +9cm) and the higher delay between measurements (67-68 vs 2 – 3Msec). So the last two plots from yesterday were performed in the ‘fast’ (low accuracy) mode, even after changing (and re-uploading) the Teensy sketch. Now that I think about it – this makes sense, as the change I made from the ‘fast’ configuration to the ‘high accuracy’ one was to comment out the write to register 0XEB. Because I didn’t cycle power to the Garmin, this left register 0XEB’s contents unchanged – e.g. still in ‘fast’ mode! Oops!!!

This is not the first time (by a LOT!!) that my pre-sleep review of the day’s efforts has paid off. Maybe the lack of outside stimulus allows my brain to sift through conflicting or incomplete data without interference. I wonder if this is one of those hidden talents that comes with “The Knack” 🙂

Anyway, now that I have figured out how to get into (and out of) ‘high accuracy’ mode, I re-ran the above experiment (with the unit in ‘high accuracy’ mode) where the LIDAR is aimed at various distances, with the following results:

Distances and measurement times for various distances in ‘High Accuracy’ mode

Distances and measurement times for various distances in ‘Low Accuracy’ mode

From the above it is clear that the ‘high accuracy’ mode is quite expensive in terms of measurement frequency. In ‘high accuracy’ mode measurements can take 500mSec or more for long ranges, whereas in ‘low accuracy’ mode the same measurement takes less than 50mSec even in the worst case.

Next I tried some different values for the number of measurements/point written to Garmin register 0xEB. With values of 10 (0x0A) and 5 (0x05), I got the following responses for medium to long distances:

Distances and measurement times for various distances using 10 acq/meas

Distances and measurement times for various distances using 5 acq/meas

With 5 acquisitions/measurement the measurement time is still well less than 200mSec, and I currently use FRONT_DISTANCE_UPDATE_INTERVAL_MSEC = 250, so this should be compatible with current operations, even at the longest ranges. I’m not really sure why I care all that much about accuracy, as all I’m trying to do is detect either the ‘STUCK_AHEAD’ condition (when the calculated measurement variance goes below a threshold) and the ‘OBSTACLE_AHEAD’ condition (where the measurement falls below a set distance). Neither of these cases requires much in the way of absolute accuracy.

19 February 2023 Update:

I decided to run Garmin’s ‘Low Power’ example, modified for better printout with the Teensy 3.5 as shown below:

/*
    Name:       Garmin_LIDARV4_LOW_POWER.ino
    Created:	2/19/2023 12:52:32 PM
    Author:     FRANKNEWXPS15\Frank
*/
/*------------------------------------------------------------------------------

  LIDARLite Arduino Library
  v4LED/v4LED_lowpower

  This example shows how to take single distance measurements with a
  LIDAR-Lite v4 LED using only the I2C port in a lower power state.

  *** NOTE ***
  The LIDAR-Lite v4 LED is strictly a 3.3V system. The Arduino Due is a
  3.3V system and is recommended for use with the LIDAR-Lite v4 LED.
  Care MUST be taken if connecting to a 5V system such as the Arduino Uno.
  See comment block in the setup() function for details on I2C connections.
  It is recommended to use a voltage level-shifter if connecting the GPIO
  pins to any 5V system I/O.

  Connections:
  LIDAR-Lite 5 VDC   (pin 1) to Arduino 5V
  LIDAR-Lite Ground  (pin 2) to Arduino GND
  LIDAR-Lite I2C SDA (pin 3) to Arduino SDA
  LIDAR-Lite I2C SCL (pin 4) to Arduino SCL

  Optional connections to utilize GPIO triggering:
  LIDAR-Lite GPIOA   (pin 5) to Arduino Digital 2
  LIDAR-Lite GPIOB   (pin 6) to Arduino Digital 3

  (Capacitor recommended to mitigate inrush current when device is enabled)
  680uF capacitor (+) to Arduino 5V
  680uF capacitor (-) to Arduino GND

  See the Operation Manual for wiring diagrams and more information

------------------------------------------------------------------------------*/

#include <stdint.h>
#include <Wire.h>
#include "LIDARLite_v4LED.h"

LIDARLite_v4LED myLidarLite;

#define FAST_I2C

#define MonitorPin    3
#define TriggerPin    2

//---------------------------------------------------------------------
void setup()
//---------------------------------------------------------------------
{
  // Initialize Arduino serial port (for display of ASCII output to PC)
  Serial.begin(115200);

  // Initialize Arduino I2C (for communication to LidarLite)
  Wire.begin();
#ifdef FAST_I2C
#if ARDUINO >= 157
  Wire.setClock(400000UL); // Set I2C frequency to 400kHz (for Arduino Due)
#else
  TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
#endif
#endif

  // ----------------------------------------------------------------------
  // The LIDAR-Lite v4 LED is strictly a 3.3V system. The Arduino Due is a
  // 3.3V system and is recommended for use with the LIDAR-Lite v4 LED.
  // Care MUST be taken if connecting to a 5V system such as the Arduino Uno.
  //
  // I2C is a two wire communications bus that requires a pull-up resistor
  // on each signal. In the Arduino microcontrollers the Wire.begin()
  // function (called above) turns on pull-up resistors that pull the I2C
  // signals up to the system voltage rail. The Arduino Uno is a 5V system
  // and using the Uno's internal pull-ups risks damage to the LLv4.
  //
  // The two digitalWrite() functions (below) turn off the micro's internal
  // pull-up resistors. This protects the LLv4 from damage via overvoltage
  // but requires external pullups to 3.3V for the I2C signals.
  //
  // External pull-ups are NOT present on the Arduino Uno and must be added
  // manually to the I2C signals. 3.3V is available on pin 2 of the 6pin
  // "POWER" connector and can be used for this purpose. See the Uno
  // schematic for details:
  // https://www.arduino.cc/en/uploads/Main/arduino-uno-schematic.pdf
  //
  // External 1.5k ohm pull-ups to 3.3V are already present on the
  // Arduino Due. If using the Due no further action is required
  // ----------------------------------------------------------------------
  digitalWrite(SCL, LOW);
  digitalWrite(SDA, LOW);

  // ----------------------------------------------------------------------
  // Optional GPIO pin assignments for measurement triggering & monitoring
  // ----------------------------------------------------------------------
  pinMode(MonitorPin, INPUT);
  pinMode(TriggerPin, OUTPUT);
  digitalWrite(TriggerPin, LOW);

  // ----------------------------------------------------------------------
  // Optionally configure the LidarLite parameters to lend itself to
  // various modes of operation by altering 'configure' input integer.
  // See LIDARLite_v4LED.cpp for details.
  // ----------------------------------------------------------------------
  myLidarLite.configure(0);

  // ----------------------------------------------------------------------
  // ** To utilize the low power capabilities of the LIDAR-Lite v4 LED,
  //    turn off high accuracy mode and then switch on asynchronous mode.
  // ** Note that when defeating high accuracy mode, repeatability
  //    of measurements is reduced.
  // ** Note that while in asynchronous mode, minimum time between
  //    measurements is 100 milliseconds.
  // ----------------------------------------------------------------------
  uint8_t dataByte = 0x00;
  myLidarLite.write(0xEB, &dataByte, 1, 0x62); // Turn off high accuracy mode
  myLidarLite.write(0xE2, &dataByte, 1, 0x62); // Turn on asynchronous mode

  Serial.printf("Msec\tDistCm\n");  //02/19/23 gfp

}


//---------------------------------------------------------------------
void loop()
//---------------------------------------------------------------------
{
  uint16_t distance;

  distanceSingle(&distance);   // take a single distance measurement

  Serial.printf("%lu\t%d\n", millis(),distance);  //02/19/23 gfp

  //Serial.println(distance);    // print measurement to serial terminal
  //delay(250);                  // wait around for a bit
  //delay(150);                  // wait around for a bit
  delay(100);                  // wait around for a bit
}

//---------------------------------------------------------------------
// Read Single Distance Measurement
//
// This is the simplest form of taking a measurement. This is a
// blocking function as it will not return until a range has been
// taken and a new distance measurement can be read.
//---------------------------------------------------------------------
uint8_t distanceSingle(uint16_t* distance)
{
  // 1. Trigger range measurement.
  myLidarLite.takeRange();

  // 2. Wait for busyFlag to indicate device is idle.
  myLidarLite.waitForBusy();

  // 3. Read new distance data from device registers
  *distance = myLidarLite.readDistance();

  return 1;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

Name: Garmin_LIDARV4_LOW_POWER.ino

Created: 2/19/2023 12:52:32 PM

Author: FRANKNEWXPS15\Frank

/*------------------------------------------------------------------------------

LIDARLite Arduino Library

v4LED/v4LED_lowpower

This example shows how to take single distance measurements with a

LIDAR-Lite v4 LED using only the I2C port in a lower power state.