Skip to content

Instantly share code, notes, and snippets.

@svetlyak40wt

svetlyak40wt/ld19.lisp

Created October 31, 2025 09:39
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save svetlyak40wt/eddee9a9f7fc9fbcb633cd729cd31e0c to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save svetlyak40wt/eddee9a9f7fc9fbcb633cd729cd31e0c to your computer and use it in GitHub Desktop.
Download ZIP
LD19 Lidar reader for Common Lisp
Raw
ld19.lisp
(uiop:define-package #:app/hardware/ld19
(:use :cl)
(:import-from :cserial-port
:close-serial
:with-serial
:read-serial-byte-vector)
(:export :make-frames-reader
:update-lidar-points-from-frames))
(in-package #:app/hardware/ld19)
;; -------------------- Constants --------------------
(defconstant +header+ #x54)
(defconstant +verlen+ #x2C)
(defconstant +frame-len+ 47)
(defconstant +points-per-pack+ 12)
(defconstant +bytes-per-point+ 3)
;; -------------------- Classes --------------------
(defclass ld19-point ()
((distance :initarg :distance
:type float
:accessor point-distance)
(intensity :initarg :intensity
:type integer
:accessor point-intensity)
(angle :initarg :angle
:type float
:accessor point-angle)))
(defmethod print-object ((frame ld19-point) stream)
"Print ld19-point object showing angle, distance and intencity"
(print-unreadable-object (frame stream :type t)
(format stream "angle=~a distance=~A intencity=~A"
(point-angle frame)
(point-distance frame)
(point-intensity frame))))
(defclass ld19-frame ()
((lidar-speed :initarg :speed :accessor frame-lidar-speed)
(start-angle :initarg :start-angle :accessor frame-start-angle)
(end-angle :initarg :end-angle :accessor frame-end-angle)
(timestamp :initarg :timestamp :accessor frame-timestamp)
(points :initarg :points :accessor frame-points)))
(defmethod print-object ((frame ld19-frame) stream)
"Print ld19-frame object showing timestamp, lidar-speed, start-angle and end-angle"
(print-unreadable-object (frame stream :type t)
(format stream "timestamp=~a speed=~a start-angle=~a end-angle=~a"
(frame-timestamp frame)
(frame-lidar-speed frame)
(frame-start-angle frame)
(frame-end-angle frame))))
;; -------------------- Helper Functions --------------------
(defun get-2-bytes-lsb-msb (buffer offset)
"Read 2 bytes from buffer in little-endian format (LSB first, MSB second)"
(logior (aref buffer offset)
(ash (aref buffer (1+ offset)) 8)))
(defun angle-step (start-angle end-angle)
"Calculate angle step between start and end angles for 12 points"
(let ((angle-diff (mod (- end-angle start-angle) 36000)))
(if (> +points-per-pack+ 1)
(/ angle-diff
(1- +points-per-pack+))
0)))
(defun angle-from-step (start-angle step point-index)
"Calculate angle for a specific point index"
(/ (mod (+ start-angle
(* step point-index))
36000)
100.0))
;; -------------------- CRC Functions --------------------
(defparameter *crc-table*
#(#x00 #x4D #x9A #xD7 #x79 #x34 #xE3 #xAE #xF2 #xBF #x68 #x25 #x8B #xC6 #x11 #x5C
#xA9 #xE4 #x33 #x7E #xD0 #x9D #x4A #x07 #x5B #x16 #xC1 #x8C #x22 #x6F #xB8 #xF5
#x1F #x52 #x85 #xC8 #x66 #x2B #xFC #xB1 #xED #xA0 #x77 #x3A #x94 #xD9 #x0E #x43
#xB6 #xFB #x2C #x61 #xCF #x82 #x55 #x18 #x44 #x09 #xDE #x93 #x3D #x70 #xA7 #xEA
#x3E #x73 #xA4 #xE9 #x47 #x0A #xDD #x90 #xCC #x81 #x56 #x1B #xB5 #xF8 #x2F #x62
#x97 #xDA #x0D #x40 #xEE #xA3 #x74 #x39 #x65 #x28 #xFF #xB2 #x1C #x51 #x86 #xCB
#x21 #x6C #xBB #xF6 #x58 #x15 #xC2 #x8F #xD3 #x9E #x49 #x04 #xAA #xE7 #x30 #x7D
#x88 #xC5 #x12 #x5F #xF1 #xBC #x6B #x26 #x7A #x37 #xE0 #xAD #x03 #x4E #x99 #xD4
#x7C #x31 #xE6 #xAB #x05 #x48 #x9F #xD2 #x8E #xC3 #x14 #x59 #xF7 #xBA #x6D #x20
#xD5 #x98 #x4F #x02 #xAC #xE1 #x36 #x7B #x27 #x6A #xBD #xF0 #x5E #x13 #xC4 #x89
#x63 #x2E #xF9 #xB4 #x1A #x57 #x80 #xCD #x91 #xDC #x0B #x46 #xE8 #xA5 #x72 #x3F
#xCA #x87 #x50 #x1D #xB3 #xFE #x29 #x64 #x38 #x75 #xA2 #xEF #x41 #x0C #xDB #x96
#x42 #x0F #xD8 #x95 #x3B #x76 #xA1 #xEC #xB0 #xFD #x2A #x67 #xC9 #x84 #x53 #x1E
#xEB #xA6 #x71 #x3C #x92 #xDF #x08 #x45 #x19 #x54 #x83 #xCE #x60 #x2D #xFA #xB7
#x5D #x10 #xC7 #x8A #x24 #x69 #xBE #xF3 #xAF #xE2 #x35 #x78 #xD6 #x9B #x4C #x01
#xF4 #xB9 #x6E #x23 #x8D #xC0 #x17 #x5A #x06 #x4B #x9C #xD1 #x7F #x32 #xE5 #xA8))
(defun cal-crc8-from-buffer (buffer len-without-crc)
"Calculate CRC8 using lookup table. Equivalent to C++ _calCRC8FromBuffer.
Starts with 0xD8 (pre-calculated value for header 0x54 0x2C)."
(let ((crc #xD8))
(loop for i from 0 below len-without-crc
for byte = (aref buffer i)
do (setf crc (aref *crc-table* (logand (logxor crc byte) #xFF))))
crc))
(defun checksum-ok (frame)
"Check frame integrity using CRC8 algorithm from LD19 specification.
Frame structure: [0x54 0x2C] [44 bytes data] [1 byte CRC]
CRC is calculated for bytes 2-45 (indices 2 to 45 inclusive)."
(when (< (length frame) +frame-len+)
(return-from checksum-ok nil))
(let* ((data-buffer (subseq frame 2 46)) ; bytes 2-45 (44 bytes)
(expected-crc (aref frame 46)) ; CRC byte at index 46
(calculated-crc (cal-crc8-from-buffer data-buffer 44)))
(= calculated-crc expected-crc)))
(defun extract-frame-from-buffer (buffer)
(loop with length = (if (array-has-fill-pointer-p buffer)
(fill-pointer buffer)
(length buffer))
for i from 0 to (- length 2)
do (when (and (= (aref buffer i) +header+)
(= (aref buffer (1+ i)) +verlen+))
(when (>= (- length i)
+frame-len+)
(let ((frame (subseq buffer i (+ i +frame-len+))))
(when (checksum-ok frame)
(return frame)))))))
(defun read-frame (byte-reader)
"Read LD19 frame using byte-reader function, following C++ implementation pattern"
;; Look for header (0x54 0x2C) - equivalent to "T," in C++ code
(loop for byte1 = (funcall byte-reader)
do (when (= byte1 +header+)
(let ((byte2 (funcall byte-reader)))
(when (= byte2 +verlen+)
;; Found header, now read 45 bytes of data
(let ((buffer (make-array 45 :element-type '(unsigned-byte 8))))
(loop for i from 0 below 45
do (setf (aref buffer i) (funcall byte-reader)))
;; Parse frame data
(let* ((lidar-speed (get-2-bytes-lsb-msb buffer 0))
(start-angle (get-2-bytes-lsb-msb buffer 2))
(points '())
(end-angle (get-2-bytes-lsb-msb buffer 40))
(timestamp (get-2-bytes-lsb-msb buffer 42))
(crc-check (aref buffer 44)))
;; Check CRC
(when (= (cal-crc8-from-buffer buffer 44) crc-check)
;; Parse 12 points (equivalent to C++ LidarPoint data array)
(let ((step (angle-step start-angle end-angle)))
(loop for i from 0 below +points-per-pack+
for offset = (+ 4 (* i 3))
do (let* ((dist-mm (get-2-bytes-lsb-msb buffer offset))
(dist-meters (/ dist-mm
1000.0))
(intensity (aref buffer (+ offset 2)))
(angle (angle-from-step start-angle step i)))
(push (make-instance 'ld19-point
:distance dist-meters
:intensity intensity
:angle angle)
points)))
;; Return frame with points in correct order
(return (make-instance 'ld19-frame
:speed lidar-speed
:start-angle start-angle
:end-angle end-angle
:timestamp timestamp
:points (reverse points))))))))))))
(defun make-frame-reader (read-chunk-func)
"Create a frame reader function that uses byte-by-byte reading"
(let ((buffer nil)
(pointer nil))
(labels ((next-byte ()
(when (or (null buffer)
(= pointer (length buffer)))
(setf buffer (funcall read-chunk-func)
pointer 0))
(prog1 (aref buffer pointer)
(incf pointer)))
(frame-reader ()
(read-frame #'next-byte)))
#'frame-reader)))
(defun test-chunks-parsing (&key limit)
(let ((num-chunks 0)
(successes 0))
(loop with paths = (directory #P"/Users/art/projects/my/mts-true-tech-polufinal/data/raw-frames-2025-10-29/*.frame")
for filename in (if limit
(serapeum:take limit paths)
paths)
for data = (alexandria:read-file-into-byte-vector filename)
for parsed = (extract-frame-from-buffer data)
do (incf num-chunks)
when parsed
do (incf successes)
finally (return (list :total num-chunks
:successes successes
:ratio (float (* (/ successes num-chunks)
100)))))))
(define-condition no-more-chunks (error)
())
(defun test-new-frame-reader (&key (filemask #P"data/raw-frames-2025-10-29/*.frame"))
"Test the new frame reader with raw frame files"
(let ((num-chunks 0)
(successes 0))
(flet ((make-chunk-reader ()
(let ((paths (directory filemask)))
(flet ((read-chunk ()
(cond
(paths
(let ((buffer (alexandria:read-file-into-byte-vector (first paths))))
(incf num-chunks)
(setf paths (rest paths))
;; Я вычитывал чанки неверное и из-за этого кусок в 128 байт всегда добавлялся в начало, а буфер рос
;; Теперь надо откусить только первый кусок
(subseq buffer 0 128)))
(t
(error 'no-more-chunks)))))
#'read-chunk))))
(handler-case
(loop with frame-reader = (make-frame-reader (make-chunk-reader))
for frame = (funcall frame-reader)
when frame
do (incf successes))
(no-more-chunks ()
(values (list :total num-chunks
:successes successes
:ratio (float (* (/ successes num-chunks)
100)))))))))
(defun read-few-test-frames (&key (limit 3)
(path #P"data/raw-frames-2025-10-29/*.frame")
&aux (num-chunks-readed 0))
"Test the new frame reader with raw frame files"
(flet ((make-chunk-reader ()
(let ((paths (directory path)))
(flet ((read-chunk ()
(cond
(paths
(let ((buffer (alexandria:read-file-into-byte-vector (first paths))))
(incf num-chunks-readed)
(setf paths (rest paths))
;; Я вычитывал чанки неверное и из-за этого кусок в 128 байт всегда добавлялся в начало, а буфер рос
;; Теперь надо откусить только первый кусок
(subseq buffer 0 128)))
(t
(error 'no-more-chunks)))))
#'read-chunk))))
(values
(uiop:while-collecting (collect-frames)
(handler-case
(loop with frame-reader = (make-frame-reader (make-chunk-reader))
for frame = (funcall frame-reader)
for i from 0 below limit
when frame
do (collect-frames frame))
(no-more-chunks ()
(values))))
num-chunks-readed)))
(defun update-lidar-points-from-frames (lidar-points frames)
(unless (= (length lidar-points)
360)
(error "Only lidars with 360 points are supported in this hackathon!"))
(loop for frame in frames
do (loop for point in (frame-points frame)
for angle = (point-angle point)
for index = (floor angle)
do (setf (aref lidar-points index)
(point-distance point)))))
(defun make-lidar-points-from-frames (&key (limit 100)
(path "data/raw-frames-2025-10-29-2/*.frame"))
(loop with result = (make-array 360 :element-type 'float
:initial-element 0.0)
for frame in (read-few-test-frames :limit limit
:path path)
do (loop for point in (frame-points frame)
for angle = (point-angle point)
for index = (floor angle)
do (setf (aref result index)
(point-distance point)))
finally
(let ((lidar (app/models/robot:robot-lidar app/state:*robot*)))
;; (app/world:reset-world)
(setf (app/models/lidar:lidar-points lidar)
result)
(return result))))
(defun make-frames-reader (&key (port "/dev/ttyUSB0") (baud-rate 230400) (buffer-size 128))
"Возвращает читальщик фреймов с реального устройства."
(let ((connection (cserial-port:open-serial port :baud-rate baud-rate)))
(flet ((read-chunk ()
(let ((buffer (make-array buffer-size :element-type '(unsigned-byte 8))))
(cserial-port:read-serial-byte-vector buffer connection)
buffer)))
(let ((frame-reader (make-frame-reader #'read-chunk)))
(flet ((read-frames (n)
(cond
((and (symbolp n)
(eql n :stop))
(close-serial connection))
(t
(loop for i from 0 below n
collect (funcall frame-reader))))))
(values #'read-frames))))))
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment