The control states of takeoff and land are activated through ROS service calls. Within the crazyflie_controller node, two ROS services are created with callback functions to be invoked by a client when a request for the service is sent. The services for /crazyflie/land and crazyflie/takeoff are created by the following statements in control_crazyflie.py:

s1 = rospy.Service("/crazyflie/land", Empty, self._Land)
s2 = rospy.Service("/crazyflie/takeoff", Empty, self._Takeoff)

Note that the /crazyflie namespace has been appended to these services to identify that they are specific for the quadrotor. Land and Takeoff are private callback functions that handle the service requests.

These services are of the type Empty, one of the service types provided by the ROS std_srvs package. The std_srvs package contains common service patterns for signals to an ROS node. The Empty service definition contains no actual data but is used only to cause the execution of a function.

For the land service, the following function is executed:

def _Land(self, req):
  rospy.loginfo("Landing requested!")
  self._cf_state = 'land'
  return ()

When the /crazyflie/land service is requested, the loginfo function writes a log message to stdout (the terminal window) and to the /rosout topic. The message also appears in the ~/.ros/log file for the crazyflie_controller node. The next statement changes the Crazyflie control state to land. An Empty service response message is returned to the client node.

The takeoff service is handled by a function similar to _Land. It also writes a log message and changes the Crazyflie control state to takeoff. An Empty service response is sent back to the client node.

The control states of hover and flight utilize the PID class constructor, attributes, and methods from pid.py and data from crazyflie2.yaml. There are three PID objects created to provide proportional, integral, and derivative control for Crazyflie's linear x, y, and z (pitch, roll, and thrust) values. The crazyflie_controller node instantiates a separate flight PID controller for X, Y, and Z, as shown in the statements below:

from pid import PID      # for PID class, attributes and methods

# object instances of type PID with initial attributes assigned
self.m_pidX = PID(rospy.get_param("~PIDs/X/kp"),
                  rospy.get_param("~PIDs/X/kd"),
                  rospy.get_param("~PIDs/X/ki"),
                  rospy.get_param("~PIDs/X/minOutput"),
                  rospy.get_param("~PIDs/X/maxOutput"),
                  rospy.get_param("~PIDs/X/integratorMin"),
                  rospy.get_param("~PIDs/X/integratorMax"))
self.m_pidY = PID(rospy.get_param("~PIDs/Y/kp"),
                  rospy.get_param("~PIDs/Y/kd"),
                  rospy.get_param("~PIDs/Y/ki"),
                  rospy.get_param("~PIDs/Y/minOutput"),
                  rospy.get_param("~PIDs/Y/maxOutput"),
                  rospy.get_param("~PIDs/Y/integratorMin"),
                  rospy.get_param("~PIDs/Y/integratorMax"))
self.m_pidZ = PID(rospy.get_param("~PIDs/Z/kp"),
                  rospy.get_param("~PIDs/Z/kd"),
                  rospy.get_param("~PIDs/Z/ki"),
                  rospy.get_param("~PIDs/Z/minOutput"),
                  rospy.get_param("~PIDs/Z/maxOutput"),
                  rospy.get_param("~PIDs/Z/integratorMin"),
                  rospy.get_param("~PIDs/Z/integratorMax"))
self.m_pidYaw = PID(rospy.get_param("~PIDs/Yaw/kp"),
                    rospy.get_param("~PIDs/Yaw/kd"),
                    rospy.get_param("~PIDs/Yaw/ki"),
                    rospy.get_param("~PIDs/Yaw/minOutput"),
                    rospy.get_param("~PIDs/Yaw/maxOutput"),
                    rospy.get_param("~PIDs/Yaw/integratorMin"),
                    rospy.get_param("~PIDs/Yaw/integratorMax"))

A PID controller is also created for yaw control but is not used at this time. The values of parameters kp, kd, ki, minOutput, maxOutput, integratorMin, and integratorMax are loaded from the crazyflie2.yaml file as part of the control_crazyflie.launch process. This arrangement of loading the parameters from the YAML file has made it quick and easy to change parameters while testing flight control.

The PID class has several methods to perform operations for the PID controller object instance. A method to reset the controller is provided by the reset method. This method sets the m_integral and m_previousError values to zero and the m_previousTime to the current time. The setIntegral method sets the m_integral value to a value passed to the function. The third method update performs the PID calculations between the current location and the target location, as shown in the following statements:

def update (self, value, targetValue):

  time = float(rospy.Time.to_sec(rospy.Time.now()))
  dt = time - self.m_previousTime

  error = targetValue - value
  self.m_integral += error * dt
  self.m_integral = max(min(self.m_integral, self.m_integratorMax),self.m_integratorMin)

  p = self.m_kp * error
  d = 0
  if dt > 0:
    d = self.m_kd * (error - self.m_previousError)/dt
  i = self.m_ki * self.m_integral

  output = p + d + i
    
  self.m_previousError = error
  self.m_previousTime = time

  return max(min(output, self.m_maxOutput), self.m_minOutput)

Note that rospy.loginfo statements have been removed to enhance clarity.

In the update method, the current time in seconds is determined by a call to the rospy routines, Time.now and Time_to_sec. The variable dt is set to the number of seconds elapsed between the last call to the controller and the current time. The difference between value and targetValue is stored as the variable error. This error value is multiplied by m_kp to obtain the proportional variable p. The difference in this error value and the last error value is calculated and divided by the delta time dt. This value is multiplied by m_kd to find the derivative term d. The last term, the integral i, is calculated as the value of m_ki times m_integral. The three terms p, i, and d are added to compute the output variable. This variable is compared to the m_minOutput and m_maxOutput values to determine whether it falls within this range. If it does, then the value of output is returned. Otherwise, if the output value is larger than m_maxOutput, m_maxOutput is returned. If it is less than m_minOutput, m_minOutput is returned.

Throughout the testing phase for this mission, Crazyflie would exhibit some erratic behavior. Due to the modular nature of ROS, we decided to implement an observer node that would keep track of the location of Crazyflie. The node crazyflie_window (in watcher.py) listens to the tf transforms publishing the location of Crazyflie. In a loop that runs at 10 times a second, the following statements are executed:

if listener.frameExists(camera_frame) and listener.frameExists(crazyflie_frame):
  t = listener.getLatestCommonTime(camera_frame, crazyflie_frame)
  trans, rotate = listener.lookupTransform(camera_frame, crazyflie_frame, t)

This code checks the transforms that are buffered by the listener for the existence of a transform between crazyflie/baselink and kinect2_ir_optical_frame. When this specific transform is found, the data fields for translational and rotational data are extracted into the trans and rotate variables. The trans variable contains the location in the x, y, and z of the Crazyflie. This location is compared to the edge of the Kinect image:

if (trans[0] < 100) or (trans[0] > (camera_width - 100)) or (trans[1] < 20) or (trans[1] > (camera_height - 20)):

  # Crazyflie is going outside the frame
  rospy.loginfo("Crazyflie outside of window %f %f %f",
                trans[0], trans[1], trans[2])
  rospy.loginfo("Landing requested")
 
  # wait until land service is available, then create handle for it
  rospy.wait_for_service('/crazyflie/land')
  try:
    _land = rospy.ServiceProxy('/crazyflie/land', Empty)
    _land()
  except rospy.ServiceException, e:
    rospy.loginfo("Service call failed: %s", e)

If the position of Crazyflie is within 100 pixels of the left or right edge of the image frame or within 20 pixels of the upper or lower edge, a service request is made for Crazyflie to land. A private local proxy _land is used to make the service call with an Empty service request. The land service request is handled by the crazyflie_controller node as described in the previous section, Using ROS services to control takeoff and land.

Messages are sent to the log file and the /rosout topic to identify the location of the Crazyflie that caused the crazyflie_window node to send the service request. These messages are important when determining the events of Crazyflie's flight. The Kinect's depth data trans[2] is too erratic to use for this monitoring instance.

The next sections describe how the Crazyflie operates when the _cf_state variable is set to flight. The Crazyflie will either hover in place or fly to a target depending on whether any target_pose messages have been received.