Esp32 & LiDAR
Posted: Sun Jan 04, 2026 5:20 pm
I mentioned New Year's Resolutions - and doing some fun projects....
So this is my 'snow day' fun.
I got a G4 LiDAR sensor - (from YDLiDAR) - and connected it to a PSU and an esp32. The eventual aim is to have the esp32 display the output (or save to SD?) - but for now it outputs the data to a PC (as X,Y pairs) - where it is displayed by a Python app.
I had intended it to be all in Flowcode - but had to add a touch of C - I couldn't get the Rx interrupt to work quickly enough (data is output by the scanner an 230400 baud) - and there is a 'generous' 1.5ms between data blocks (of up to 80 'points' each)
The scanner outputs an 8 byte header:
0xAA55 CT, PTS, .... where CT = 1 for a new 'lap'
0xAA55 CT, PTS, start_angle, end_angle, checksum - where CT = 0 (and I ignore the checksum) - followed by PTS x distances.
So I have a simple state machine that 'looks' for 0xAA then 0x55 - then header - and in a little more C - I allocate a buffer to store this header and the distance data before passing it in a queue to be processed. I probably should have pinned the 'Receive' and 'Process' macros to individual cores - which could give a little more headroom.
Process data - calculates the start and end angle (and the difference between them (/ points -1) - as individual steps) - and then calculates x, y data using polar co-ordinates (x = d * cos(angle) y = d * sin(angle)
There was originally an option to output data in 'readable' form as '(x,y)' - but for speed the binary format is better - I buffer the data then output it using WriteIntArray. The final data might have a different number of points - as I check if distance is 0 (it might be better to check > min_distance?) - so the final output format is 0xAA55 NNNN XXXXYYYY XXXXYYY - where NNNN is a 16 bit number of XXXXYYYY values.
This is output (in the attached for alternate 'laps' - n_laps constant) - to UART1 which I have set to 921600 baud - if your PC can cope - if not reduce this! My PC can cope with every lap (I think?) - however if the process queue is filled then some data may be dropped.
After a bit of 'fiddling' - originally I had left and right / up and down swapped. - the output looks reasonable - and you can click on two points for a measurement between them.... Objects have a 'shadow' - so anything 'behind' an object is hidden.
The LiDAR I have doesn't record a 'quality' value for each datum - though some do.
A sample output - current 'lap' is in blue with previous fading in green.
I'm the fuzzy line at the bottom - my monitor (which is curved!) at the top.
The documentation has a 'correction' formula - for the angle - the macro is still there - but using it gives a spiral effect - my maths isn't up to the job?
Supplementary code holds the code to create a UART and receive data from the LiDAR (with a bigger buffer) - and to send commands. This data is then passed to RxLIDAR (originally an interrupt handler) - which acts as a simple state machine. I use malloc to allocate the buffer for the current block of data - and this is passed to ProcessData when full. When 'processed' the memory is freed.
There are many possible enhancements - for example 'stop' - or just receiving data between two angles - I initially used the 'health' and ''info' commands to check comms. I'm not sure I'd trust my car control to it!
The LiDAR NEEDS an external PSU - it can draw up to 1A (although I haven't seen this) when spinning up the motor. The motor_pwm pin - although offered up as a property is actually hard-coded in supplementary code. I couldn't get the PWM component to work - and probably gave up rather easily!
Martin
So this is my 'snow day' fun.
I got a G4 LiDAR sensor - (from YDLiDAR) - and connected it to a PSU and an esp32. The eventual aim is to have the esp32 display the output (or save to SD?) - but for now it outputs the data to a PC (as X,Y pairs) - where it is displayed by a Python app.
I had intended it to be all in Flowcode - but had to add a touch of C - I couldn't get the Rx interrupt to work quickly enough (data is output by the scanner an 230400 baud) - and there is a 'generous' 1.5ms between data blocks (of up to 80 'points' each)
The scanner outputs an 8 byte header:
0xAA55 CT, PTS, .... where CT = 1 for a new 'lap'
0xAA55 CT, PTS, start_angle, end_angle, checksum - where CT = 0 (and I ignore the checksum) - followed by PTS x distances.
So I have a simple state machine that 'looks' for 0xAA then 0x55 - then header - and in a little more C - I allocate a buffer to store this header and the distance data before passing it in a queue to be processed. I probably should have pinned the 'Receive' and 'Process' macros to individual cores - which could give a little more headroom.
Process data - calculates the start and end angle (and the difference between them (/ points -1) - as individual steps) - and then calculates x, y data using polar co-ordinates (x = d * cos(angle) y = d * sin(angle)
There was originally an option to output data in 'readable' form as '(x,y)' - but for speed the binary format is better - I buffer the data then output it using WriteIntArray. The final data might have a different number of points - as I check if distance is 0 (it might be better to check > min_distance?) - so the final output format is 0xAA55 NNNN XXXXYYYY XXXXYYY - where NNNN is a 16 bit number of XXXXYYYY values.
This is output (in the attached for alternate 'laps' - n_laps constant) - to UART1 which I have set to 921600 baud - if your PC can cope - if not reduce this! My PC can cope with every lap (I think?) - however if the process queue is filled then some data may be dropped.
After a bit of 'fiddling' - originally I had left and right / up and down swapped. - the output looks reasonable - and you can click on two points for a measurement between them.... Objects have a 'shadow' - so anything 'behind' an object is hidden.
The LiDAR I have doesn't record a 'quality' value for each datum - though some do.
- lidar.png (60.52 KiB) Viewed 125 times
I'm the fuzzy line at the bottom - my monitor (which is curved!) at the top.
The documentation has a 'correction' formula - for the angle - the macro is still there - but using it gives a spiral effect - my maths isn't up to the job?
Supplementary code holds the code to create a UART and receive data from the LiDAR (with a bigger buffer) - and to send commands. This data is then passed to RxLIDAR (originally an interrupt handler) - which acts as a simple state machine. I use malloc to allocate the buffer for the current block of data - and this is passed to ProcessData when full. When 'processed' the memory is freed.
There are many possible enhancements - for example 'stop' - or just receiving data between two angles - I initially used the 'health' and ''info' commands to check comms. I'm not sure I'd trust my car control to it!
The LiDAR NEEDS an external PSU - it can draw up to 1A (although I haven't seen this) when spinning up the motor. The motor_pwm pin - although offered up as a property is actually hard-coded in supplementary code. I couldn't get the PWM component to work - and probably gave up rather easily!
Martin