I have done with basic firmware for my Lidar.
There were some problem with encoder signals - UART communication produce some electrical noise at the encoder's line.
Here is a video of first test:
I decrease angle resolution to 2 deg to increase rotational speed - it is near 5 rotations per second.
Circles at the "radar" images are 1, 2, 3 m in radius.