Lidar Sensors not working properly - How to work with two lidar Sensors over I2C on arduino - c++

Question

I'm currently working on a project with some friends about lidar measuraments based on ARDUINO and GARMIN Lidar v3HP and we are getting some reading that are questionable from the sensors. They seem to work but the measurements are not correct.
We have issues with the data and also with the address, we setup the sensors with two different addresses 0x42 and 0x43, but one of the sensors keeps on the default address.
#include <Arduino.h>
#include <Wire.h>
#include <stdint.h>
#include <LIDARLite_v3HP.h>
#include <I2CFunctions.h>
#define POWER_ENABLE_S1 12
#define POWER_ENABLE_S2 11
#define DEFAULT_ADDRESS 98
#define FAST_I2C
#define NUMERO_LIDARS 2
LIDARLite_v3HP Sensor1;
LIDARLite_v3HP Sensor2;
int detectedAddreses[NUMERO_LIDARS];
int currentAdd;
int deviceCount = 0;
int i = 0;
void scanI2C()
{
int nDevices = 0;
while (i < NUMERO_LIDARS)
{
for (byte addr = 1; addr < 127; ++addr)
{
Wire.beginTransmission(addr);
byte error = Wire.endTransmission();
if (error == 0)
{
Serial.print("Se encontro un dispositivo en ");
Serial.print(addr);
Serial.println(" !");
++nDevices;
detectedAddreses[i] = addr;
if (addr == DEFAULT_ADDRESS)
{
configSensors(i, 66 + deviceCount, addr);
detectedAddreses[i] = addr;
i++;
}else{
detectedAddreses[i] = addr;
i++;
}
}
else if (error == 4)
{
Serial.print("Error desconocido en ");
Serial.println(addr);
}
}
if (nDevices == 0)
{
Serial.println("No se encontraron dispositivos\n");
}
else
{
Serial.println("Terminado\n");
}
}
}
void configSensors(int sensor, uint8_t new_address, uint8_t old_address)
{
switch (sensor)
{
case 0:
Sensor1.setI2Caddr(new_address, 0, old_address);
digitalWrite(POWER_ENABLE_S1, LOW);
//detectedAddreses[sensor] = new_address;
deviceCount++;
Sensor1.configure(0,new_address);
break;
case 1:
Sensor2.setI2Caddr(new_address, 0, old_address);
digitalWrite(POWER_ENABLE_S2, LOW);
//detectedAddreses[sensor] = new_address;
deviceCount++;
Sensor2.configure(0,new_address);
i = 999;
break;
case 2:
break;
}
}
void setup()
{
Serial.begin(115200);
#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
pinMode(POWER_ENABLE_S1, OUTPUT);
pinMode(POWER_ENABLE_S2, OUTPUT);
digitalWrite(POWER_ENABLE_S1, HIGH);
digitalWrite(POWER_ENABLE_S2, HIGH);
Wire.begin();
scanI2C();
digitalWrite(POWER_ENABLE_S1,HIGH);
digitalWrite(POWER_ENABLE_S2,HIGH);
Sensor1.configure(3,detectedAddreses[0]);
Sensor2.configure(3,detectedAddreses[1]);
}
void measure(){
float s1;
float s2;
Sensor1.waitForBusy();
Sensor1.takeRange();
Sensor1.waitForBusy();
s1 = Sensor1.readDistance(detectedAddreses[0]);
Sensor2.waitForBusy();
Sensor2.takeRange();
Sensor2.waitForBusy();
s2 = Sensor2.readDistance(detectedAddreses[1]);
Serial.println("Sensor 1: " + String(s1) + "; Sensor 2: " + String(s2));
}
void loop()
{
/*Serial.println(detectedAddreses[0]);
Serial.println(detectedAddreses[1]);*/
measure();
}

Answer 1

Based on your top comment, there may be an issue with configuring both lidars at the same time.
From factory default, they will both respond to the default I2C address 0x62. So, when you try to reconfigure one at a time, they will both respond [and there may be a race condition] and will both get programmed to the new I2C address.
If [and this is a big if] the lidar can save the configuration to non-volatile storage on the unit, you can connect one at a time [physically/manually] and give them different addresses. The unit saves the address. And, next time, will only respond to the "new" address.
Then, after both units have been reconfigured, you can then connect both simultaneously and they will respond individually [as desired].
I looked at the .pdf and the wiring diagram. You may be able to connect the lidar's power pin [or enable pin] to an Arduino GPIO port pin (instead of +5V). Then, you can control the power up of each unit individually. Then, you can reconfigure both as above. That is, assert power to one, reconfigure it, power it down [with the saved config]. Do this for the other unit. Then, you can power up both units [at this point, they are responding to different I2C addresses].
Don't know if Garmin starts up the lasers immediately or whether you have to give it a "start" command. Being able to control power individually may be a good thing if there is no separate start command.
I'm not familiar with Garmin's lidars, but I've written S/W to control Velodyne lidars and we had to apply power in a staggered manner because the power surge when they both started up would "brown out" the system. With Garmin, YMMV.
If all else fails, you may have to put each unit on a separate/different physical I2C bus [because you can't reconfigure them separately].

Answer 2

Here's the working code,
The sensors are hocked up in the same I2C bus, power enable pins to each sensor and ground conected to arduino. Power to the sensors is supplied by a 11.1V battery with a power regulator to 5V
#include <Arduino.h>
#include <Wire.h>
#include <stdint.h>
#include <LIDARLite_v3HP.h>
#include <I2CFunctions.h>
#define POWER_ENABLE_S1 12
#define POWER_ENABLE_S2 11
#define DEFAULT_ADDRESS 98
#define FAST_I2C
#define NUMERO_LIDARS 2
LIDARLite_v3HP Sensor1;
LIDARLite_v3HP Sensor2;
int detectedAddreses[NUMERO_LIDARS];
int currentAdd;
int deviceCount = 0;
int i = 0;
void scanI2C()
{
int nDevices = 0;
while (i < NUMERO_LIDARS)
{
for (byte addr = 1; addr < 127; ++addr)
{
Wire.beginTransmission(addr);
byte error = Wire.endTransmission();
if (error == 0)
{
Serial.print("Se encontro un dispositivo en ");
Serial.print(addr);
Serial.println(" !");
++nDevices;
detectedAddreses[i] = addr;
if (addr == DEFAULT_ADDRESS)
{
configSensors(i, 66 + deviceCount, addr);
detectedAddreses[i] = addr;
i++;
}else{
detectedAddreses[i] = addr;
i++;
}
}
else if (error == 4)
{
Serial.print("Error desconocido en ");
Serial.println(addr);
}
}
if (nDevices == 0)
{
Serial.println("No se encontraron dispositivos\n");
}
else
{
Serial.println("Terminado\n");
}
}
}
void configSensors(int sensor, uint8_t new_address, uint8_t old_address)
{
switch (sensor)
{
case 0:
Sensor1.setI2Caddr(new_address, 0, old_address);
digitalWrite(POWER_ENABLE_S1, LOW);
//detectedAddreses[sensor] = new_address;
deviceCount++;
Sensor1.configure(0,new_address);
break;
case 1:
Sensor2.setI2Caddr(new_address, 0, old_address);
digitalWrite(POWER_ENABLE_S2, LOW);
//detectedAddreses[sensor] = new_address;
deviceCount++;
Sensor2.configure(0,new_address);
i = 999;
break;
case 2:
break;
}
}
void setup()
{
Serial.begin(115200);
#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
pinMode(POWER_ENABLE_S1, OUTPUT);
pinMode(POWER_ENABLE_S2, OUTPUT);
digitalWrite(POWER_ENABLE_S1, HIGH);
digitalWrite(POWER_ENABLE_S2, HIGH);
Wire.begin();
scanI2C();
digitalWrite(POWER_ENABLE_S1,HIGH);
digitalWrite(POWER_ENABLE_S2,HIGH);
Sensor1.configure(3,detectedAddreses[0]);
Sensor2.configure(3,detectedAddreses[1]);
}
void measure(){
float s1;
float s2;
digitalWrite(POWER_ENABLE_S1,HIGH);
digitalWrite(POWER_ENABLE_S2,LOW);
delay(25);
Sensor1.waitForBusy();
Sensor1.takeRange();
Sensor1.waitForBusy();
s1 = Sensor1.readDistance(detectedAddreses[0]);
digitalWrite(POWER_ENABLE_S1,LOW);
digitalWrite(POWER_ENABLE_S2,HIGH);
delay(25);
Sensor2.waitForBusy();
Sensor2.takeRange();
Sensor2.waitForBusy();
s2 = Sensor2.readDistance(detectedAddreses[1]);
Serial.println("Sensor 1: " + String(s1) + "; Sensor 2: " + String(s2));
}
void loop()
{
/*Serial.println(detectedAddreses[0]);
Serial.println(detectedAddreses[1]);*/
measure();
}