I need to test my slow PID controller but my quadcopter is not too stable, so I don't know if it's moving because of my PID or because of it's own movement. I think that the best way to do that is to make steps like this: https://imgur.com/yQAbmUY (I can't paste the picture because I have less than 5 points, sorry)

The output of the controller are the velocities in x, y and z (ux, uy and uz). In the time-lapse of the PID (for example, 0.2 seconds) I want the ux, uy and uz to have a determined value, and then in the time-lapse of the stop I want them to be ux=0.0, uy=0.0 and uz=0.0.

So my code would be something like this:

#!/usr/bin/env python

# a lot of imports

class gonnaGetDown():
    def __init__(self):

        self.velocity_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.orientation_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

        print "b"

        global delta_t
        global u_x_k, u_x_k_pre
        global kp_x, ki_x, kd_x
        global Ti_x, Td_x
        global error_x_k, error_x_k_pre, error_x_k_pre_x2
        global setpoint_x
        global u_y_k, u_y_k_pre
        global kp_y, ki_y, kd_y
        global Ti_y, Td_y
        global error_y_k, error_y_k_pre, error_y_k_pre_x2
        global setpoint_y
        global u_z_k, u_z_k_pre
        global kp_z, ki_z, kd_z
        global Ti_z, Td_z
        global error_z_k, error_z_k_pre, error_z_k_pre_x2
        global setpoint_z

        delta_t = 1/3
        setpoint_x = 0
        setpoint_y = 0 
        setpoint_z = 1 

        #x:
        error_x_k, error_x_k_pre, error_x_k_pre_x2 = 0, 0, 0
        u_x_k = 0
        #y:
        error_y_k, error_y_k_pre, error_y_k_pre_x2 = 0, 0, 0
        u_y_k = 0
        #z:
        error_z_k, error_z_k_pre, error_z_k_pre_x2 = 0, 0, 0
        u_z_k = 0

        print "b"

        rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.detect_marker)

    def detect_marker(self, msg):
        print "c"

        global u_x_k, u_x_k_pre, u_y_k, u_y_k_pre, u_z_k, u_z_k_pre
        global kp_x, ki_x, kd_x, kp_y, ki_y, kd_y, kp_z, ki_z, kd_z
        global delta_t
        global error_x_k, error_x_k_pre, error_x_k_pre_x2, error_y_k, error_y_k_pre, error_y_k_pre_x2, error_z_k, error_z_k_pre, error_z_k_pre_x2
        global contador

        Ti_x, Td_x = kp_x/ki_x, kd_x/kp_x
        Ti_y, Td_y = kp_y/ki_y, kd_y/kp_y
        Ti_z, Td_z = kp_z/ki_z, kd_z/kp_z    

        if not msg.markers:
            print "Tag not found"
            vel_msg = Twist()

            vel_msg.linear.x = 0.0
            vel_msg.linear.y = 0.0
            vel_msg.linear.z = 0.0
            vel_msg.angular.x = 0.0
            vel_msg.angular.y = 0.0
            vel_msg.angular.z = 0.0

            u_x_k = 0.0
            u_y_k = 0.0
            u_z_k = 0.0
            self.velocity_publisher.publish(vel_msg)
            return
        else:
            marker = msg.markers[0]
            print "d"
            #Primero ajusto las posiciones x e y con un PID
            error_x_k_pre_x2 = error_x_k_pre
            error_x_k_pre = error_x_k
            error_x_k = setpoint_x - marker.pose.pose.position.x

            error_y_k_pre_x2 = error_y_k_pre
            error_y_k_pre = error_y_k
            error_y_k = setpoint_y - marker.pose.pose.position.y

            error_z_k_pre_x2 = error_z_k_pre
            error_z_k_pre = error_z_k
            error_z_k = setpoint_z - marker.pose.pose.position.z

            u_x_k = ... # algorithm
            u_y_k = ... # algorithm
            u_z_k = ... # algorithm

            vel_msg = Twist()
            vel_msg.linear.x = u_y_k 
            vel_msg.linear.y = u_x_k 
            vel_msg.linear.z = u_z_k 

            self.velocity_publisher.publish(vel_msg)

            time.sleep(0.2) #0.2 seconds

            vel_msg.linear.x = 0.0
            vel_msg.linear.y = 0.0
            vel_msg.linear.z = 0.0
            self.velocity_publisher.publish(vel_msg)

            time.sleep(0.7) #0.7 seconds


if __name__ == '__main__':

    rospy.init_node('go_down', anonymous=True)

    try:
        rate = rospy.Rate(0.1) 
        gonna_get_down = gonnaGetDown()
        while not rospy.is_shutdown():

            print "a"

            rate.sleep()
    except rospy.ROSInterruptException:
        rospy.loginfo("Node terminated.")

It enters to the function detect_marker each 1/3 second.

I want the code to maintain the velocity in a determined value. For example, when it's value must be 0.0, I want to be 0.0 in all the 0.7 time-lapse (i.e, if there's wind that moves the drone, I want the drone to try to keep in the same position without moving).

How can I make that happen? Thanks (if you think that there's a better way to do this, please tell me)