def __init__(self, ip): """Initializes the drone object and sets properties. Sets self.ip to the ip parameter. Creates a CommandSender thread as a daemon (daemons are automatically killed when the main process ends). Creates a NavdataListener thread as a daemon. Starts both of these threads. Puts an FTRIM command on the command queue. (This might not do anything because the drone may get it before it boots fully.) ip -- the ip address of the drone. """ print("Initializing drone object...") self.ip = ip #Start Command sender and Navdata listener threads self.__cmd = CommandSender(self) self.__cmd.daemon = True self.__nav = NavdataListener(self) self.__nav.daemon = True self.__cmd.start() self.__nav.start() #Reset Trims self.__cmd.q.put(Command(command.FTRIM))
class Drone: """A class for interacting with a Parrot AR.Drone. Properties: - ip -- the ip address of the drone """ ip = "192.168.1.1" __u = cv.CreateMat(4, 1, cv.CV_32FC1) def __init__(self, ip): """Initializes the drone object and sets properties. Sets self.ip to the ip parameter. Creates a CommandSender thread as a daemon (daemons are automatically killed when the main process ends). Creates a NavdataListener thread as a daemon. Starts both of these threads. Puts an FTRIM command on the command queue. (This might not do anything because the drone may get it before it boots fully.) ip -- the ip address of the drone. """ print("Initializing drone object...") self.ip = ip #Start Command sender and Navdata listener threads self.__cmd = CommandSender(self) self.__cmd.daemon = True self.__nav = NavdataListener(self) self.__nav.daemon = True self.__cmd.start() self.__nav.start() #Reset Trims self.__cmd.q.put(Command(command.FTRIM)) def reset_watchdog(self): """Puts a RESET_WATCHDOG command on the CommandSender thread's queue.""" self.__cmd.q.put(Command(command.RESET_WATCHDOG)) def get_navdata(self, current=True): """Gets the drone's navdata object. current -- If set to False, pulls the navdata off the queue. Otherwise returns the most recent navdata. Defaults to True. """ if not current: return self.__nav.q.get() return self.__nav.get_navdata() def get_state(self): """Gets the drone's state vector as a cv matrix""" return self.__nav.get_state() def takeoff(self): """Puts a TAKEOFF command on the CommandSender thread's queue.""" self.__cmd.q.put(Command(command.TAKEOFF)) return True def land(self): """Puts a LAND command on the CommandSender thread's queue.""" self.__cmd.q.put(Command(command.LAND)) return True def go(self, pitch, roll, yawRate, gaz): """Puts a GO command on the CommandSender thread's queue. This is used to order the drone to move. pitch -- A float from -1 to 1 commanding the drone's pitch angle roll -- A float crom -1 to 1 commanding the drone's roll angle yawRate -- A float from -1 to 1 commanding the drone's yaw rotation rate gaz -- A float from -1 to 1 commanding the drone's vertical velocity """ if math.fabs(pitch) > .01 or math.fabs(roll) > .01 or math.fabs(yawRate) > .01 or math.fabs(gaz) > .01: int_pitch=struct.unpack('i',struct.pack('f',pitch))[0] int_roll=struct.unpack('i',struct.pack('f',roll))[0] int_yawRate=struct.unpack('i',struct.pack('f',yawRate))[0] int_gaz=struct.unpack('i',struct.pack('f',gaz))[0] self.__cmd.q.put(Command(command.GO,(int_roll, int_pitch, int_gaz, int_yawRate))) self.__u[0, 0] = pitch self.__u[1, 0] = roll self.__u[2, 0] = gaz self.__u[3, 0] = yawRate else: self.__cmd.q.put(Command(command.GO,(0,))) #hover cv.Zero(self.__u) self.__nav.set_command(self.__u) return True def hover(self): """Puts a GO command on the CommandSender thread's queue with all velocities set to 0.""" self.__cmd.q.put(Command(command.GO)) return True def reset(self): """Resets the kalman filter""" self.__nav.kalman.reset() def reset_emergency(self): """Resets the drone if it is in the emergency state by putting an ACK_EMERGENCY and an EMERGENCY command on the CommandSender thread's queue. This function works.""" self.__cmd.q.put(Command(command.ACK_EMERGENCY)) self.__cmd.q.put(Command(command.EMERGENCY)) return True def trim(self): """Puts an FTRIM command on the CommandSender thread's queue. Should only be called when the drone is on a flat surface!""" self.__cmd.q.put(Command(command.FTRIM)) return True def emergency(self): """Orders the drone into an emergency state by putting an EMERGENCY command on the CommandSender thread's queue. Prints the message \"Drone is in emergency state.\" """ self.__cmd.q.put(Command(command.EMERGENCY)) print "Drone is in emergency state (ordered by user)." return True def kill(self): """Kills the NavdataListener and CommandSender threads, then waits for them to die.""" print "Killing Drone..." self.__nav.kill = True print "Killing Navdata Loop..." self.__nav.join() self.__cmd.kill = True print "Killing Command Loop..." self.__cmd.join()