Skip to content

Instantly share code, notes, and snippets.

@Fabien-B
Created June 18, 2018 22:55
Show Gist options
  • Star 3 You must be signed in to star a gist
  • Fork 2 You must be signed in to fork a gist
  • Save Fabien-B/87e57316e5b265ca42d179f3c860f879 to your computer and use it in GitHub Desktop.
Save Fabien-B/87e57316e5b265ca42d179f3c860f879 to your computer and use it in GitHub Desktop.
getting UST-05LN data over serial USB.
from serial import Serial
import time
from collections import namedtuple
class UST:
Command = namedtuple('Command',['command', 'answer_expected', 'answer'])
START_RANGING = Command('#GT15466', False, '')
STOP_RANGING = Command('#ST5297', True, '#ST00A845')
ID = Command('#IN0D54', True, '')
ID2 = Command('#CLC2DD', True, '')
PLOP = Command('#GR0EEE1', True, '')
MESURE_LENGHT = 4359
def __init__(self, port="/dev/ttyACM0", baudrate=115200):
self.ser = Serial(port, baudrate)
self.stop_ranging()
def send_command(self, command, timeout=2):
self.ser.write(command.command.encode()+b'\n') # writes command to LIDAR
self.ser.reset_input_buffer()
data = b''
start_time = time.time()
if command.answer_expected and command.answer != '': #precise answer expected, search for it !
while time.time() - start_time < timeout:
if self.ser.in_waiting:
data+=self.ser.read()
data = data.split(b'\n')[-1]
if command.answer.encode() in data:
break
return data
elif command.answer_expected: # answer expected but be don't known which : return the first one (until \n)
while time.time() - start_time < timeout:
if self.ser.in_waiting:
data+=self.ser.read()
if b'\n' in data:
data = data.split(b'\n')[0]
break
return data
else:
return b''
def stop_ranging(self):
self.send_command(self.STOP_RANGING)
def start_ranging(self):
self.stop_ranging()
self.send_command(self.START_RANGING)
def stop(self):
self.stop_ranging()
def get_measures(self):
"""
returns measures under the form (timestamp, [(distance, quality), ...])
timestamp : time in seconds since the LIDAR startup
distance range : 0 - 65635
valur range : 0 - ??? (65635 max)
eg: (102.123456, [(552, 1244), (646, 1216), (676, 1270), ...])
"""
raw_bytes=self.ser.read(ust.MESURE_LENGHT)
data = raw_bytes.split(b':')
timestamp = int(data[1], 16)/10**6
measurement_bytes = data[3]
measurements = [(int(measurement_bytes[i:i+4],16), int(measurement_bytes[i+4:i+8],16)) for i in range(0, len(measurement_bytes)-8, 8)]
return (timestamp, measurements)
def get_data(self, nb_bytes):
data = self.ser.read(nb_bytes)
data = data.split(b':')
data = data[3]
aa = [(int(data[i:i+4],16), int(data[i+4:i+8],16)) for i in range(0, len(data)-8, 8)]
print('data received')
if __name__ == "__main__":
ust = UST()
try:
ret = ust.send_command(ust.ID)
print(ret)
ust.start_ranging()
while True:
data = ust.get_measures()
if len(data[1]) == 541 :
print("ok at {:.06f} s".format(data[0]))
#print(data)
finally:
ust.stop()
@Fabien-B
Copy link
Author

Fabien-B commented Mar 4, 2022

Hi, the output of the lidar (at least the UST05LN, but probably others too) is ASCII, so you can take a look at it as a text.
For each scan, the lidar output some data separated by ":".
One of the field is the timestamp of the scan, an other field is the list of the distances and signal strength, on 2 bytes each.
For example, 01F404B0020803E8022605DC means, in the (distance, quality) form: [(500, 1200), (520, 1000), (550, 1500)].
Based on this, you can look if the protocol is similar.
What I can see in your error message is that it tries to interpret as hexa a string containing L and N letters, which are not hexadecimals symbols. It is probably not measurement data, you can certainly figure that out by reading the ASCII output.

Also, this code id very fragile to communications errors, so you may want to write a better code, but at least you have the idea of the protocol!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment