diff --git a/DroneSimulator/Drone.cs b/DroneSimulator/Drone.cs
index cee859c..5031cd1 100644
--- a/DroneSimulator/Drone.cs
+++ b/DroneSimulator/Drone.cs
@@ -255,11 +255,12 @@ namespace DroneSimulator
 
       Orientation = ori;
 
-      if(PosXYZ.Z < 0)
+      if(PosXYZ.Z <= 0)
       {
         PosXYZ.Z = 0;
         SpdXYZ.Z = 0;
         LaserRange = 0;
+        Acc = new Vector3(0, 0, 1);
       }
 
       /*if (PosXYZ.Z < 0)
@@ -281,8 +282,8 @@ namespace DroneSimulator
         Acc = new Vector3(0, 0, 1);
         Gyr = Vector3.Zero;
         LaserRange = 0;
-      }
-      else*/
+      }*/
+      else
       {
         if (ori.W < 0)
         {