Translating Lidar Lite I2C example to Teensy

Status
Not open for further replies.
Tested yesterday, got it working with this, see the comments at start


/*
http://pulsedlight3d.com
This sketch demonstrates getting distance with the LIDAR-Lite Sensor
It utilizes the 'Arduino Wire Library'

Modified to Teensy 3.1 by Kim Janson, www.levitezer.com
Sensor connected to pins 18 (SDA) and 19 (SCL), power to GND and Vin and there is a 4.7 uF capasitor, not sure if needed.
No other resistors etc. Powered from USB, based on half hour testing works ok.
Sorry for messy code.
*/
#include <i2c_t3.h>
//#include <Wire.h>
#define LIDARLite_ADDRESS 0x62 // Default I2C Address of LIDAR-Lite.
#define RegisterMeasure 0x00 // Register to write to initiate ranging.
#define MeasureValue 0x04 // Value to initiate ranging.
#define RegisterHighLowB 0x8f // Register to get both High and Low bytes in 1 call.

byte MeasureArray[2];
int reading = 0;
int reading2 = 0;
long time;
uint8_t nackack = 100;
void setup()
{
Wire.begin(I2C_MASTER,0x00 ,I2C_PINS_18_19, I2C_PULLUP_INT, I2C_RATE_100);
Wire.setOpMode(I2C_OP_MODE_ISR);
Serial.begin(115200); // start serial communication at 9600bps
MeasureArray[0]=0x00; // Register to write to initiate ranging.
MeasureArray[1]=0x04; // Value to initiate ranging.
}

void loop()
{

nackack = 0; //initiate ranging.
while ( nackack == 0){
Wire.beginTransmission((int)LIDARLite_ADDRESS); // transmit to LIDAR-Lite
nackack = Wire.write( MeasureArray,2); if (nackack == 0){delay(1);} }

nackack = 100; //end transmission
while ( nackack != 0){ nackack = Wire.endTransmission(); if (nackack != 0){delay(1);} }

delay(15);
/*http://kb.pulsedlight3d.com/support/solutions/articles/5000548624-quick-start-guide
"Periodically poll the unit and wait until an ACK is received. The unit responds to read
or write requests with a NACK when the sensor is busy processing a command or performing a
measurement. (Optionally, wait approx. 20 milliseconds after acquisition and then proceed
to read high and low bytes)"
15 ms gives about 17 to 18 ms reads. Would like to use polling, but how?*/

nackack = 0;
while ( nackack == 0){ //if error write again
Wire.beginTransmission((int)LIDARLite_ADDRESS); // transmit to LIDAR-Lite
nackack = Wire.write((int)RegisterHighLowB); // sets register pointer to (0x8f)
if (nackack == 0){delay(1);} }

nackack = 100;
while ( nackack != 0){ // if not sucses (0) do a gain
nackack = Wire.endTransmission(); // stop transmitting
if (nackack != 0){delay(1);}
}
// delay(20); // Wait 20ms for transmit
nackack = 0;
while ( nackack == 0){ nackack = Wire.requestFrom((int)LIDARLite_ADDRESS, 2); if (nackack == 0){delay(1);}} // request 2 bytes from LIDAR-Lite

delay(1);
if(2 == Wire.available()) // if two bytes were received
{
delay(1);

reading = -1;
while ( reading == -1){ reading = Wire.read(); if (reading == -1){delay(1);}}
reading = reading << 8; // shift high byte to be high 8 bits

reading2 = -1;
while ( reading2 == -1){ reading2 = Wire.read(); if (reading2 == -1){delay(1);}}
reading |= reading2; // receive low byte as lower 8 bits
// Serial.println(reading); // print the reading

// Print update rate and distance
Serial.print(millis()-time);
Serial.print(" ");
Serial.println(reading);
time=millis();
}
}
 
Garug, I ran your code and it eventually froze as well. Have you run it for more then a few minutes?

A few comments on it, you normally don't need to validate the return value of Wire.write() operations, they aren't actually writing data, just loading it into a transmit buffer. It would only return an error if it was filled.

See the code on post #21 in earlier on in this chain, its very similar to what you have here, but a bit stripped down.
 
For me it is running for long times, no problem. If the delay is sorter, it eventually froze for me, especially when doing fast changes on the distance, like waving hand in front of the sensor. 15 ms delay is ok for me, maybe if you try 20 ms? I really would like to remove the delay, but did not figure jet how to poll the sensor without frozen it.

The 4.7 uF capacitor was added when the code was not yet working, and I am using short and strong USB cable. After I got the code working I did not try removing the capacitor. Maybe it makes a difference? No other external components are used in addition of the LIDAR. I am using the latest i2c_t3.h.

Thanks for the comment on Wire.write(), this was the first time I actually programmed with any I2C library...
 
For me it is running for long times, no problem. If the delay is sorter, it eventually froze for me, especially when doing fast changes on the distance, like waving hand in front of the sensor. 15 ms delay is ok for me, maybe if you try 20 ms? I really would like to remove the delay, but did not figure jet how to poll the sensor without frozen it.

Ok, well this suggests there still is a problem beyond just my teensy. The arduino example polls every 1 ms and never freezes. For me, even at 20ms, it will freeze on the teensy, but it may last a long time. I suggest leaving it running for 30 minutes. Come back and if its still running, wave your hand in front of it. Within a few minutes it usually dies.
 
I noted when using short 12 to 14 ms delay, that some times it hang so badly that reseting Teensy did not help, USB cable (Power) had to be disconnected to recover. Maybe the hanging was because of LIDAR lite?

With 15 ms delay I have tested about 5 to 10 minutes and this was ok, will test longer periods later.

about "don't need to validate the return value of Wire.write() operations, they aren't actually writing data, just loading it into a transmit buffer. " is it then Wire.endTransmission(); that writes?

What command should be used to poll the NACK? "The unit responds to read
or write requests with a NACK when the sensor is busy processing a command or performing a
measurement."
 
Just started playing with a LIDAR-Lite device on the Teensy. Ran into the intermittent hang problem others are experiencing. Seems to occur with both the wire and i2c_t3 libraries. I focused on the old wire library and traced the problem down to a i2c_wait() call in the endTransmission() function. The i2c_wait() function waits for an interrupt flag to get set and will wait forever so if the flag doesn't get set, the Teensy locks up. I'm not going to debate whether the LIDAR or Teensy library is at fault for the hang but in my opinion, the Wire library should NEVER lock the Teensy up regardless of what occurs on the I2C bus except maybe a lightning strike :).

To get around the problem I made the following change to the Wire.cpp which basically limits the wait to about 30ms and then exits the endTransmission() function with an error 4 if the interrupt does not occur. Use at your own risk;

for (i=0; i < txBufferLength; i++) {
I2C0_D = txBuffer;

/*----------------------*/
/* Added Code Begin jdc */
/*----------------------*/
// i2c_wait(); // This call sometimes hangs waiting for interrupt flag
int _waitCnt = 1000000;
while (!(I2C0_S & I2C_S_IICIF))
{
if(--_waitCnt == 0)
{
break;
}
}
if(_waitCnt == 0)
{
ret = 4; // 4:eek:ther error
Serial.println("endTransmission() Timeout waiting for interrupt!"); // Should comment out this line after testing the fix
break;
}
/*--------------------*/
/* Added Code End jdc */
/*--------------------*/

I2C0_S = I2C_S_IICIF;
status = I2C0_S;
 
Last edited:
Interesting work around. Did the device start working on the next query after this (without restarting the teensy)? I used the timeout features of the newer I2C library to avoid a lockup, but the device never works afterwords (even if i push the "EN" input on the lidar down to reset the lidar) .
 
Interesting work around. Did the device start working on the next query after this (without restarting the teensy)? I used the timeout features of the newer I2C library to avoid a lockup, but the device never works afterwords (even if i push the "EN" input on the lidar down to reset the lidar) .

Yes, the device (program) resumes proper operation after a hang event, don't have to restart or reset anything. I am able to create the error by taping the LIDAR device on its side, sometimes takes several taps before the failure occurs. After the failure, I may get a couple of the timeout message displays I added to the library code but the device then settles down and continues to work without timeouts.... until I bang it around again ;).
 
Just got my lidar today. The code posted earlier in the thread works so far without needing the timeout patch. Will run overnight and see.
 
Lidar test code

This code from Garug: https://forum.pjrc.com/threads/2803...xample-to-Teensy?p=67951&viewfull=1#post67951

I modified it slightly to print out a new reading only when the change from the previous reading was more than 10 cm. Anyway, this code ran for 10 hours overnight without locking up, but it was just sitting still on my desk, I wasn't moving the sensor or slapping it around.
Code:
/* http://pulsedlight3d.com
Modified to Teensy 3.1 by Kim Janson, www.levitezer.com
Further modified by John Beale, www.bealecorner.com
See also: forum.pjrc.com/threads/28036-Translating-Lidar-Lite-I2C-example-to-Teensy

Sensor connected to pins 18 (SDA) and 19 (SCL), power to GND and Vin
and there is a 4.7 uF capasitor, not sure if needed.
No other resistors etc. Powered from USB, based on half hour testing works ok.  */

#include <i2c_t3.h> // from https://github.com/nox771/i2c_t3
#define LIDARLite_ADDRESS 0x62 // Default I2C Address of LIDAR-Lite.
#define RegisterMeasure 0x00 // Register to write to initiate ranging.
#define MeasureValue 0x04 // Value to initiate ranging.
#define RegisterHighLowB 0x8f // Register to get both High and Low bytes in 1 call.
#define DLIMIT 10  // how many cm of reading difference to notice

byte MeasureArray[2];
int reading = 0;
int reading2 = 0;
int dOld = 0;  // previous distance reading
int dNew = 0;  // new reading
unsigned long tcount = 0;
long time;
uint8_t nackack = 100;
const int LED1 = 13;         // onboard signal LED 

void setup()
{
  pinMode(LED1,OUTPUT);       // enable digital output for turning on LED indicator   
  digitalWrite(LED1,HIGH);   
  Wire.begin(I2C_MASTER,0x00 ,I2C_PINS_18_19, I2C_PULLUP_INT, I2C_RATE_100); 
  Wire.setOpMode(I2C_OP_MODE_ISR);
  Serial.begin(115200); // start serial communication at 9600bps
  MeasureArray[0]=0x00; // Register to write to initiate ranging.
  MeasureArray[1]=0x04; // Value to initiate ranging.
  delay(500);
  digitalWrite(LED1,LOW);   
}

void loop()
{
nackack = 0; //initiate ranging.
while ( nackack == 0) { 
  Wire.beginTransmission((int)LIDARLite_ADDRESS); // transmit to LIDAR-Lite
  nackack = Wire.write( MeasureArray,2); 
  if (nackack == 0)  { delay(1);} 
 } 

 nackack = 100; //end transmission
 while ( nackack != 0) { 
   nackack = Wire.endTransmission(); 
   if (nackack != 0) {delay(1);} 
 }

delay(15); 
/* http://kb.pulsedlight3d.com/support/...ck-start-guide
"Periodically poll the unit and wait until an ACK is received. The unit responds to read 
or write requests with a NACK when the sensor is busy processing a command or performing a 
measurement. (Optionally, wait approx. 20 milliseconds after acquisition and then proceed
to read high and low bytes)"
15 ms gives about 17 to 18 ms reads. Would like to use polling, but how?  */

 nackack = 0;
 while ( nackack == 0){ //if error write again
 Wire.beginTransmission((int)LIDARLite_ADDRESS); // transmit to LIDAR-Lite
 nackack = Wire.write((int)RegisterHighLowB); // sets register pointer to (0x8f)
 if (nackack == 0){delay(1);} }

 nackack = 100;
 while ( nackack != 0){ // if not success (0) do a gain
 nackack = Wire.endTransmission(); // stop transmitting
 if (nackack != 0){delay(1);}
 }
// delay(20); // Wait 20ms for transmit
 nackack = 0;
 while ( nackack == 0) { 
   nackack = Wire.requestFrom((int)LIDARLite_ADDRESS, 2); 
   if (nackack == 0) {delay(1);}
 } // request 2 bytes from LIDAR-Lite

delay(1);
if(2 == Wire.available()) // if two bytes were received
 {
  delay(1);
  reading = -1;
  while ( reading == -1) { 
    reading = Wire.read(); 
    if (reading == -1){delay(1);}
  }

  reading = reading << 8; // shift high byte to be high 8 bits
  reading2 = -1;
  while ( reading2 == -1)
    { 
     reading2 = Wire.read(); 
     if (reading2 == -1) {delay(1);}
    }
 
    reading |= reading2; // receive low byte as lower 8 bits

    tcount++;
    dNew = reading;
    // if new distance has changed much, print: distance, time, reading count
    if (abs(dNew - dOld) > DLIMIT) {
      digitalWrite(LED1,HIGH);   
      Serial.print(dNew);
      Serial.print(", ");
      Serial.print(millis()-time);
      Serial.print(", ");
      Serial.print(tcount);
      Serial.println();
      time=millis();
      dOld = reading;
      tcount=0;
      delay(50);  // let LED ON be more visible
      digitalWrite(LED1,LOW);   
   }
  }
}
 
Last edited:
Can you try replacing the delay(15) with delay(1)? this will basically make the program depend on polling the device for ready state and put a bit more activity over I2C. Even better, run the code below (you may likely need to replace Serial1.. with Serial if you are using serial terminal over usb.

Code:
#include <i2c_t3.h>
#define    LIDARLite_ADDRESS   0x62          // Default I2C Address of LIDAR-Lite.
#define    RegisterMeasure     0x00          // Register to write to initiate ranging.
#define    MeasureValue        0x04          // Value to initiate ranging.
#define    RegisterHighLowB    0x8f          // Register to get both High and Low bytes in 1 call.

elapsedMillis sincePrint;

void setup()
{
  //Wire.begin(); // join i2c bus
  Wire.begin(I2C_MASTER, 0, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_100, I2C_OP_MODE_DMA );
  Serial1.begin(115200); 
  pinMode(13,OUTPUT);
  digitalWrite(13, HIGH);
  delay(100);  // give the lidar time to startup
}


void loop(){
  int out ;
  Serial1.print(sincePrint);
  sincePrint = 0;

  Serial1.print(">");
  Serial1.println(out = readDistance());

  if (out == 0){
    Serial1.println("Getting sick");
  }

}

int readDistance(){
  uint8_t nackack = 100; // Setup variable to hold ACK/NACK resopnses     
  while (nackack != 0){ // While NACK keep going (i.e. continue polling until sucess message (ACK) is received )
   // Wait 1 ms to prevent overpolling
    Wire.beginTransmission(LIDARLite_ADDRESS);
    Wire.write(RegisterMeasure);
    Wire.write(MeasureValue);
    nackack = Wire.endTransmission(I2C_STOP,1000);
    if (nackack!=0){
        Serial1.print("A");
        Serial1.print(nackack);
        }
  }
  delay(1); // change to 18 for 20ms fixed reads
  nackack = 100;
  uint8_t trys = 0;
  while (nackack != 0){ 

    Wire.beginTransmission(LIDARLite_ADDRESS);
    Wire.write(RegisterHighLowB);
    nackack = Wire.endTransmission(I2C_STOP,1000);
    delay(1);
    if (nackack!=0){
      Serial1.print("C");
      Serial1.print(nackack);
          delay(1);
      continue;
    }
    Wire.requestFrom(LIDARLite_ADDRESS,2,I2C_STOP,1000);
  }

  int reading = Wire.readByte() ;
  reading = reading << 8;
  reading |= Wire.readByte();
  return reading;
 
}
 
Lidar testing

Not sure I understand what your code does, but here's a sample of its output:

Code:
288
3>288
3>288
3>288
2>288
3>C2C2C2C2289
12>289
2>289
3>289
3>289
3>289
3>289
3>289
2>C2C2C2C2290
12>290
3>290
2>290
3>290
3>290
3>290
3>C2C2C2C2290
11>290
3>290
3>290
2>290
3>290
3>290
3>290
3>290
3>290
2>C2C2C2C2290
12>290
3>290
2>290
3>290
3>290
3>290
3>290
3>290
2>C2C2C2C2289
12>289
3>289
2>289
3>C2C2C2C2290
12>290
2>290
3>290
3>290
3>290
3>290
2>290
3>290
3>C2C2C2C2289
11>C2C2C2C2289
12>289
3>289
2>289
3>289
3>289
3>289
3>C2C2C2C2289
11>289
3>289
3>289
3>289
2>C2C2C2C2289
12>289
3>289
2>289
3>289
3>289
3>289
3>289
3>C2C2C2C2290
11>290
3>290
2>290
3>290
3>290
3>290
3>290
3>C2C2C2C2290
11>290
3>290
3>C2C2C2C2290
11>290
3>290
3>290
2>290
3>290
3>290
3>290
3>C2C2C2C2290
11>290
3>290
3>290
3>290
2>290
3>C2C2C2C2289
11>289
3>289
3>289
3>289
3>289
3>289
2>289
3>289
3>C2C2C2C2290
11>290
3>290

UPDATE: this code worked fine for about an hour indoors with no "sick" messages. Outdoors is a different story, at longer ranges I see a lot of "getting sick..." message.

Code:
3>3455
3>3455
2>C2C2C2C2C2C23450
16>3450
3>3450
3>3450
2>3450
3>3450
3>C2C2C2C2C2C23467
16>3467
2>3467
3>3467
3>3467
3>3467
3>C2C2C2C2C2C20
Getting sick
15>0
Getting sick
3>0
Getting sick
3>0
Getting sick
 
Last edited:
Hmm, that is very good behavior. Its working as expected and your getting a very fast read rate, moving between 3ms per read and 12ms per read. If you swing your arm in front, you should see more slower reads and fewer 3ms reads. How long does it keep this up for???

Quick translation of what the codes are :

3>289 : took 3 ms last cycle and read a distance of 289

3>C2C2C2C2290 : took 3 ms last cycle, this cycle it paused a few times in the "C" step with a normal error code (2), which is the expected when we wait for ack. Finally read a distance of 290

12>290 : took 12ms last cycle (which makes sense because it took a few pauses to wait for ack on last measure. Finally read 290.
 
Last edited:
See my previous update re: outdoors; I guess the max range is about 35 meters? At least that's the largest reading I've seen.

Tried it from my front porch pointing across the street. It sees cars passing by at 26 meters and you can obviously tell which side of the street they are on, from the range. It can see people also to about 30 meters, however a person walking by on the near side sidewalk (range about 16 meters) wearing all black clothes just registered "0".
 
Last edited:
Cool, never tried it outside. If its sending 0 on purpose, then you can ignore the getting sick messages. Big thing is .. has it ever froze in a "C4C4C4...." loop? If not .. please share the magic :) How do you have it wired/powered/...
 
LIDAR wiring

No lockups. I have never had it freeze, in about 15 hours of running total so far (mostly with my code though, not yours). Nothing special on the setup: I'm using 4 of the 6 LIDAR wires: +5V, GND, SCL, SDA. Power supply is from laptop USB shared with Teensy 3.1, measured voltage is 5.06 V. I do have 0.1 uF ceramic + 10 uF tantalum between +5V and GND on the breadboard.

LIDAR-wiring.jpg
 
I2C data from LIDAR

One observation, with the expected I2C transaction happening at about 18 msec intervals, I notice a 5 microsecond low glitch on the SDA data line about halfway in between the normal data, while SCK clock stays high. I believe that corresponds to I2C START followed immediately by I2C STOP. Could that be the source of the occasional hangup problem? But in my case it doesn't seem to be a problem. But if that glitch happened during the main I2C transaction, that would be trouble.

Lidar-I2C-scope.jpg
 
Last edited:
Interesting, I suppose that will cause more problems if I have other devices on this I2C connection. Do you mind posting your code?
 
This is my code. It takes measurements continually but only prints out data when the value changes beyond some threshold from the previous printed value (eg. 10 cm). https://forum.pjrc.com/threads/2803...xample-to-Teensy?p=68472&viewfull=1#post68472
Total runtime over 24 hours so far, without any issues.

Looking at the projected laser spot from this device with an IR camera, it is pretty well defined so you are really sampling a small area on your target. I was wondering if this could be used to measure speed of cars passing when set at an angle looking in from the side of the road, but it might be hard if the spot falls onto a shiny area on the car; the specular reflection will send the light energy elsewhere and not much will return to the detector. Unless it was actively aimed at a license plate or other retroreflective area.
 
Last edited:
Last edited:
Looking at the projected laser spot from this device with an IR camera,.

Man you're like batman, you have all the cool toys! What would you recommend for my first logic analyzer. I do always have a laptop on my work bench, so a usb managed device seems like a good idea, but I have no idea what is good or bad.
 
further testing shows delay(13) eventually locks up as well. delay(15) is apparently necessary to avoid it.

Also, just FWIW, at a distance of 6 meters the spot size is about 4 cm in diameter.
 
Last edited:
Status
Not open for further replies.
Back
Top