Garmin Lidar Lite v3 3D Scanner difficulty ready planar surfaces

I recently constructed a 3D scanner with a LIDAR-lite but have been having difficulty getting an accurate reading of the ground floor, walls and ceilings. When I scan the interior of a room it reads the walls and ceiling as being curved even though the room is all orthogonal. It also does the same for the ground in which it reads the ground as elevating up at an angle even though the ground is a constant flat surface. I have tested both inside and outside and have received the same results with the ground being at an angle. I’m fairly new to LIDAR and have been following a tutorial by Charles Grassin to get me started with this project (Charles' Labs - 3D Lidar Scanner)

The schematics are the same from his tutorial except i’m powering the servos through a 9V battery that’s connected to the vin pin.

Could this be at fault with the way the LIDAR is connected the the two servo motors on the rig or with the LIDAR sensor itself?

I provide pictures of examples of the 3d scanner and of my current rig.

Thanks for the Help

This is the code for the servos and LIDAR

#include <Servo.h>
#include <Wire.h>
#include <LIDARLite.h>

//Delay between each sample to avoid mechanical wobble
//Size of the steps of YAW/PITCH in degrees (1 = full res)
#define YAW_STEP 1
#define PITCH_STEP 1
#define MATH_PI 3.1415f

LIDARLite myLidarLite;
Servo servoYaw,servoPitch;
char s[15];
int yawAngle,pitchAngle;
int x,y,z,r;
float theta,phi;

void setup() {
  // Initialize serial connection to display distance readings

  //Servo init

  //Lidar Lite v3 init
  myLidarLite.begin(0, true);

void loop() {
  //Sweep Yaw servomotor
  for (yawAngle = 0; yawAngle <= 180; yawAngle += YAW_STEP) {
    //Sweep Pitch servomotor. The direction depends on the current directory
    if(pitchAngle < 90){
      for (pitchAngle = 0; pitchAngle <= 180;pitchAngle+= PITCH_STEP){
    } else {
      for (pitchAngle = 180; pitchAngle >= 0;pitchAngle-= PITCH_STEP){

// Function to acquire, convert and send the measurement.
void sendMeasurement(){
  // Get spherical coordinates
  r = myLidarLite.distance(false);
  theta = (float)yawAngle * PI / 180.0f;
  phi = (float)pitchAngle * PI / 180.0f;

  // Convert and send them
  sprintf(s,"%d %d %d\n\0",(int)(r*cos(phi)*cos(theta)),(int)(r*cos(phi)*sin(theta)),(int)(r*sin(phi)));

Looks like you need to calibrate your servos. RC servos are not precision devices in any sense, rough and ready at best. Also if your Arduino uses a ceramic resonator for its clock you have a +/-0.5% error from that too.

For the servos use uSeconds instead of degrees. Use metal geared servos.

I torque the servo by 2mS and hold for 12mS and during the hold allow a TFMiniPlus to take a single measurement, and store the measurement in a ring buffer. When the TFMiniPlus is done it causes a servo torque; repeat.

Bi-directional sweep.

To flatten out the display, Each measurement dot, before display, is angle corrected through the use of Trigonometry and solving for the hypotenuse.

Never power servos with the 5V pin. That can actually destroy the Arduino.

Use a separate servo power supply and connect the grounds.

Dear, ColumbusQ.

I had the same situation. Probably, your problem is not the hardware.
I think you didn't convert your angles to radians in formula for converting a spherical to Cartesian coordinates.

You can use this formula:

x = r * sin(radians(vertical angle)) * cos(radians(horizontal angle))
y = r * sin(radians(vertical angle)) * sin(radians(horizontal angle))
z = r * cos(radians(vertical angle))

r - distance reading from the lidar