diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index 0bd6519de95e368b2f8948798ad3728844ad0312..886c13cbbf10e37b5a9d9d73f5af17b0656ca574 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -121,7 +121,7 @@ class PPSRadioClient:
 
         #publish battery voltage for GUI
         #cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
-        print "batteryVolt: %s" % batteryVolt
+        # print "batteryVolt: %s" % batteryVolt
         cfbattery_pub.publish(batteryVolt)
 
 
diff --git a/pps_ws/src/d_fall_pps/scripts/land_crazyflie b/pps_ws/src/d_fall_pps/scripts/land_crazyflie
new file mode 100755
index 0000000000000000000000000000000000000000..38d16c7d0eab1ef9e63e1c43e50f901ab72063cc
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/scripts/land_crazyflie
@@ -0,0 +1,6 @@
+#!/bin/bash
+
+if [ "$#" -ne 0 ]
+then echo "usage: land crazyfly <no arguments>"
+else rostopic pub -1 /$ROS_NAMESPACE/PPSClient/Command std_msgs/Int32 4;
+fi
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index d0238a06b635e167b514eba2b20f0858ea652a52..a0f1d49e61381f05dcc7e5553f49174b09ee63eb 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -46,8 +46,10 @@
 #define STATE_FLYING     3
 #define STATE_LAND       4
 
-#define TAKE_OFF_OFFSET  0.7    //in meters
-#define LANDING_DISTANCE 0.3    //in meters
+#define TAKE_OFF_OFFSET  1    //in meters
+#define LANDING_DISTANCE 0.15    //in meters
+#define DURATION_TAKE_OFF  300   //100 is 1 second
+#define DURATION_LANDING   300   //100 is 1 second
 
 #define PI 3.141592653589
 
@@ -167,8 +169,14 @@ void takeOffCF(CrazyflieData& current_local_coordinates) //local because the set
     setpoint_msg.x = current_local_coordinates.x;           // previous one
     setpoint_msg.y = current_local_coordinates.y;           //previous one
     setpoint_msg.z = current_local_coordinates.z + TAKE_OFF_OFFSET;           //previous one plus some offset
-    setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
+    // setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
+    setpoint_msg.yaw = 0.0;
     safeControllerServiceSetpointPublisher.publish(setpoint_msg);
+    ROS_INFO("Take OFF:");
+    ROS_INFO("------Current coordinates:");
+    ROS_INFO("X: %f, Y: %f, Z: %f", current_local_coordinates.x, current_local_coordinates.y, current_local_coordinates.z);
+    ROS_INFO("------New coordinates:");
+    ROS_INFO("X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
 
     // now, use safe controller to go to that setpoint
     usingSafeController = true;
@@ -191,8 +199,19 @@ void landCF(CrazyflieData& current_local_coordinates)
     loadSafeController();
 }
 
+void goToOrigin()
+{
+    Setpoint setpoint_msg;
+    setpoint_msg.x = 0;
+    setpoint_msg.y = 0;
+    setpoint_msg.z = 0.4;
+    setpoint_msg.yaw = 0;
+    safeControllerServiceSetpointPublisher.publish(setpoint_msg);
+}
+
 void changeFlyingStateTo(int new_state)
 {
+    ROS_INFO("Change state to: %d", new_state);
     flying_state = new_state;
     changed_state_flag = true;
 }
@@ -230,7 +249,7 @@ void viconCallback(const ViconData& viconData) {
                         finished_take_off = false;
                     }
                     counter++;
-                    if(counter >= 200)
+                    if(counter >= DURATION_TAKE_OFF)
                     {
                         counter = 0;
                         finished_take_off = true;
@@ -245,6 +264,8 @@ void viconCallback(const ViconData& viconData) {
                     if(changed_state_flag) // stuff that will be run only once when changing state
                     {
                         changed_state_flag = false;
+                        // need to change setpoint to the one from file
+                        goToOrigin();
                     }
                     break;
                 case STATE_LAND:
@@ -256,7 +277,7 @@ void viconCallback(const ViconData& viconData) {
                         finished_land = false;
                     }
                     counter++;
-                    if(counter >= 200)
+                    if(counter >= DURATION_LANDING)
                     {
                         counter = 0;
                         finished_land = true;