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.