# Copyright (c) 2011 Bastian Venthur # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal # in the Software without restriction, including without limitation the rights # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell # copies of the Software, and to permit persons to whom the Software is # furnished to do so, subject to the following conditions: # # The above copyright notice and this permission notice shall be included in # all copies or substantial portions of the Software. # # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. """ Python library for the AR.Drone. V.1 This module was tested with Python 2.6.6 and AR.Drone vanilla firmware 1.5.1. V.2.alpha """ import logging import socket import struct import sys import threading import multiprocessing import arnetwork import time import numpy as np __author__ = "Bastian Venthur" ARDRONE_NAVDATA_PORT = 5554 ARDRONE_VIDEO_PORT = 5555 ARDRONE_COMMAND_PORT = 5556 ARDRONE_CONTROL_PORT = 5559 SESSION_ID = "943dac23" USER_ID = "36355d78" APP_ID = "21d958e4" DEBUG = False # 0: "Not defined" # 131072: "Landed" # 393216: "Taking-off-Floor" # 393217: "Taking-off-Air" # 262144: "Hovering" # 524288: "Landing" # 458752: "Stabilizing" # 196608: "Moving" # 262153 and 196613 and 262155 and 196614 and 458753: "Undefined" ctrl_state_dict={0:0, 131072:1, 393216:2, 393217:3, 262144:4, 524288:5, 458752:6, 196608:7, 262153:8, 196613:9, 262155:10, 196614:11, 458753: 12} class ARDrone(object): """ARDrone Class. Instanciate this class to control your drone and receive decoded video and navdata. Possible value for video codec (drone2): NULL_CODEC = 0, UVLC_CODEC = 0x20, // codec_type value is used for START_CODE P264_CODEC = 0x40, MP4_360P_CODEC = 0x80, H264_360P_CODEC = 0x81, MP4_360P_H264_720P_CODEC = 0x82, H264_720P_CODEC = 0x83, MP4_360P_SLRS_CODEC = 0x84, H264_360P_SLRS_CODEC = 0x85, H264_720P_SLRS_CODEC = 0x86, H264_AUTO_RESIZE_CODEC = 0x87, // resolution is automatically adjusted according to bitrate MP4_360P_H264_360P_CODEC = 0x88, """ def __init__(self, is_ar_drone_2=False, hd=False): self.seq_nr = 1 self.timer_t = 0.2 self.com_watchdog_timer = threading.Timer(self.timer_t, self.commwdg) self.lock = threading.Lock() self.speed = 0.2 self.hd = hd if (self.hd): self.image_shape = (720, 1280, 3) else: self.image_shape = (360, 640, 3) time.sleep(0.5) self.config_ids_string = [SESSION_ID, USER_ID, APP_ID] self.configure_multisession(SESSION_ID, USER_ID, APP_ID, self.config_ids_string) self.set_session_id (self.config_ids_string, SESSION_ID) time.sleep(0.5) self.set_profile_id(self.config_ids_string, USER_ID) time.sleep(0.5) self.set_app_id(self.config_ids_string, APP_ID) time.sleep(0.5) self.set_video_bitrate_control_mode(self.config_ids_string, "1") time.sleep(0.5) self.set_video_bitrate(self.config_ids_string, "10000") time.sleep(0.5) self.set_max_bitrate(self.config_ids_string, "10000") time.sleep(0.5) self.set_fps(self.config_ids_string, "30") time.sleep(0.5) if (self.hd): self.set_video_codec(self.config_ids_string, 0x83) else: self.set_video_codec(self.config_ids_string, 0x88) self.last_command_is_hovering = True self.com_pipe, com_pipe_other = multiprocessing.Pipe() self.navdata = dict() self.navdata[0] = dict(zip(['ctrl_state', 'battery', 'theta', 'phi', 'psi', 'altitude', 'vx', 'vy', 'vz', 'num_frames'], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0])) self.network_process = arnetwork.ARDroneNetworkProcess(com_pipe_other, is_ar_drone_2, self) self.network_process.start() self.image = np.zeros(self.image_shape, np.uint8) self.time = 0 self.last_command_is_hovering = True time.sleep(1.0) self.at(at_config_ids , self.config_ids_string) self.at(at_config, "general:navdata_demo", "TRUE") def takeoff(self): """Make the drone takeoff.""" self.at(at_ftrim) self.at(at_config, "control:altitude_max", "20000") self.at(at_ref, True) def set_max_alt(self, alt): self.at(at_config_ids , self.config_ids_string) self.at(at_config, "control:altitude_max", alt) def set_max_vz(self, speed): self.at(at_config_ids , self.config_ids_string) self.at(at_config, "control:control_vz_max", speed) def set_max_angle(self, eul): self.at(at_config_ids , self.config_ids_string) self.at(at_config, "control:euler_angle_max", eul) def set_max_rotspeed(self, speed): self.at(at_config_ids , self.config_ids_string) self.at(at_config, "control:control_yaw", speed) def land(self): """Make the drone land.""" self.at(at_ref, False) def hover(self): """Make the drone hover.""" self.at(at_pcmd, False, 0, 0, 0, 0) def move_freely(self, hov, x,y, alt, rot): self.at(at_pcmd, hov, x, y, alt, rot) def move_left(self): """Make the drone move left.""" self.at(at_pcmd, True, -self.speed, 0, 0, 0) def move_right(self): """Make the drone move right.""" self.at(at_pcmd, True, self.speed, 0, 0, 0) def move_up(self): """Make the drone rise upwards.""" self.at(at_pcmd, True, 0, 0, self.speed, 0) def move_down(self): """Make the drone decent downwards.""" self.at(at_pcmd, True, 0, 0, -self.speed, 0) def move_forward(self): """Make the drone move forward.""" self.at(at_pcmd, True, 0, -self.speed, 0, 0) def move_backward(self): """Make the drone move backwards.""" self.at(at_pcmd, True, 0, self.speed, 0, 0) def turn_left(self): """Make the drone rotate left.""" self.at(at_pcmd, True, 0, 0, 0, -self.speed) def turn_right(self): """Make the drone rotate right.""" self.at(at_pcmd, True, 0, 0, 0, self.speed) def reset(self): """Toggle the drone's emergency state.""" # Enter emergency mode self.at(at_ref, False, True) self.at(at_ref, False, False) # Leave emergency mode self.at(at_ref, False, True) def trim(self): """Flat trim the drone.""" self.at(at_ftrim) def set_speed(self, speed): """Set the drone's speed. Valid values are floats from [0..1] """ self.speed = speed def set_camera_view(self, downward): """ Set which video camera is used. If 'downward' is true, downward camera will be viewed - otherwise frontwards. """ channel = None if downward: channel = 0 else: channel = 1 self.set_video_channel(self.config_ids_string, channel) def at(self, cmd, *args, **kwargs): """Wrapper for the low level at commands. This method takes care that the sequence number is increased after each at command and the watchdog timer is started to make sure the drone receives a command at least every second. """ self.lock.acquire() self.com_watchdog_timer.cancel() cmd(self.seq_nr, *args, **kwargs) self.seq_nr += 1 self.com_watchdog_timer = threading.Timer(self.timer_t, self.commwdg) self.com_watchdog_timer.start() self.lock.release() def configure_multisession(self, session_id, user_id, app_id, config_ids_string): self.at(at_config, "custom:session_id", session_id) self.at(at_config, "custom:profile_id", user_id) self.at(at_config, "custom:application_id", app_id) def set_session_id (self, config_ids_string, session_id): self.at(at_config_ids , config_ids_string) self.at(at_config, "custom:session_id", session_id) def set_profile_id (self, config_ids_string, profile_id): self.at(at_config_ids , config_ids_string) self.at(at_config, "custom:profile_id", profile_id) def set_app_id (self, config_ids_string, app_id): self.at(at_config_ids , config_ids_string) self.at(at_config, "custom:application_id", app_id) def set_video_bitrate_control_mode (self, config_ids_string, mode): self.at(at_config_ids , config_ids_string) self.at(at_config, "video:bitrate_control_mode", mode) def set_video_bitrate (self, config_ids_string, bitrate): self.at(at_config_ids , config_ids_string) self.at(at_config, "video:bitrate", bitrate) def set_video_channel(self, config_ids_string, channel): self.at(at_config_ids , config_ids_string) self.at(at_config, "video:video_channel", channel) def set_max_bitrate(self, config_ids_string, max_bitrate): self.at(at_config_ids , config_ids_string) self.at(at_config, "video:max_bitrate", max_bitrate) def set_fps (self, config_ids_string, fps): self.at(at_config_ids , config_ids_string) self.at(at_config, "video:codec_fps", fps) def set_video_codec (self, config_ids_string, codec): self.at(at_config_ids , config_ids_string) self.at(at_config, "video:video_codec", codec) def commwdg(self): """Communication watchdog signal. This needs to be send regulary to keep the communication w/ the drone alive. """ self.at(at_comwdg) def halt(self): """Shutdown the drone. This method does not land or halt the actual drone, but the communication with the drone. You should call it at the end of your application to close all sockets, pipes, processes and threads related with this object. """ self.lock.acquire() self.com_watchdog_timer.cancel() self.com_pipe.send('die!') self.network_process.terminate() self.network_process.join() self.lock.release() def get_image(self): _im = np.copy(self.image) return _im def get_navdata(self): return self.navdata def set_navdata(self, navdata): self.navdata = navdata self.get_navdata() def set_image(self, image): if (image.shape == self.image_shape): self.image = image self.image = image def apply_command(self, command): available_commands = ["emergency", "land", "takeoff", "move_left", "move_right", "move_down", "move_up", "move_backward", "move_forward", "turn_left", "turn_right", "hover"] if command not in available_commands: logging.error("Command %s is not a recognized command" % command) if command != "hover": self.last_command_is_hovering = False if (command == "emergency"): self.reset() elif (command == "land"): self.land() self.last_command_is_hovering = True elif (command == "takeoff"): self.takeoff() self.last_command_is_hovering = True elif (command == "move_left"): self.move_left() elif (command == "move_right"): self.move_right() elif (command == "move_down"): self.move_down() elif (command == "move_up"): self.move_up() elif (command == "move_backward"): self.move_backward() elif (command == "move_forward"): self.move_forward() elif (command == "turn_left"): self.turn_left() elif (command == "turn_right"): self.turn_right() elif (command == "hover" and not self.last_command_is_hovering): self.hover() self.last_command_is_hovering = True class ARDrone2(ARDrone): def __init__(self, hd=False): ARDrone.__init__(self, True, hd) ############################################################################### ### Low level AT Commands ############################################################################### def at_ref(seq, takeoff, emergency=False): """ Basic behaviour of the drone: take-off/landing, emergency stop/reset) Parameters: seq -- sequence number takeoff -- True: Takeoff / False: Land emergency -- True: Turn off the engines """ p = 0b10001010101000000000000000000 if takeoff: p += 0b1000000000 if emergency: p += 0b0100000000 at("REF", seq, [p]) def at_pcmd(seq, progressive, lr, fb, vv, va): """ Makes the drone move (translate/rotate). Parameters: seq -- sequence number progressive -- True: enable progressive commands, False: disable (i.e. enable hovering mode) lr -- left-right tilt: float [-1..1] negative: left, positive: right rb -- front-back tilt: float [-1..1] negative: forwards, positive: backwards vv -- vertical speed: float [-1..1] negative: go down, positive: rise va -- angular speed: float [-1..1] negative: spin left, positive: spin right The above float values are a percentage of the maximum speed. """ p = 1 if progressive else 0 at("PCMD", seq, [p, float(lr), float(fb), float(vv), float(va)]) def at_ftrim(seq): """ Tell the drone it's lying horizontally. Parameters: seq -- sequence number """ at("FTRIM", seq, []) def at_zap(seq, stream): """ Selects which video stream to send on the video UDP port. Parameters: seq -- sequence number stream -- Integer: video stream to broadcast """ # FIXME: improve parameters to select the modes directly at("ZAP", seq, [stream]) def at_config(seq, option, value): """Set configuration parameters of the drone.""" at("CONFIG", seq, [str(option), str(value)]) def at_config_ids(seq, value): """Set configuration parameters of the drone.""" at("CONFIG_IDS", seq, value) def at_ctrl(seq, num): """Ask the parrot to drop its configuration file""" at("CTRL", seq, [num, 0]) def at_comwdg(seq): """ Reset communication watchdog. """ # FIXME: no sequence number at("COMWDG", seq, []) def at_aflight(seq, flag): """ Makes the drone fly autonomously. Parameters: seq -- sequence number flag -- Integer: 1: start flight, 0: stop flight """ at("AFLIGHT", seq, [flag]) def at_pwm(seq, m1, m2, m3, m4): """ Sends control values directly to the engines, overriding control loops. Parameters: seq -- sequence number m1 -- front left command m2 -- fright right command m3 -- back right command m4 -- back left command """ # FIXME: what type do mx have? raise NotImplementedError() def at_led(seq, anim, f, d): """ Control the drones LED. Parameters: seq -- sequence number anim -- Integer: animation to play f -- ?: frequence in HZ of the animation d -- Integer: total duration in seconds of the animation """ pass def at_anim(seq, anim, d): """ Makes the drone execute a predefined movement (animation). Parameters: seq -- sequcence number anim -- Integer: animation to play d -- Integer: total duration in sections of the animation """ at("ANIM", seq, [anim, d]) def at(command, seq, params): """ Parameters: command -- the command seq -- the sequence number params -- a list of elements which can be either int, float or string """ param_str = '' for p in params: if type(p) == int: param_str += ",%d" % p elif type(p) == float: param_str += ",%d" % f2i(p) elif type(p) == str: param_str += ',"' + p + '"' msg = "AT*%s=%i%s\r" % (command, seq, param_str) sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.sendto(msg.encode("utf-8"), ("192.168.1.1", ARDRONE_COMMAND_PORT)) def f2i(f): """Interpret IEEE-754 floating-point value as signed integer. Arguments: f -- floating point value """ return struct.unpack('i', struct.pack('f', f))[0] ############################################################################### ### navdata ############################################################################### def decode_navdata(packet): """Decode a navdata packet.""" offset = 0 _ = struct.unpack_from("IIII", packet, offset) drone_state = dict() drone_state['fly_mask'] = _[1] & 1 # FLY MASK : (0) ardrone is landed, (1) ardrone is flying drone_state['video_mask'] = _[1] >> 1 & 1 # VIDEO MASK : (0) video disable, (1) video enable drone_state['vision_mask'] = _[1] >> 2 & 1 # VISION MASK : (0) vision disable, (1) vision enable */ drone_state['control_mask'] = _[1] >> 3 & 1 # CONTROL ALGO (0) euler angles control, (1) angular speed control */ drone_state['altitude_mask'] = _[1] >> 4 & 1 # ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active */ drone_state['user_feedback_start'] = _[1] >> 5 & 1 # USER feedback : Start button state */ drone_state['command_mask'] = _[1] >> 6 & 1 # Control command ACK : (0) None, (1) one received */ drone_state['fw_file_mask'] = _[1] >> 7 & 1 # Firmware file is good (1) */ drone_state['fw_ver_mask'] = _[1] >> 8 & 1 # Firmware update is newer (1) */ drone_state['fw_upd_mask'] = _[1] >> 9 & 1 # Firmware update is ongoing (1) */ drone_state['navdata_demo_mask'] = _[1] >> 10 & 1 # Navdata demo : (0) All navdata, (1) only navdata demo */ drone_state['navdata_bootstrap'] = _[1] >> 11 & 1 # Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */ drone_state['motors_mask'] = _[1] >> 12 & 1 # Motor status : (0) Ok, (1) Motors problem */ drone_state['com_lost_mask'] = _[1] >> 13 & 1 # Communication lost : (1) com problem, (0) Com is ok */ drone_state['vbat_low'] = _[1] >> 15 & 1 # VBat low : (1) too low, (0) Ok */ drone_state['user_el'] = _[1] >> 16 & 1 # User Emergency Landing : (1) User EL is ON, (0) User EL is OFF*/ drone_state['timer_elapsed'] = _[1] >> 17 & 1 # Timer elapsed : (1) elapsed, (0) not elapsed */ drone_state['angles_out_of_range'] = _[1] >> 19 & 1 # Angles : (0) Ok, (1) out of range */ drone_state['ultrasound_mask'] = _[1] >> 21 & 1 # Ultrasonic sensor : (0) Ok, (1) deaf */ drone_state['cutout_mask'] = _[1] >> 22 & 1 # Cutout system detection : (0) Not detected, (1) detected */ drone_state['pic_version_mask'] = _[1] >> 23 & 1 # PIC Version number OK : (0) a bad version number, (1) version number is OK */ drone_state['atcodec_thread_on'] = _[1] >> 24 & 1 # ATCodec thread ON : (0) thread OFF (1) thread ON */ drone_state['navdata_thread_on'] = _[1] >> 25 & 1 # Navdata thread ON : (0) thread OFF (1) thread ON */ drone_state['video_thread_on'] = _[1] >> 26 & 1 # Video thread ON : (0) thread OFF (1) thread ON */ drone_state['acq_thread_on'] = _[1] >> 27 & 1 # Acquisition thread ON : (0) thread OFF (1) thread ON */ drone_state['ctrl_watchdog_mask'] = _[1] >> 28 & 1 # CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */ drone_state['adc_watchdog_mask'] = _[1] >> 29 & 1 # ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */ drone_state['com_watchdog_mask'] = _[1] >> 30 & 1 # Communication Watchdog : (1) com problem, (0) Com is ok */ drone_state['emergency_mask'] = _[1] >> 31 & 1 # Emergency landing : (0) no emergency, (1) emergency */ data = dict() data['drone_state'] = drone_state data['header'] = _[0] data['seq_nr'] = _[2] data['vision_flag'] = _[3] offset += struct.calcsize("IIII") has_flying_information = False while 1: try: id_nr, size = struct.unpack_from("HH", packet, offset) offset += struct.calcsize("HH") except struct.error: break values = [] for i in range(size - struct.calcsize("HH")): values.append(struct.unpack_from("c", packet, offset)[0]) offset += struct.calcsize("c") # navdata_tag_t in navdata-common.h if id_nr == 0: has_flying_information = True values = struct.unpack_from("IIfffifffI", "".join(values)) values = dict(zip(['ctrl_state', 'battery', 'theta', 'phi', 'psi', 'altitude', 'vx', 'vy', 'vz', 'num_frames'], values)) # convert the millidegrees into degrees and round to int, as they try: values['ctrl_state'] = ctrl_state_dict[values['ctrl_state']] except KeyError: values['ctrl_state'] = -1 # are not so precise anyways for i in 'theta', 'phi', 'psi': values[i] = int(values[i] / 1000) data[id_nr] = values return data, has_flying_information if __name__ == "__main__": ''' For testing purpose only ''' import termios import fcntl import os fd = sys.stdin.fileno() oldterm = termios.tcgetattr(fd) newattr = termios.tcgetattr(fd) newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO termios.tcsetattr(fd, termios.TCSANOW, newattr) oldflags = fcntl.fcntl(fd, fcntl.F_GETFL) fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK) drone = ARDrone(is_ar_drone_2=True) import cv2 try: startvideo = True video_waiting = False while 1: time.sleep(.0001) if startvideo: try: cv2.imshow("Drone camera", cv2.cvtColor(drone.get_image(), cv2.COLOR_BGR2RGB)) cv2.waitKey(1) except: if not video_waiting: print("Video will display when ready") video_waiting = True pass try: c = sys.stdin.read(1) c = c.lower() print("Got character", c) if c == 'a': drone.move_left() if c == 'd': drone.move_right() if c == 'w': drone.move_forward() if c == 's': drone.move_backward() if c == ' ': drone.land() if c == '\n': drone.takeoff() if c == 'q': drone.turn_left() if c == 'e': drone.turn_right() if c == '1': drone.move_up() if c == '2': drone.hover() if c == '3': drone.move_down() if c == 't': drone.reset() if c == 'x': drone.hover() if c == 'y': drone.trim() if c == 'i': startvideo = True try: navdata = drone.get_navdata() print('Emergency landing =', navdata['drone_state']['emergency_mask']) print('User emergency landing = ', navdata['drone_state']['user_el']) print('Navdata type= ', navdata['drone_state']['navdata_demo_mask']) print('Altitude= ', navdata[0]['altitude']) print('video enable= ', navdata['drone_state']['video_mask']) print('vision enable= ', navdata['drone_state']['vision_mask']) print('command_mask= ', navdata['drone_state']['command_mask']) except: pass if c == 'j': print("Asking for configuration...") drone.at(at_ctrl, 5) time.sleep(0.5) drone.at(at_ctrl, 4) except IOError: pass finally: termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm) fcntl.fcntl(fd, fcntl.F_SETFL, oldflags) drone.halt()