├── .gitignore ├── .idea ├── .gitignore ├── .name ├── encodings.xml ├── misc.xml ├── modules.xml ├── python-ardrone.iml ├── scopes │ └── scope_settings.xml └── vcs.xml ├── .travis.yml ├── README.md ├── demo ├── demo_pygame.py └── demo_termios.py ├── dev-requirements.txt ├── libardrone ├── __init__.py ├── ar2video.py ├── ardrone.py ├── arnetwork.py ├── arvideo.py ├── at.py ├── h264decoder.py ├── navdata.py └── paveparser.py ├── requirements.txt ├── setup.py ├── test ├── ardrone2_video_example.capture ├── paveparser.output ├── test_h264_decoder.py ├── test_libardrone.py ├── test_losing_connection.py └── test_paveparser.py └── tox.ini /.gitignore: -------------------------------------------------------------------------------- 1 | .tox 2 | *.pyc 3 | *~ 4 | .cache 5 | *.egg-info 6 | -------------------------------------------------------------------------------- /.idea/.gitignore: -------------------------------------------------------------------------------- 1 | workspace.xml 2 | -------------------------------------------------------------------------------- /.idea/.name: -------------------------------------------------------------------------------- 1 | python-ardrone -------------------------------------------------------------------------------- /.idea/encodings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /.idea/python-ardrone.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /.idea/scopes/scope_settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | python: 3 | - "2.7" 4 | notifications: 5 | - email: false 6 | install: 7 | - sudo apt-get update 8 | - sudo apt-get install ffmpeg 9 | - pip install -r requirements.txt 10 | - pip install -r dev-requirements.txt 11 | script: py.test -n 4 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Getting Started: 2 | ---------------- 3 | 4 | ```python 5 | >>> from libardrone import ardrone 6 | >>> drone = ardrone.ARDrone() 7 | >>> # You might need to call drone.reset() before taking off if the drone is in 8 | >>> # emergency mode 9 | >>> drone.takeoff() 10 | >>> drone.land() 11 | >>> drone.halt() 12 | ``` 13 | 14 | Using the drone's function `get_image()` you can get the latest image from the camera. 15 | At present this is in the format of a numpy array with dimensions (width, height, 3) that can be used e.g. in opencv: 16 | 17 | 18 | ```python 19 | >>> import cv2 20 | >>> cv2.namedWindow('image', cv2.WINDOW_AUTOSIZE) 21 | >>> while True: 22 | >>> # get image data as numpy array 23 | >>> img = drone.get_image() 24 | >>> # show image using opencv 25 | >>> cv2.imshow('image', cv2.cvtColor(img, cv2.COLOR_BGR2RGB)) 26 | >>> if cv2.waitKey(1) & 0xFF == ord('q'): # press 'q' to quit 27 | >>> break 28 | ``` 29 | 30 | The drone's property `navdata` contains always the latest navdata. 31 | You can for example get the current battery charge from that: 32 | 33 | ```python 34 | >>> bat = drone.navdata.get(0, dict()).get('battery', 0) 35 | >>> print('Battery: %i%%' % bat) 36 | ``` 37 | 38 | Demo (pygame): 39 | -------------- 40 | 41 | There is also a demo application included which shows the video from the drone 42 | and lets you remote-control the drone with the keyboard (you need pygame for it to work): 43 | 44 | RETURN - takeoff 45 | SPACE - land 46 | BACKSPACE - reset (from emergency - DO NOT USE IN FLIGHT) 47 | w - forward 48 | a - left 49 | s - back 50 | d - right 51 | LEFT/q - turn left 52 | RIGHT/e - turn right 53 | 1,2,...,0 - speed ('1' slowest with speed 0.1, then adding 0.1 for every number until '0' for fastest with 1.0) 54 | UP/DOWN - altitude 55 | r - switch to front facing camera 56 | f - switch to downward facing camera 57 | 58 | Here is a [video] of the library in action: 59 | 60 | [video]: http://youtu.be/2HEV37GbUow 61 | 62 | Repository: 63 | ----------- 64 | 65 | The public repository is located here for the AR.Drone 1.0: 66 | 67 | git://github.com/venthur/python-ardrone.git 68 | 69 | At present the AR.Drone 2.0 has a separate fork here: 70 | 71 | git://github.com/adetaylor/python-ardrone.git 72 | 73 | Requirements: 74 | ------------- 75 | 76 | This software was tested with the following setup: 77 | 78 | * Python 2.7.13 79 | * Unmodified AR.Drone firmware 2.0 80 | 81 | 82 | License: 83 | -------- 84 | 85 | This software is published under the terms of the MIT License: 86 | 87 | http://www.opensource.org/licenses/mit-license.php 88 | -------------------------------------------------------------------------------- /demo/demo_pygame.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2011 Bastian Venthur 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | # THE SOFTWARE. 22 | 23 | 24 | """Demo app for the AR.Drone. 25 | 26 | This simple application allows to control the drone and see the drone's video 27 | stream. 28 | """ 29 | 30 | 31 | import pygame 32 | import pygame.surfarray 33 | 34 | import pygame.transform 35 | from libardrone import ardrone 36 | 37 | def main(): 38 | pygame.init() 39 | W, H = 320, 240 40 | screen = pygame.display.set_mode((W, H)) 41 | drone = ardrone.ARDrone(True) 42 | drone.reset() 43 | clock = pygame.time.Clock() 44 | running = True 45 | while running: 46 | for event in pygame.event.get(): 47 | if event.type == pygame.QUIT: 48 | running = False 49 | elif event.type == pygame.KEYUP: 50 | drone.hover() 51 | elif event.type == pygame.KEYDOWN: 52 | if event.key == pygame.K_ESCAPE: 53 | drone.reset() 54 | running = False 55 | # takeoff / land 56 | elif event.key == pygame.K_RETURN: 57 | drone.takeoff() 58 | elif event.key == pygame.K_SPACE: 59 | drone.land() 60 | # emergency 61 | elif event.key == pygame.K_BACKSPACE: 62 | drone.reset() 63 | # forward / backward 64 | elif event.key == pygame.K_w: 65 | drone.move_forward() 66 | elif event.key == pygame.K_s: 67 | drone.move_backward() 68 | # left / right 69 | elif event.key == pygame.K_a: 70 | drone.move_left() 71 | elif event.key == pygame.K_d: 72 | drone.move_right() 73 | # up / down 74 | elif event.key == pygame.K_UP: 75 | drone.move_up() 76 | elif event.key == pygame.K_DOWN: 77 | drone.move_down() 78 | # turn left / turn right 79 | elif event.key in [pygame.K_LEFT, pygame.K_q]: 80 | drone.turn_left() 81 | elif event.key in [pygame.K_RIGHT, pygame.K_e]: 82 | drone.turn_right() 83 | # speed 84 | elif event.key == pygame.K_1: 85 | drone.speed = 0.1 86 | elif event.key == pygame.K_2: 87 | drone.speed = 0.2 88 | elif event.key == pygame.K_3: 89 | drone.speed = 0.3 90 | elif event.key == pygame.K_4: 91 | drone.speed = 0.4 92 | elif event.key == pygame.K_5: 93 | drone.speed = 0.5 94 | elif event.key == pygame.K_6: 95 | drone.speed = 0.6 96 | elif event.key == pygame.K_7: 97 | drone.speed = 0.7 98 | elif event.key == pygame.K_8: 99 | drone.speed = 0.8 100 | elif event.key == pygame.K_9: 101 | drone.speed = 0.9 102 | elif event.key == pygame.K_0: 103 | drone.speed = 1.0 104 | elif event.key == pygame.K_r: 105 | drone.set_camera_view(True) 106 | elif event.key == pygame.K_f: 107 | drone.set_camera_view(False) 108 | try: 109 | pixelarray = drone.get_image() 110 | if (not pixelarray is None) and pixelarray.any(): 111 | surface = pygame.surfarray.make_surface(pixelarray) 112 | rotsurface = pygame.transform.rotate(surface, 270) 113 | screen.blit(rotsurface, (0, 0)) 114 | # battery status 115 | hud_color = (255, 0, 0) if drone.navdata.get('drone_state', dict()).get('emergency_mask', 1) else (10, 10, 255) 116 | bat = drone.navdata.get(0, dict()).get('battery', 0) 117 | f = pygame.font.Font(None, 20) 118 | hud = f.render('Battery: %i%%' % bat, True, hud_color) 119 | screen.blit(hud, (10, 10)) 120 | except KeyboardInterrupt: 121 | break 122 | except: 123 | pass 124 | 125 | pygame.display.flip() 126 | clock.tick(50) 127 | pygame.display.set_caption("FPS: %.2f" % clock.get_fps()) 128 | 129 | print("Shutting down...") 130 | drone.halt() 131 | print("Ok.") 132 | 133 | if __name__ == '__main__': 134 | main() 135 | -------------------------------------------------------------------------------- /demo/demo_termios.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2011 Bastian Venthur 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | # THE SOFTWARE. 22 | 23 | 24 | """ 25 | For testing purpose only 26 | """ 27 | import termios 28 | import fcntl 29 | import os 30 | import cv2 31 | 32 | def main(): 33 | fd = sys.stdin.fileno() 34 | 35 | oldterm = termios.tcgetattr(fd) 36 | newattr = termios.tcgetattr(fd) 37 | newattr[3] = newattr[3] & ~termios.ICANON & ~termios.ECHO 38 | termios.tcsetattr(fd, termios.TCSANOW, newattr) 39 | 40 | oldflags = fcntl.fcntl(fd, fcntl.F_GETFL) 41 | fcntl.fcntl(fd, fcntl.F_SETFL, oldflags | os.O_NONBLOCK) 42 | 43 | drone = ARDrone(is_ar_drone_2=True) 44 | 45 | try: 46 | startvideo = True 47 | video_waiting = False 48 | while 1: 49 | time.sleep(.0001) 50 | if startvideo: 51 | try: 52 | cv2.imshow("Drone camera", cv2.cvtColor(drone.get_image(), cv2.COLOR_BGR2RGB)) 53 | cv2.waitKey(1) 54 | except: 55 | if not video_waiting: 56 | print("Video will display when ready") 57 | video_waiting = True 58 | pass 59 | 60 | try: 61 | c = sys.stdin.read(1) 62 | c = c.lower() 63 | print("Got character", c) 64 | if c == 'a': 65 | drone.move_left() 66 | if c == 'd': 67 | drone.move_right() 68 | if c == 'w': 69 | drone.move_forward() 70 | if c == 's': 71 | drone.move_backward() 72 | if c == ' ': 73 | drone.land() 74 | if c == '\n': 75 | drone.takeoff() 76 | if c == 'q': 77 | drone.turn_left() 78 | if c == 'e': 79 | drone.turn_right() 80 | if c == '1': 81 | drone.move_up() 82 | if c == '2': 83 | drone.hover() 84 | if c == '3': 85 | drone.move_down() 86 | if c == 't': 87 | drone.reset() 88 | if c == 'x': 89 | drone.hover() 90 | if c == 'y': 91 | drone.trim() 92 | if c == 'i': 93 | startvideo = True 94 | try: 95 | navdata = drone.get_navdata() 96 | 97 | print('Emergency landing =', navdata['drone_state']['emergency_mask']) 98 | print('User emergency landing = ', navdata['drone_state']['user_el']) 99 | print('Navdata type= ', navdata['drone_state']['navdata_demo_mask']) 100 | print('Altitude= ', navdata[0]['altitude']) 101 | print('video enable= ', navdata['drone_state']['video_mask']) 102 | print('vision enable= ', navdata['drone_state']['vision_mask']) 103 | print('command_mask= ', navdata['drone_state']['command_mask']) 104 | except: 105 | pass 106 | 107 | if c == 'j': 108 | print("Asking for configuration...") 109 | drone.at(at_ctrl, 5) 110 | time.sleep(0.5) 111 | drone.at(at_ctrl, 4) 112 | except IOError: 113 | pass 114 | finally: 115 | termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm) 116 | fcntl.fcntl(fd, fcntl.F_SETFL, oldflags) 117 | drone.halt() 118 | 119 | if __name__ == "__main__": 120 | main() 121 | -------------------------------------------------------------------------------- /dev-requirements.txt: -------------------------------------------------------------------------------- 1 | pytest==2.3.5 2 | pytest-xdist==1.8 3 | -------------------------------------------------------------------------------- /libardrone/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2011 Bastian Venthur 2 | # Copyright (c) 2013 Adrian Taylor 3 | # Copyright (c) 2017 Andreas Bresser 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | # THE SOFTWARE. 22 | 23 | 24 | """ 25 | Python library for the AR.Drone. 26 | 27 | This module was tested with Python 2.7.13 and AR.Drone vanilla firmware 1.5.1. 28 | """ 29 | 30 | __author__ = "Bastian Venthur, Adrian Taylor, Andreas Bresser" 31 | 32 | __all__ = ['ar2video', 'ardrone', 'at', 'arnetwork', 'arvideo', 'h264decoder', 'navdata', 'paveparser'] 33 | -------------------------------------------------------------------------------- /libardrone/ar2video.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2011 Bastian Venthur 4 | # Copyright (c) 2013 Adrian Taylor 5 | # 6 | # Permission is hereby granted, free of charge, to any person obtaining a copy 7 | # of this software and associated documentation files (the "Software"), to deal 8 | # in the Software without restriction, including without limitation the rights 9 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | # copies of the Software, and to permit persons to whom the Software is 11 | # furnished to do so, subject to the following conditions: 12 | # 13 | # The above copyright notice and this permission notice shall be included in 14 | # all copies or substantial portions of the Software. 15 | # 16 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | # THE SOFTWARE. 23 | 24 | 25 | """ 26 | Video decoding for the AR.Drone 2.0. 27 | 28 | This is just H.264 encapsulated in a funny way. 29 | """ 30 | 31 | import h264decoder 32 | import paveparser 33 | 34 | 35 | class ARVideo2(object): 36 | def __init__(self, drone, debug=False): 37 | h264 = h264decoder.H264Decoder(self, drone.image_shape) 38 | self.paveparser = paveparser.PaVEParser(h264) 39 | self._drone = drone 40 | 41 | """ 42 | Called by the H264 decoder when there's an image ready 43 | """ 44 | def image_ready(self, image): 45 | self._drone.set_image(image) 46 | 47 | def write(self, data): 48 | self.paveparser.write(data) 49 | -------------------------------------------------------------------------------- /libardrone/ardrone.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2011 Bastian Venthur 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in 11 | # all copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | # THE SOFTWARE. 20 | 21 | 22 | """ 23 | Python library for the AR.Drone. 24 | 25 | V.1 This module was tested with Python 2.6.6 and AR.Drone vanilla firmware 1.5.1. 26 | V.2.alpha 27 | """ 28 | 29 | import logging 30 | import sys 31 | import threading 32 | import multiprocessing 33 | import time 34 | import numpy as np 35 | 36 | from . import arnetwork 37 | from . import at 38 | 39 | SESSION_ID = "943dac23" 40 | USER_ID = "36355d78" 41 | APP_ID = "21d958e4" 42 | 43 | DEBUG = False 44 | 45 | 46 | class ARDrone(object): 47 | """ARDrone Class. 48 | 49 | Instanciate this class to control your drone and receive decoded video and 50 | navdata. 51 | Possible value for video codec (drone2): 52 | NULL_CODEC = 0, 53 | UVLC_CODEC = 0x20, // codec_type value is used for START_CODE 54 | P264_CODEC = 0x40, 55 | MP4_360P_CODEC = 0x80, 56 | H264_360P_CODEC = 0x81, 57 | MP4_360P_H264_720P_CODEC = 0x82, 58 | H264_720P_CODEC = 0x83, 59 | MP4_360P_SLRS_CODEC = 0x84, 60 | H264_360P_SLRS_CODEC = 0x85, 61 | H264_720P_SLRS_CODEC = 0x86, 62 | H264_AUTO_RESIZE_CODEC = 0x87, // resolution is automatically adjusted according to bitrate 63 | MP4_360P_H264_360P_CODEC = 0x88, 64 | """ 65 | 66 | def __init__(self, is_ar_drone_2=False, hd=False, debug=DEBUG): 67 | 68 | self.seq_nr = 1 69 | self.timer_t = 0.2 70 | self.com_watchdog_timer = threading.Timer(self.timer_t, self.commwdg) 71 | self.lock = threading.Lock() 72 | self.speed = 0.2 73 | self.hd = hd 74 | self.debug = debug 75 | if (self.hd): 76 | self.image_shape = (720, 1280, 3) 77 | else: 78 | self.image_shape = (360, 640, 3) 79 | 80 | time.sleep(0.5) 81 | self.config_ids_string = [SESSION_ID, USER_ID, APP_ID] 82 | self.configure_multisession(SESSION_ID, USER_ID, APP_ID, self.config_ids_string) 83 | self.set_session_id (self.config_ids_string, SESSION_ID) 84 | time.sleep(0.5) 85 | self.set_profile_id(self.config_ids_string, USER_ID) 86 | time.sleep(0.5) 87 | self.set_app_id(self.config_ids_string, APP_ID) 88 | time.sleep(0.5) 89 | self.set_video_bitrate_control_mode(self.config_ids_string, "1") 90 | time.sleep(0.5) 91 | self.set_video_bitrate(self.config_ids_string, "10000") 92 | time.sleep(0.5) 93 | self.set_max_bitrate(self.config_ids_string, "10000") 94 | time.sleep(0.5) 95 | self.set_fps(self.config_ids_string, "30") 96 | time.sleep(0.5) 97 | if (self.hd): 98 | self.set_video_codec(self.config_ids_string, 0x83) 99 | else: 100 | self.set_video_codec(self.config_ids_string, 0x81) 101 | 102 | self.last_command_is_hovering = True 103 | self.com_pipe, com_pipe_other = multiprocessing.Pipe() 104 | 105 | self.navdata = dict() 106 | 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])) 107 | 108 | self.network_process = arnetwork.ARDroneNetworkProcess(com_pipe_other, is_ar_drone_2, self) 109 | self.network_process.start() 110 | 111 | self.image = np.zeros(self.image_shape, np.uint8) 112 | self.time = 0 113 | 114 | self.last_command_is_hovering = True 115 | 116 | time.sleep(1.0) 117 | 118 | self.at(at.at_config_ids , self.config_ids_string) 119 | self.at(at.at_config, "general:navdata_demo", "TRUE") 120 | 121 | 122 | def takeoff(self): 123 | """Make the drone takeoff.""" 124 | self.at(at.at_ftrim) 125 | self.at(at.at_config, "control:altitude_max", "20000") 126 | self.at(at.at_ref, True) 127 | 128 | def land(self): 129 | """Make the drone land.""" 130 | self.at(at.at_ref, False) 131 | 132 | def hover(self): 133 | """Make the drone hover.""" 134 | self.at(at.at_pcmd, False, 0, 0, 0, 0) 135 | 136 | def move_left(self): 137 | """Make the drone move left.""" 138 | self.at(at.at_pcmd, True, -self.speed, 0, 0, 0) 139 | 140 | def move_right(self): 141 | """Make the drone move right.""" 142 | self.at(at.at_pcmd, True, self.speed, 0, 0, 0) 143 | 144 | def move_up(self): 145 | """Make the drone rise upwards.""" 146 | self.at(at.at_pcmd, True, 0, 0, self.speed, 0) 147 | 148 | def move_down(self): 149 | """Make the drone decent downwards.""" 150 | self.at(at.at_pcmd, True, 0, 0, -self.speed, 0) 151 | 152 | def move_forward(self): 153 | """Make the drone move forward.""" 154 | self.at(at.at_pcmd, True, 0, -self.speed, 0, 0) 155 | 156 | def move_backward(self): 157 | """Make the drone move backwards.""" 158 | self.at(at.at_pcmd, True, 0, self.speed, 0, 0) 159 | 160 | def turn_left(self): 161 | """Make the drone rotate left.""" 162 | self.at(at.at_pcmd, True, 0, 0, 0, -self.speed) 163 | 164 | def turn_right(self): 165 | """Make the drone rotate right.""" 166 | self.at(at.at_pcmd, True, 0, 0, 0, self.speed) 167 | 168 | def reset(self): 169 | """Toggle the drone's emergency state.""" 170 | # Enter emergency mode 171 | self.at(at.at_ref, False, True) 172 | self.at(at.at_ref, False, False) 173 | # Leave emergency mode 174 | self.at(at.at_ref, False, True) 175 | 176 | def trim(self): 177 | """Flat trim the drone.""" 178 | self.at(at.at_ftrim) 179 | 180 | def set_speed(self, speed): 181 | """Set the drone's speed. 182 | 183 | Valid values are floats from [0..1] 184 | """ 185 | self.speed = speed 186 | 187 | def set_camera_view(self, downward): 188 | """ 189 | Set which video camera is used. If 'downward' is true, 190 | downward camera will be viewed - otherwise frontwards. 191 | """ 192 | channel = None 193 | if downward: 194 | channel = 0 195 | else: 196 | channel = 1 197 | self.set_video_channel(self.config_ids_string, channel) 198 | 199 | def at(self, cmd, *args, **kwargs): 200 | """Wrapper for the low level at commands. 201 | 202 | This method takes care that the sequence number is increased after each 203 | at command and the watchdog timer is started to make sure the drone 204 | receives a command at least every second. 205 | """ 206 | self.lock.acquire() 207 | self.com_watchdog_timer.cancel() 208 | cmd(self.seq_nr, *args, **kwargs) 209 | self.seq_nr += 1 210 | self.com_watchdog_timer = threading.Timer(self.timer_t, self.commwdg) 211 | self.com_watchdog_timer.start() 212 | self.lock.release() 213 | 214 | def configure_multisession(self, session_id, user_id, app_id, config_ids_string): 215 | self.at(at.at_config, "custom:session_id", session_id) 216 | self.at(at.at_config, "custom:profile_id", user_id) 217 | self.at(at.at_config, "custom:application_id", app_id) 218 | 219 | def set_session_id (self, config_ids_string, session_id): 220 | self.at(at.at_config_ids , config_ids_string) 221 | self.at(at.at_config, "custom:session_id", session_id) 222 | 223 | def set_profile_id (self, config_ids_string, profile_id): 224 | self.at(at.at_config_ids , config_ids_string) 225 | self.at(at.at_config, "custom:profile_id", profile_id) 226 | 227 | def set_app_id (self, config_ids_string, app_id): 228 | self.at(at.at_config_ids , config_ids_string) 229 | self.at(at.at_config, "custom:application_id", app_id) 230 | 231 | def set_video_bitrate_control_mode (self, config_ids_string, mode): 232 | self.at(at.at_config_ids , config_ids_string) 233 | self.at(at.at_config, "video:bitrate_control_mode", mode) 234 | 235 | def set_video_bitrate (self, config_ids_string, bitrate): 236 | self.at(at.at_config_ids , config_ids_string) 237 | self.at(at.at_config, "video:bitrate", bitrate) 238 | 239 | def set_video_channel(self, config_ids_string, channel): 240 | self.at(at.at_config_ids , config_ids_string) 241 | self.at(at.at_config, "video:video_channel", channel) 242 | 243 | def set_max_bitrate(self, config_ids_string, max_bitrate): 244 | self.at(at.at_config_ids , config_ids_string) 245 | self.at(at.at_config, "video:max_bitrate", max_bitrate) 246 | 247 | def set_fps (self, config_ids_string, fps): 248 | self.at(at.at_config_ids , config_ids_string) 249 | self.at(at.at_config, "video:codec_fps", fps) 250 | 251 | def set_video_codec (self, config_ids_string, codec): 252 | self.at(at.at_config_ids , config_ids_string) 253 | self.at(at.at_config, "video:video_codec", codec) 254 | 255 | def commwdg(self): 256 | """Communication watchdog signal. 257 | 258 | This needs to be send regulary to keep the communication w/ the drone 259 | alive. 260 | """ 261 | self.at(at.at_comwdg) 262 | 263 | def halt(self): 264 | """Shutdown the drone. 265 | 266 | This method does not land or halt the actual drone, but the 267 | communication with the drone. You should call it at the end of your 268 | application to close all sockets, pipes, processes and threads related 269 | with this object. 270 | """ 271 | self.lock.acquire() 272 | self.com_watchdog_timer.cancel() 273 | self.com_pipe.send('die!') 274 | self.network_process.terminate() 275 | 276 | #self.network_process.join() 277 | self.lock.release() 278 | 279 | def get_image(self): 280 | _im = np.copy(self.image) 281 | return _im 282 | 283 | def get_navdata(self): 284 | return self.navdata 285 | 286 | def set_navdata(self, navdata): 287 | self.navdata = navdata 288 | self.get_navdata() 289 | 290 | def set_image(self, image): 291 | if (image.shape == self.image_shape): 292 | self.image = image 293 | self.image = image 294 | 295 | def apply_command(self, command): 296 | available_commands = ["emergency", 297 | "land", "takeoff", "move_left", "move_right", "move_down", "move_up", 298 | "move_backward", "move_forward", "turn_left", "turn_right", "hover"] 299 | if command not in available_commands: 300 | logging.error("Command %s is not a recognized command" % command) 301 | 302 | if command != "hover": 303 | self.last_command_is_hovering = False 304 | 305 | if (command == "emergency"): 306 | self.reset() 307 | elif (command == "land"): 308 | self.land() 309 | self.last_command_is_hovering = True 310 | elif (command == "takeoff"): 311 | self.takeoff() 312 | self.last_command_is_hovering = True 313 | elif (command == "move_left"): 314 | self.move_left() 315 | elif (command == "move_right"): 316 | self.move_right() 317 | elif (command == "move_down"): 318 | self.move_down() 319 | elif (command == "move_up"): 320 | self.move_up() 321 | elif (command == "move_backward"): 322 | self.move_backward() 323 | elif (command == "move_forward"): 324 | self.move_forward() 325 | elif (command == "turn_left"): 326 | self.turn_left() 327 | elif (command == "turn_right"): 328 | self.turn_right() 329 | elif (command == "hover" and not self.last_command_is_hovering): 330 | self.hover() 331 | self.last_command_is_hovering = True 332 | 333 | class ARDrone2(ARDrone): 334 | def __init__(self, hd=False, debug=DEBUG): 335 | ARDrone.__init__(self, True, hd, debug) 336 | -------------------------------------------------------------------------------- /libardrone/arnetwork.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2011 Bastian Venthur 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in 11 | # all copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | # THE SOFTWARE. 20 | import logging 21 | 22 | """ 23 | This module provides access to the data provided by the AR.Drone. 24 | """ 25 | import threading 26 | import select 27 | import socket 28 | import multiprocessing 29 | 30 | from . import navdata 31 | 32 | ARDRONE_NAVDATA_PORT = 5554 33 | ARDRONE_VIDEO_PORT = 5555 34 | ARDRONE_CONTROL_PORT = 5559 35 | 36 | class ARDroneNetworkProcess(threading.Thread): 37 | """ARDrone Network Process. 38 | 39 | This process collects data from the video and navdata port, converts the 40 | data and sends it to the IPCThread. 41 | """ 42 | 43 | def __init__(self, com_pipe, is_ar_drone_2, drone): 44 | threading.Thread.__init__(self) 45 | self._drone = drone 46 | self.com_pipe = com_pipe 47 | self.is_ar_drone_2 = is_ar_drone_2 48 | self.stopping = False 49 | if is_ar_drone_2: 50 | from . import ar2video 51 | self.ar2video = ar2video.ARVideo2(self._drone, drone.debug) 52 | else: 53 | from . import arvideo 54 | 55 | def run(self): 56 | 57 | def _connect(): 58 | logging.warn('Connection to ardrone') 59 | if self.is_ar_drone_2: 60 | video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 61 | video_socket.connect(('192.168.1.1', ARDRONE_VIDEO_PORT)) 62 | video_socket.setblocking(0) 63 | else: 64 | video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 65 | video_socket.setblocking(0) 66 | video_socket.bind(('', ARDRONE_VIDEO_PORT)) 67 | video_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', ARDRONE_VIDEO_PORT)) 68 | 69 | nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) 70 | nav_socket.setblocking(0) 71 | nav_socket.bind(('', ARDRONE_NAVDATA_PORT)) 72 | nav_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', ARDRONE_NAVDATA_PORT)) 73 | 74 | control_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 75 | control_socket.connect(('192.168.1.1', ARDRONE_CONTROL_PORT)) 76 | control_socket.setblocking(0) 77 | logging.warn('Connection established') 78 | return video_socket, nav_socket, control_socket 79 | 80 | def _disconnect(video_socket, nav_socket, control_socket): 81 | logging.warn('Disconnection to ardrone streams') 82 | video_socket.close() 83 | nav_socket.close() 84 | control_socket.close() 85 | 86 | video_socket, nav_socket, control_socket = _connect() 87 | 88 | self.stopping = False 89 | connection_lost = 1 90 | reconnection_needed = False 91 | while not self.stopping: 92 | if reconnection_needed: 93 | _disconnect(video_socket, nav_socket, control_socket) 94 | video_socket, nav_socket, control_socket = _connect() 95 | reconnection_needed = False 96 | inputready, outputready, exceptready = select.select([nav_socket, video_socket, self.com_pipe, control_socket], [], [], 1.) 97 | if len(inputready) == 0: 98 | connection_lost += 1 99 | reconnection_needed = True 100 | for i in inputready: 101 | if i == video_socket: 102 | while 1: 103 | try: 104 | data = video_socket.recv(65536) 105 | if self.is_ar_drone_2: 106 | self.ar2video.write(data) 107 | except IOError: 108 | # we consumed every packet from the socket and 109 | # continue with the last one 110 | break 111 | # Sending is taken care of by the decoder 112 | if not self.is_ar_drone_2: 113 | w, h, image, t = arvideo.read_picture(data) 114 | self._drone.set_image(image) 115 | elif i == nav_socket: 116 | while 1: 117 | try: 118 | data = nav_socket.recv(500) 119 | except IOError: 120 | # we consumed every packet from the socket and 121 | # continue with the last one 122 | break 123 | nav_data, has_information = navdata.decode_navdata(data) 124 | if (has_information): 125 | self._drone.set_navdata(nav_data) 126 | elif i == self.com_pipe: 127 | _ = self.com_pipe.recv() 128 | self.stopping = True 129 | break 130 | elif i == control_socket: 131 | reconnection_needed = False 132 | while not reconnection_needed: 133 | try: 134 | data = control_socket.recv(65536) 135 | if len(data) == 0: 136 | logging.warning('Received an empty packet on control socket') 137 | reconnection_needed = True 138 | else: 139 | logging.warning("Control Socket says : %s", data) 140 | except IOError: 141 | break 142 | _disconnect(video_socket, nav_socket, control_socket) 143 | 144 | def terminate(self): 145 | self.stopping = True 146 | -------------------------------------------------------------------------------- /libardrone/arvideo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Copyright (c) 2011 Bastian Venthur 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | # THE SOFTWARE. 22 | 23 | 24 | """ 25 | Video decoding for the AR.Drone. 26 | 27 | This library uses psyco to speed-up the decoding process. It is however written 28 | in a way that it works also without psyco installed. On the author's 29 | development machine the speed up is from 2FPS w/o psyco to > 20 FPS w/ psyco. 30 | """ 31 | 32 | 33 | import array 34 | import cProfile 35 | import datetime 36 | import struct 37 | import sys 38 | 39 | try: 40 | import psyco 41 | except ImportError: 42 | print "Please install psyco for better video decoding performance." 43 | 44 | 45 | # from zig-zag back to normal 46 | ZIG_ZAG_POSITIONS = array.array('B', 47 | ( 0, 1, 8, 16, 9, 2, 3, 10, 48 | 17, 24, 32, 25, 18, 11, 4, 5, 49 | 12, 19, 26, 33, 40, 48, 41, 34, 50 | 27, 20, 13, 6, 7, 14, 21, 28, 51 | 35, 42, 49, 56, 57, 50, 43, 36, 52 | 29, 22, 15, 23, 30, 37, 44, 51, 53 | 58, 59, 52, 45, 38, 31, 39, 46, 54 | 53, 60, 61, 54, 47, 55, 62, 63)) 55 | 56 | # Inverse quantization 57 | IQUANT_TAB = array.array('B', 58 | ( 3, 5, 7, 9, 11, 13, 15, 17, 59 | 5, 7, 9, 11, 13, 15, 17, 19, 60 | 7, 9, 11, 13, 15, 17, 19, 21, 61 | 9, 11, 13, 15, 17, 19, 21, 23, 62 | 11, 13, 15, 17, 19, 21, 23, 25, 63 | 13, 15, 17, 19, 21, 23, 25, 27, 64 | 15, 17, 19, 21, 23, 25, 27, 29, 65 | 17, 19, 21, 23, 25, 27, 29, 31)) 66 | 67 | # Used for upscaling the 8x8 b- and r-blocks to 16x16 68 | SCALE_TAB = array.array('B', 69 | ( 0, 0, 1, 1, 2, 2, 3, 3, 70 | 0, 0, 1, 1, 2, 2, 3, 3, 71 | 8, 8, 9, 9, 10, 10, 11, 11, 72 | 8, 8, 9, 9, 10, 10, 11, 11, 73 | 16, 16, 17, 17, 18, 18, 19, 19, 74 | 16, 16, 17, 17, 18, 18, 19, 19, 75 | 24, 24, 25, 25, 26, 26, 27, 27, 76 | 24, 24, 25, 25, 26, 26, 27, 27, 77 | 78 | 4, 4, 5, 5, 6, 6, 7, 7, 79 | 4, 4, 5, 5, 6, 6, 7, 7, 80 | 12, 12, 13, 13, 14, 14, 15, 15, 81 | 12, 12, 13, 13, 14, 14, 15, 15, 82 | 20, 20, 21, 21, 22, 22, 23, 23, 83 | 20, 20, 21, 21, 22, 22, 23, 23, 84 | 28, 28, 29, 29, 30, 30, 31, 31, 85 | 28, 28, 29, 29, 30, 30, 31, 31, 86 | 87 | 32, 32, 33, 33, 34, 34, 35, 35, 88 | 32, 32, 33, 33, 34, 34, 35, 35, 89 | 40, 40, 41, 41, 42, 42, 43, 43, 90 | 40, 40, 41, 41, 42, 42, 43, 43, 91 | 48, 48, 49, 49, 50, 50, 51, 51, 92 | 48, 48, 49, 49, 50, 50, 51, 51, 93 | 56, 56, 57, 57, 58, 58, 59, 59, 94 | 56, 56, 57, 57, 58, 58, 59, 59, 95 | 96 | 36, 36, 37, 37, 38, 38, 39, 39, 97 | 36, 36, 37, 37, 38, 38, 39, 39, 98 | 44, 44, 45, 45, 46, 46, 47, 47, 99 | 44, 44, 45, 45, 46, 46, 47, 47, 100 | 52, 52, 53, 53, 54, 54, 55, 55, 101 | 52, 52, 53, 53, 54, 54, 55, 55, 102 | 60, 60, 61, 61, 62, 62, 63, 63, 103 | 60, 60, 61, 61, 62, 62, 63, 63)) 104 | 105 | # Count leading zeros look up table 106 | CLZLUT = array.array('B', 107 | (8, 7, 6, 6, 5, 5, 5, 5, 4, 4, 4, 4, 4, 4, 4, 4, 108 | 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 109 | 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 110 | 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 111 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 112 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 113 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 114 | 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 115 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 116 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 117 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 118 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 119 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 120 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 121 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 122 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)) 123 | 124 | # Map pixels from four 8x8 blocks to one 16x16 125 | MB_TO_GOB_MAP = array.array('B', 126 | [ 0, 1, 2, 3, 4, 5, 6, 7, 127 | 16, 17, 18, 19, 20, 21, 22, 23, 128 | 32, 33, 34, 35, 36, 37, 38, 39, 129 | 48, 49, 50, 51, 52, 53, 54, 55, 130 | 64, 65, 66, 67, 68, 69, 70, 71, 131 | 80, 81, 82, 83, 84, 85, 86, 87, 132 | 96, 97, 98, 99, 100, 101, 102, 103, 133 | 112, 113, 114, 115, 116, 117, 118, 119, 134 | 8, 9, 10, 11, 12, 13, 14, 15, 135 | 24, 25, 26, 27, 28, 29, 30, 31, 136 | 40, 41, 42, 43, 44, 45, 46, 47, 137 | 56, 57, 58, 59, 60, 61, 62, 63, 138 | 72, 73, 74, 75, 76, 77, 78, 79, 139 | 88, 89, 90, 91, 92, 93, 94, 95, 140 | 104, 105, 106, 107, 108, 109, 110, 111, 141 | 120, 121, 122, 123, 124, 125, 126, 127, 142 | 128, 129, 130, 131, 132, 133, 134, 135, 143 | 144, 145, 146, 147, 148, 149, 150, 151, 144 | 160, 161, 162, 163, 164, 165, 166, 167, 145 | 176, 177, 178, 179, 180, 181, 182, 183, 146 | 192, 193, 194, 195, 196, 197, 198, 199, 147 | 208, 209, 210, 211, 212, 213, 214, 215, 148 | 224, 225, 226, 227, 228, 229, 230, 231, 149 | 240, 241, 242, 243, 244, 245, 246, 247, 150 | 136, 137, 138, 139, 140, 141, 142, 143, 151 | 152, 153, 154, 155, 156, 157, 158, 159, 152 | 168, 169, 170, 171, 172, 173, 174, 175, 153 | 184, 185, 186, 187, 188, 189, 190, 191, 154 | 200, 201, 202, 203, 204, 205, 206, 207, 155 | 216, 217, 218, 219, 220, 221, 222, 223, 156 | 232, 233, 234, 235, 236, 237, 238, 239, 157 | 248, 249, 250, 251, 252, 253, 254, 255]) 158 | MB_ROW_MAP = array.array('B', [i / 16 for i in MB_TO_GOB_MAP]) 159 | MB_COL_MAP = array.array('B', [i % 16 for i in MB_TO_GOB_MAP]) 160 | 161 | # An array of zeros. It is much faster to take the zeros from here than to 162 | # generate a new list when needed. 163 | ZEROS = array.array('i', [0 for i in range(256)]) 164 | 165 | # Constants needed for the inverse discrete cosine transform. 166 | FIX_0_298631336 = 2446 167 | FIX_0_390180644 = 3196 168 | FIX_0_541196100 = 4433 169 | FIX_0_765366865 = 6270 170 | FIX_0_899976223 = 7373 171 | FIX_1_175875602 = 9633 172 | FIX_1_501321110 = 12299 173 | FIX_1_847759065 = 15137 174 | FIX_1_961570560 = 16069 175 | FIX_2_053119869 = 16819 176 | FIX_2_562915447 = 20995 177 | FIX_3_072711026 = 25172 178 | CONST_BITS = 13 179 | PASS1_BITS = 1 180 | F1 = CONST_BITS - PASS1_BITS - 1 181 | F2 = CONST_BITS - PASS1_BITS 182 | F3 = CONST_BITS + PASS1_BITS + 3 183 | 184 | # tuning parameter for get_block 185 | TRIES = 16 186 | MASK = 2**(TRIES*32)-1 187 | SHIFT = 32*(TRIES-1) 188 | 189 | 190 | def _first_half(data): 191 | """Helper function used to precompute the zero values in a 12 bit datum. 192 | """ 193 | # data has to be 12 bits wide 194 | streamlen = 0 195 | # count the zeros 196 | zerocount = CLZLUT[data >> 4]; 197 | data = (data << (zerocount + 1)) & 0b111111111111 198 | streamlen += zerocount + 1 199 | # get number of remaining bits to read 200 | toread = 0 if zerocount <= 1 else zerocount - 1 201 | additional = data >> (12 - toread) 202 | data = (data << toread) & 0b111111111111 203 | streamlen += toread 204 | # add as many zeros to out_list as indicated by additional bits 205 | # if zerocount is 0, tmp = 0 else the 1 merged with additional bits 206 | tmp = 0 if zerocount == 0 else (1 << toread) | additional 207 | return [streamlen, tmp] 208 | 209 | 210 | def _second_half(data): 211 | """Helper function to precompute the nonzeror values in a 15 bit datum. 212 | """ 213 | # data has to be 15 bits wide 214 | streamlen = 0 215 | zerocount = CLZLUT[data >> 7] 216 | data = (data << (zerocount + 1)) & 0b111111111111111 217 | streamlen += zerocount + 1 218 | # 01 == EOB 219 | eob = False 220 | if zerocount == 1: 221 | eob = True 222 | return [streamlen, None, eob] 223 | # get number of remaining bits to read 224 | toread = 0 if zerocount == 0 else zerocount - 1 225 | additional = data >> (15 - toread) 226 | data = (data << toread) & 0b111111111111111 227 | streamlen += toread 228 | tmp = (1 << toread) | additional 229 | # get one more bit for the sign 230 | tmp = -tmp if data >> (15 - 1) else tmp 231 | tmp = int(tmp) 232 | streamlen += 1 233 | return [streamlen, tmp, eob] 234 | 235 | 236 | # Precompute all 12 and 15 bit values for the entropy decoding process 237 | FH = [_first_half(i) for i in range(2**12)] 238 | SH = [_second_half(i) for i in range(2**15)] 239 | 240 | 241 | class BitReader(object): 242 | """Bitreader. Given a stream of data, it allows to read it bitwise.""" 243 | 244 | def __init__(self, packet): 245 | self.packet = packet 246 | self.offset = 0 247 | self.bits_left = 0 248 | self.chunk = 0 249 | self.read_bits = 0 250 | 251 | def read(self, nbits, consume=True): 252 | """Read nbits and return the integervalue of the read bits. 253 | 254 | If consume is False, it behaves like a 'peek' method (ie it reads the 255 | bits but does not consume them. 256 | """ 257 | # Read enough bits into chunk so we have at least nbits available 258 | while nbits > self.bits_left: 259 | try: 260 | self.chunk = (self.chunk << 32) | struct.unpack_from('> shift 268 | if consume: 269 | self.chunk -= res << shift 270 | self.bits_left -= nbits 271 | self.read_bits += nbits 272 | return res 273 | 274 | def align(self): 275 | """Byte align the data stream.""" 276 | shift = (8 - self.read_bits) % 8 277 | self.read(shift) 278 | 279 | 280 | def inverse_dct(block): 281 | """Inverse discrete cosine transform. 282 | """ 283 | workspace = ZEROS[0:64] 284 | data = ZEROS[0:64] 285 | for pointer in range(8): 286 | if (block[pointer + 8] == 0 and block[pointer + 16] == 0 and 287 | block[pointer + 24] == 0 and block[pointer + 32] == 0 and 288 | block[pointer + 40] == 0 and block[pointer + 48] == 0 and 289 | block[pointer + 56] == 0): 290 | dcval = block[pointer] << PASS1_BITS 291 | for i in range(8): 292 | workspace[pointer + i*8] = dcval 293 | continue 294 | 295 | z2 = block[pointer + 16] 296 | z3 = block[pointer + 48] 297 | z1 = (z2 + z3) * FIX_0_541196100 298 | tmp2 = z1 + z3 * -FIX_1_847759065 299 | tmp3 = z1 + z2 * FIX_0_765366865 300 | z2 = block[pointer] 301 | z3 = block[pointer + 32] 302 | tmp0 = (z2 + z3) << CONST_BITS 303 | tmp1 = (z2 - z3) << CONST_BITS 304 | tmp10 = tmp0 + tmp3 305 | tmp13 = tmp0 - tmp3 306 | tmp11 = tmp1 + tmp2 307 | tmp12 = tmp1 - tmp2 308 | tmp0 = block[pointer + 56] 309 | tmp1 = block[pointer + 40] 310 | tmp2 = block[pointer + 24] 311 | tmp3 = block[pointer + 8] 312 | z1 = tmp0 + tmp3 313 | z2 = tmp1 + tmp2 314 | z3 = tmp0 + tmp2 315 | z4 = tmp1 + tmp3 316 | z5 = (z3 + z4) * FIX_1_175875602 317 | tmp0 *= FIX_0_298631336 318 | tmp1 *= FIX_2_053119869 319 | tmp2 *= FIX_3_072711026 320 | tmp3 *= FIX_1_501321110 321 | z1 *= -FIX_0_899976223 322 | z2 *= -FIX_2_562915447 323 | z3 *= -FIX_1_961570560 324 | z4 *= -FIX_0_390180644 325 | z3 += z5 326 | z4 += z5 327 | tmp0 += z1 + z3 328 | tmp1 += z2 + z4 329 | tmp2 += z2 + z3 330 | tmp3 += z1 + z4 331 | workspace[pointer + 0] = ((tmp10 + tmp3 + (1 << F1)) >> F2) 332 | workspace[pointer + 56] = ((tmp10 - tmp3 + (1 << F1)) >> F2) 333 | workspace[pointer + 8] = ((tmp11 + tmp2 + (1 << F1)) >> F2) 334 | workspace[pointer + 48] = ((tmp11 - tmp2 + (1 << F1)) >> F2) 335 | workspace[pointer + 16] = ((tmp12 + tmp1 + (1 << F1)) >> F2) 336 | workspace[pointer + 40] = ((tmp12 - tmp1 + (1 << F1)) >> F2) 337 | workspace[pointer + 24] = ((tmp13 + tmp0 + (1 << F1)) >> F2) 338 | workspace[pointer + 32] = ((tmp13 - tmp0 + (1 << F1)) >> F2) 339 | 340 | for pointer in range(0, 64, 8): 341 | z2 = workspace[pointer + 2] 342 | z3 = workspace[pointer + 6] 343 | z1 = (z2 + z3) * FIX_0_541196100 344 | tmp2 = z1 + z3 * -FIX_1_847759065 345 | tmp3 = z1 + z2 * FIX_0_765366865 346 | tmp0 = (workspace[pointer] + workspace[pointer + 4]) << CONST_BITS 347 | tmp1 = (workspace[pointer] - workspace[pointer + 4]) << CONST_BITS 348 | tmp10 = tmp0 + tmp3 349 | tmp13 = tmp0 - tmp3 350 | tmp11 = tmp1 + tmp2 351 | tmp12 = tmp1 - tmp2 352 | tmp0 = workspace[pointer + 7] 353 | tmp1 = workspace[pointer + 5] 354 | tmp2 = workspace[pointer + 3] 355 | tmp3 = workspace[pointer + 1] 356 | z1 = tmp0 + tmp3 357 | z2 = tmp1 + tmp2 358 | z3 = tmp0 + tmp2 359 | z4 = tmp1 + tmp3 360 | z5 = (z3 + z4) * FIX_1_175875602 361 | tmp0 *= FIX_0_298631336 362 | tmp1 *= FIX_2_053119869 363 | tmp2 *= FIX_3_072711026 364 | tmp3 *= FIX_1_501321110 365 | z1 *= -FIX_0_899976223 366 | z2 *= -FIX_2_562915447 367 | z3 *= -FIX_1_961570560 368 | z4 *= -FIX_0_390180644 369 | z3 += z5 370 | z4 += z5 371 | tmp0 += z1 + z3 372 | tmp1 += z2 + z4 373 | tmp2 += z2 + z3 374 | tmp3 += z1 + z4 375 | data[pointer + 0] = (tmp10 + tmp3) >> F3 376 | data[pointer + 7] = (tmp10 - tmp3) >> F3 377 | data[pointer + 1] = (tmp11 + tmp2) >> F3 378 | data[pointer + 6] = (tmp11 - tmp2) >> F3 379 | data[pointer + 2] = (tmp12 + tmp1) >> F3 380 | data[pointer + 5] = (tmp12 - tmp1) >> F3 381 | data[pointer + 3] = (tmp13 + tmp0) >> F3 382 | data[pointer + 4] = (tmp13 - tmp0) >> F3 383 | 384 | return data 385 | 386 | 387 | def get_pheader(bitreader): 388 | """Read the picture header. 389 | 390 | Returns the width and height of the image. 391 | """ 392 | bitreader.align() 393 | psc = bitreader.read(22) 394 | assert(psc == 0b0000000000000000100000) 395 | pformat = bitreader.read(2) 396 | assert(pformat != 0b00) 397 | if pformat == 1: 398 | # CIF 399 | width, height = 88, 72 400 | else: 401 | # VGA 402 | width, height = 160, 120 403 | presolution = bitreader.read(3) 404 | assert(presolution != 0b000) 405 | # double resolution presolution-1 times 406 | width = width << presolution - 1 407 | height = height << presolution - 1 408 | #print "width/height:", width, height 409 | ptype = bitreader.read(3) 410 | pquant = bitreader.read(5) 411 | pframe = bitreader.read(32) 412 | return width, height 413 | 414 | 415 | def get_mb(bitreader, picture, width, offset): 416 | """Get macro block. 417 | 418 | This method does not return data but modifies the picture parameter in 419 | place. 420 | """ 421 | mbc = bitreader.read(1) 422 | if mbc == 0: 423 | mbdesc = bitreader.read(8) 424 | assert(mbdesc >> 7 & 1) 425 | if mbdesc >> 6 & 1: 426 | mbdiff = bitreader.read(2) 427 | y = get_block(bitreader, mbdesc & 1) 428 | y.extend(get_block(bitreader, mbdesc >> 1 & 1)) 429 | y.extend(get_block(bitreader, mbdesc >> 2 & 1)) 430 | y.extend(get_block(bitreader, mbdesc >> 3 & 1)) 431 | cb = get_block(bitreader, mbdesc >> 4 & 1) 432 | cr = get_block(bitreader, mbdesc >> 5 & 1) 433 | # ycbcr to rgb 434 | for i in range(256): 435 | j = SCALE_TAB[i] 436 | Y = y[i] - 16 437 | B = cb[j] - 128 438 | R = cr[j] - 128 439 | r = (298 * Y + 409 * R + 128) >> 8 440 | g = (298 * Y - 100 * B - 208 * R + 128) >> 8 441 | b = (298 * Y + 516 * B + 128) >> 8 442 | r = 0 if r < 0 else r 443 | r = 255 if r > 255 else r 444 | g = 0 if g < 0 else g 445 | g = 255 if g > 255 else g 446 | b = 0 if b < 0 else b 447 | b = 255 if b > 255 else b 448 | # re-order the pixels 449 | row = MB_ROW_MAP[i] 450 | col = MB_COL_MAP[i] 451 | picture[offset + row*width + col] = ''.join((chr(r), chr(g), chr(b))) 452 | else: 453 | print "mbc was not zero" 454 | 455 | 456 | def get_block(bitreader, has_coeff): 457 | """Read a 8x8 block from the data stream. 458 | 459 | This method takes care of the huffman-, RLE, zig-zag and idct and returns a 460 | list of 64 ints. 461 | """ 462 | # read the first 10 bits in a 16 bit datum 463 | out_list = ZEROS[0:64] 464 | out_list[0] = int(bitreader.read(10)) * IQUANT_TAB[0] 465 | if not has_coeff: 466 | return inverse_dct(out_list) 467 | i = 1 468 | while 1: 469 | _ = bitreader.read(32*TRIES, False) 470 | streamlen = 0 471 | ####################################################################### 472 | for j in range(TRIES): 473 | data = (_ << streamlen) & MASK 474 | data >>= SHIFT 475 | 476 | l, tmp = FH[data >> 20] 477 | streamlen += l 478 | data = (data << l) & 0xffffffff 479 | i += tmp 480 | 481 | l, tmp, eob = SH[data >> 17] 482 | streamlen += l 483 | if eob: 484 | bitreader.read(streamlen) 485 | return inverse_dct(out_list) 486 | j = ZIG_ZAG_POSITIONS[i] 487 | out_list[j] = tmp*IQUANT_TAB[j] 488 | i += 1 489 | ####################################################################### 490 | bitreader.read(streamlen) 491 | return inverse_dct(out_list) 492 | 493 | 494 | def get_gob(bitreader, picture, slicenr, width): 495 | """Read a group of blocks. 496 | 497 | The method does not return data, the picture parameter is modified in place 498 | instead. 499 | """ 500 | # the first gob has a special header 501 | if slicenr > 0: 502 | bitreader.align() 503 | gobsc = bitreader.read(22) 504 | if gobsc == 0b0000000000000000111111: 505 | print "weeeee" 506 | return False 507 | elif (not (gobsc & 0b0000000000000000100000) or 508 | (gobsc & 0b1111111111111111000000)): 509 | print "Got wrong GOBSC, aborting.", bin(gobsc) 510 | return False 511 | _ = bitreader.read(5) 512 | offset = slicenr*16*width 513 | for i in range(width / 16): 514 | get_mb(bitreader, picture, width, offset+16*i) 515 | 516 | 517 | def read_picture(data): 518 | """Convert an AR.Drone image packet to rgb-string. 519 | 520 | Returns: width, height, image and time to decode the image 521 | """ 522 | bitreader = BitReader(data) 523 | t = datetime.datetime.now() 524 | width, height = get_pheader(bitreader) 525 | slices = height / 16 526 | blocks = width / 16 527 | image = [0 for i in range(width*height)] 528 | for i in range(0, slices): 529 | get_gob(bitreader, image, i, width) 530 | bitreader.align() 531 | eos = bitreader.read(22) 532 | assert(eos == 0b0000000000000000111111) 533 | t2 = datetime.datetime.now() 534 | return width, height, ''.join(image), (t2 - t).microseconds / 1000000. 535 | 536 | 537 | try: 538 | psyco.bind(BitReader) 539 | psyco.bind(get_block) 540 | psyco.bind(get_gob) 541 | psyco.bind(get_mb) 542 | psyco.bind(inverse_dct) 543 | psyco.bind(read_picture) 544 | except NameError: 545 | print "Unable to bind video decoding methods with psyco. Proceeding anyways, but video decoding will be slow!" 546 | 547 | 548 | def main(): 549 | fh = open('framewireshark.raw', 'r') 550 | #fh = open('videoframe.raw', 'r') 551 | data = fh.read() 552 | fh.close() 553 | runs = 20 554 | t = 0 555 | for i in range(runs): 556 | print '.', 557 | width, height, image, ti = read_picture(data) 558 | #show_image(image, width, height) 559 | t += ti 560 | print 561 | print 'avg time:\t', t / runs, 'sec' 562 | print 'avg fps:\t', 1 / (t / runs), 'fps' 563 | if 'image' in sys.argv: 564 | import pygame 565 | pygame.init() 566 | W, H = 320, 240 567 | screen = pygame.display.set_mode((W, H)) 568 | surface = pygame.image.fromstring(image, (width, height), 'RGB') 569 | screen.blit(surface, (0, 0)) 570 | pygame.display.flip() 571 | raw_input() 572 | 573 | 574 | if __name__ == '__main__': 575 | if 'profile' in sys.argv: 576 | cProfile.run('main()') 577 | else: 578 | main() 579 | 580 | -------------------------------------------------------------------------------- /libardrone/at.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2011 Bastian Venthur, 2016 Andreas Bresser 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in 11 | # all copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | # THE SOFTWARE. 20 | 21 | """ 22 | low level AT Commands 23 | """ 24 | import struct 25 | import socket 26 | 27 | ARDRONE_COMMAND_PORT = 5556 28 | 29 | def at_ref(seq, takeoff, emergency=False): 30 | """ 31 | Basic behaviour of the drone: take-off/landing, emergency stop/reset) 32 | 33 | Parameters: 34 | seq -- sequence number 35 | takeoff -- True: Takeoff / False: Land 36 | emergency -- True: Turn off the engines 37 | """ 38 | p = 0b10001010101000000000000000000 39 | if takeoff: 40 | p += 0b1000000000 41 | if emergency: 42 | p += 0b0100000000 43 | at("REF", seq, [p]) 44 | 45 | def at_pcmd(seq, progressive, lr, fb, vv, va): 46 | """ 47 | Makes the drone move (translate/rotate). 48 | 49 | Parameters: 50 | seq -- sequence number 51 | progressive -- True: enable progressive commands, False: disable (i.e. 52 | enable hovering mode) 53 | lr -- left-right tilt: float [-1..1] negative: left, positive: right 54 | rb -- front-back tilt: float [-1..1] negative: forwards, positive: 55 | backwards 56 | vv -- vertical speed: float [-1..1] negative: go down, positive: rise 57 | va -- angular speed: float [-1..1] negative: spin left, positive: spin 58 | right 59 | 60 | The above float values are a percentage of the maximum speed. 61 | """ 62 | p = 1 if progressive else 0 63 | at("PCMD", seq, [p, float(lr), float(fb), float(vv), float(va)]) 64 | 65 | def at_ftrim(seq): 66 | """ 67 | Tell the drone it's lying horizontally. 68 | 69 | Parameters: 70 | seq -- sequence number 71 | """ 72 | at("FTRIM", seq, []) 73 | 74 | def at_zap(seq, stream): 75 | """ 76 | Selects which video stream to send on the video UDP port. 77 | 78 | Parameters: 79 | seq -- sequence number 80 | stream -- Integer: video stream to broadcast 81 | """ 82 | # FIXME: improve parameters to select the modes directly 83 | at("ZAP", seq, [stream]) 84 | 85 | def at_config(seq, option, value): 86 | """Set configuration parameters of the drone.""" 87 | at("CONFIG", seq, [str(option), str(value)]) 88 | 89 | def at_config_ids(seq, value): 90 | """Set configuration parameters of the drone.""" 91 | at("CONFIG_IDS", seq, value) 92 | 93 | def at_ctrl(seq, num): 94 | """Ask the parrot to drop its configuration file""" 95 | at("CTRL", seq, [num, 0]) 96 | 97 | def at_comwdg(seq): 98 | """ 99 | Reset communication watchdog. 100 | """ 101 | # FIXME: no sequence number 102 | at("COMWDG", seq, []) 103 | 104 | def at_aflight(seq, flag): 105 | """ 106 | Makes the drone fly autonomously. 107 | 108 | Parameters: 109 | seq -- sequence number 110 | flag -- Integer: 1: start flight, 0: stop flight 111 | """ 112 | at("AFLIGHT", seq, [flag]) 113 | 114 | def at_pwm(seq, m1, m2, m3, m4): 115 | """ 116 | Sends control values directly to the engines, overriding control loops. 117 | 118 | Parameters: 119 | seq -- sequence number 120 | m1 -- front left command 121 | m2 -- fright right command 122 | m3 -- back right command 123 | m4 -- back left command 124 | """ 125 | # FIXME: what type do mx have? 126 | raise NotImplementedError() 127 | 128 | def at_led(seq, anim, f, d): 129 | """ 130 | Control the drones LED. 131 | 132 | Parameters: 133 | seq -- sequence number 134 | anim -- Integer: animation to play 135 | f -- ?: frequence in HZ of the animation 136 | d -- Integer: total duration in seconds of the animation 137 | """ 138 | pass 139 | 140 | def at_anim(seq, anim, d): 141 | """ 142 | Makes the drone execute a predefined movement (animation). 143 | 144 | Parameters: 145 | seq -- sequcence number 146 | anim -- Integer: animation to play 147 | d -- Integer: total duration in sections of the animation 148 | """ 149 | at("ANIM", seq, [anim, d]) 150 | 151 | def at(command, seq, params): 152 | """ 153 | Parameters: 154 | command -- the command 155 | seq -- the sequence number 156 | params -- a list of elements which can be either int, float or string 157 | """ 158 | param_str = '' 159 | for p in params: 160 | if type(p) == int: 161 | param_str += ",%d" % p 162 | elif type(p) == float: 163 | param_str += ",%d" % f2i(p) 164 | elif type(p) == str: 165 | param_str += ',"' + p + '"' 166 | msg = "AT*%s=%i%s\r" % (command, seq, param_str) 167 | sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 168 | sock.sendto(msg.encode("utf-8"), ("192.168.1.1", ARDRONE_COMMAND_PORT)) 169 | 170 | def f2i(f): 171 | """Interpret IEEE-754 floating-point value as signed integer. 172 | 173 | Arguments: 174 | f -- floating point value 175 | """ 176 | return struct.unpack('i', struct.pack('f', f))[0] 177 | -------------------------------------------------------------------------------- /libardrone/h264decoder.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Copyright (c) 2013 Adrian Taylor 3 | # Inspired by equivalent node.js code by Felix Geisendörfer 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | # THE SOFTWARE. 22 | import os 23 | 24 | 25 | """ 26 | H.264 video decoder for AR.Drone 2.0. Uses ffmpeg. 27 | """ 28 | 29 | import sys 30 | from subprocess import PIPE, Popen 31 | from threading import Thread 32 | import time 33 | import libardrone 34 | import ctypes 35 | import numpy as np 36 | import sys 37 | 38 | 39 | try: 40 | from Queue import Queue, Empty 41 | except ImportError: 42 | from queue import Queue, Empty # python 3.x 43 | 44 | ON_POSIX = 'posix' in sys.builtin_module_names 45 | 46 | 47 | def enqueue_output(out, outfileobject, frame_size): 48 | frame_size_bytes = frame_size[0] * frame_size[1] * 3 49 | while True: 50 | buffer_str = out.read(frame_size_bytes) 51 | im = np.frombuffer(buffer_str, count=frame_size_bytes, dtype=np.uint8) 52 | im = im.reshape((frame_size[0], frame_size[1], 3)) 53 | outfileobject.image_ready(im) 54 | 55 | 56 | # Logic for making ffmpeg terminate on the death of this process 57 | def set_death_signal(signal): 58 | libc = ctypes.CDLL('libc.so.6') 59 | PR_SET_DEATHSIG = 1 60 | libc.prctl(PR_SET_DEATHSIG, signal) 61 | 62 | 63 | def set_death_signal_int(): 64 | if sys.platform != 'darwin': 65 | SIGINT = 2 66 | SIGTERM = 15 67 | set_death_signal(SIGINT) 68 | 69 | 70 | """ 71 | Usage: pass a listener, with a method 'data_ready' which will be called whenever there's output 72 | from ffmpeg. This will be called in an arbitrary thread. You can later call H264ToPng.get_data_if_any to retrieve 73 | said data. 74 | You should then call write repeatedly to write some encoded H.264 data. 75 | """ 76 | class H264Decoder(object): 77 | 78 | def __init__(self, outfileobject=None, frame_size=(360, 640)): 79 | if outfileobject is not None: 80 | 81 | if (H264Decoder.which('ffmpeg') is None): 82 | raise Exception("You need to install ffmpeg to be able to run ardrone") 83 | 84 | p = Popen(["nice", "-n", "15", "ffmpeg", "-i", "-", "-f", "sdl", 85 | "-probesize", "2048", "-flags", "low_delay", "-f", 86 | "rawvideo", "-pix_fmt", 'rgb24', "-"], 87 | stdin=PIPE, stdout=PIPE, stderr=open('/dev/null', 'w'), 88 | bufsize=0, preexec_fn=set_death_signal_int) 89 | t = Thread(target=enqueue_output, args=(p.stdout, outfileobject, frame_size)) 90 | t.daemon = True # thread dies with the program 91 | t.start() 92 | else: 93 | if (H264Decoder.which('ffplay') is None): 94 | raise Exception("You need to install ffmpeg and ffplay to be able to run ardrone in debug mode") 95 | 96 | p = Popen(["nice", "-n", "15", "ffplay", "-probesize", "2048", 97 | "-flags", "low_delay", "-i", "-"], 98 | stdin=PIPE, stdout=open('/dev/null', 'w'), 99 | stderr=open('/dev/null', 'w'), bufsize=-1, 100 | preexec_fn=set_death_signal_int) 101 | 102 | self.writefd = p.stdin 103 | 104 | def write(self, data): 105 | self.writefd.write(data) 106 | 107 | @staticmethod 108 | def which(program): 109 | def is_exe(fpath): 110 | return os.path.isfile(fpath) and os.access(fpath, os.X_OK) 111 | 112 | fpath, fname = os.path.split(program) 113 | if fpath: 114 | if is_exe(program): 115 | return program 116 | else: 117 | for path in os.environ["PATH"].split(os.pathsep): 118 | path = path.strip('"') 119 | exe_file = os.path.join(path, program) 120 | if is_exe(exe_file): 121 | return exe_file 122 | 123 | return None 124 | -------------------------------------------------------------------------------- /libardrone/navdata.py: -------------------------------------------------------------------------------- 1 | import struct 2 | 3 | # 0: "Not defined" 4 | # 131072: "Landed" 5 | # 393216: "Taking-off-Floor" 6 | # 393217: "Taking-off-Air" 7 | # 262144: "Hovering" 8 | # 524288: "Landing" 9 | # 458752: "Stabilizing" 10 | # 196608: "Moving" 11 | # 262153 and 196613 and 262155 and 196614 and 458753: "Undefined" 12 | ctrl_state_dict={ 13 | 0:0, 14 | 131072:1, 15 | 393216:2, 16 | 393217:3, 17 | 262144:4, 18 | 524288:5, 19 | 458752:6, 20 | 196608:7, 21 | 262153:8, 22 | 196613:9, 23 | 262155:10, 24 | 196614:11, 25 | 458753:12 26 | } 27 | 28 | def decode_navdata(packet): 29 | """Decode a navdata packet.""" 30 | offset = 0 31 | _ = struct.unpack_from("IIII", packet, offset) 32 | drone_state = dict() 33 | drone_state['fly_mask'] = _[1] & 1 # FLY MASK : (0) ardrone is landed, (1) ardrone is flying 34 | drone_state['video_mask'] = _[1] >> 1 & 1 # VIDEO MASK : (0) video disable, (1) video enable 35 | drone_state['vision_mask'] = _[1] >> 2 & 1 # VISION MASK : (0) vision disable, (1) vision enable */ 36 | drone_state['control_mask'] = _[1] >> 3 & 1 # CONTROL ALGO (0) euler angles control, (1) angular speed control */ 37 | drone_state['altitude_mask'] = _[1] >> 4 & 1 # ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active */ 38 | drone_state['user_feedback_start'] = _[1] >> 5 & 1 # USER feedback : Start button state */ 39 | drone_state['command_mask'] = _[1] >> 6 & 1 # Control command ACK : (0) None, (1) one received */ 40 | drone_state['fw_file_mask'] = _[1] >> 7 & 1 # Firmware file is good (1) */ 41 | drone_state['fw_ver_mask'] = _[1] >> 8 & 1 # Firmware update is newer (1) */ 42 | drone_state['fw_upd_mask'] = _[1] >> 9 & 1 # Firmware update is ongoing (1) */ 43 | drone_state['navdata_demo_mask'] = _[1] >> 10 & 1 # Navdata demo : (0) All navdata, (1) only navdata demo */ 44 | drone_state['navdata_bootstrap'] = _[1] >> 11 & 1 # Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */ 45 | drone_state['motors_mask'] = _[1] >> 12 & 1 # Motor status : (0) Ok, (1) Motors problem */ 46 | drone_state['com_lost_mask'] = _[1] >> 13 & 1 # Communication lost : (1) com problem, (0) Com is ok */ 47 | drone_state['vbat_low'] = _[1] >> 15 & 1 # VBat low : (1) too low, (0) Ok */ 48 | drone_state['user_el'] = _[1] >> 16 & 1 # User Emergency Landing : (1) User EL is ON, (0) User EL is OFF*/ 49 | drone_state['timer_elapsed'] = _[1] >> 17 & 1 # Timer elapsed : (1) elapsed, (0) not elapsed */ 50 | drone_state['angles_out_of_range'] = _[1] >> 19 & 1 # Angles : (0) Ok, (1) out of range */ 51 | drone_state['ultrasound_mask'] = _[1] >> 21 & 1 # Ultrasonic sensor : (0) Ok, (1) deaf */ 52 | drone_state['cutout_mask'] = _[1] >> 22 & 1 # Cutout system detection : (0) Not detected, (1) detected */ 53 | drone_state['pic_version_mask'] = _[1] >> 23 & 1 # PIC Version number OK : (0) a bad version number, (1) version number is OK */ 54 | drone_state['atcodec_thread_on'] = _[1] >> 24 & 1 # ATCodec thread ON : (0) thread OFF (1) thread ON */ 55 | drone_state['navdata_thread_on'] = _[1] >> 25 & 1 # Navdata thread ON : (0) thread OFF (1) thread ON */ 56 | drone_state['video_thread_on'] = _[1] >> 26 & 1 # Video thread ON : (0) thread OFF (1) thread ON */ 57 | drone_state['acq_thread_on'] = _[1] >> 27 & 1 # Acquisition thread ON : (0) thread OFF (1) thread ON */ 58 | drone_state['ctrl_watchdog_mask'] = _[1] >> 28 & 1 # CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */ 59 | drone_state['adc_watchdog_mask'] = _[1] >> 29 & 1 # ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */ 60 | drone_state['com_watchdog_mask'] = _[1] >> 30 & 1 # Communication Watchdog : (1) com problem, (0) Com is ok */ 61 | drone_state['emergency_mask'] = _[1] >> 31 & 1 # Emergency landing : (0) no emergency, (1) emergency */ 62 | data = dict() 63 | data['drone_state'] = drone_state 64 | data['header'] = _[0] 65 | data['seq_nr'] = _[2] 66 | data['vision_flag'] = _[3] 67 | offset += struct.calcsize("IIII") 68 | has_flying_information = False 69 | while 1: 70 | try: 71 | id_nr, size = struct.unpack_from("HH", packet, offset) 72 | offset += struct.calcsize("HH") 73 | except struct.error: 74 | break 75 | values = [] 76 | for i in range(size - struct.calcsize("HH")): 77 | values.append(struct.unpack_from("c", packet, offset)[0]) 78 | offset += struct.calcsize("c") 79 | # navdata_tag_t in navdata-common.h 80 | if id_nr == 0: 81 | has_flying_information = True 82 | values = struct.unpack_from("IIfffifffI", "".join(values)) 83 | values = dict(zip(['ctrl_state', 'battery', 'theta', 'phi', 'psi', 'altitude', 'vx', 'vy', 'vz', 'num_frames'], values)) 84 | # convert the millidegrees into degrees and round to int, as they 85 | values['ctrl_state'] = ctrl_state_dict[values['ctrl_state']] 86 | # are not so precise anyways 87 | for i in 'theta', 'phi', 'psi': 88 | values[i] = int(values[i] / 1000) 89 | data[id_nr] = values 90 | return data, has_flying_information 91 | -------------------------------------------------------------------------------- /libardrone/paveparser.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # Copyright (c) 2013 Adrian Taylor 3 | # Inspired by equivalent node.js code by Felix Geisendörfer 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in 13 | # all copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 21 | # THE SOFTWARE. 22 | 23 | import struct 24 | """ 25 | The AR Drone 2.0 allows a tcp client to receive H264 (MPEG4.10 AVC) video 26 | from the drone. However, the frames are wrapped by Parrot Video 27 | Encapsulation (PaVE), which this class parses. 28 | """ 29 | 30 | """ 31 | Usage: Pass in an output file object into the constructor, then call write on this. 32 | """ 33 | class PaVEParser(object): 34 | 35 | HEADER_SIZE_SHORT = 64; # sometimes header is longer 36 | 37 | def __init__(self, outfileobject): 38 | self.buffer = "" 39 | self.state = self.handle_header 40 | self.outfileobject = outfileobject 41 | self.misaligned_frames = 0 42 | self.payloads = 0 43 | self.drop_old_frames = True 44 | self.align_on_iframe = True 45 | 46 | if self.drop_old_frames: 47 | self.state = self.handle_header_drop_frames 48 | 49 | def write(self, data): 50 | self.buffer += data 51 | while True: 52 | made_progress = self.state() 53 | if not made_progress: 54 | return 55 | 56 | def handle_header(self): 57 | if self.fewer_remaining_than(self.HEADER_SIZE_SHORT): 58 | return False 59 | 60 | (signature, version, video_codec, header_size, self.payload_size, encoded_stream_width, 61 | encoded_stream_height, display_width, display_height, frame_number, timestamp, total_chunks, 62 | chunk_index, frame_type, control, stream_byte_position_lw, stream_byte_position_uw, 63 | stream_id, total_slices, slice_index, header1_size, header2_size, 64 | reserved2, advertised_size, reserved3) = struct.unpack("<4sBBHIHHHHIIBBBBIIHBBBB2sI12s", 65 | self.buffer[0:self.HEADER_SIZE_SHORT]) 66 | 67 | if signature != "PaVE": 68 | self.state = self.handle_misalignment 69 | return True 70 | self.buffer = self.buffer[header_size:] 71 | self.state = self.handle_payload 72 | return True 73 | 74 | def handle_header_drop_frames(self): 75 | 76 | eligible_index = self.buffer.find('PaVE') 77 | 78 | if (eligible_index < 0): 79 | return False 80 | self.buffer = self.buffer[eligible_index:] 81 | 82 | if self.fewer_remaining_than(self.HEADER_SIZE_SHORT): 83 | return False 84 | 85 | eligible_index = 0 86 | current_index = eligible_index 87 | 88 | while current_index != -1 and len(self.buffer[current_index:]) > self.HEADER_SIZE_SHORT: 89 | (signature, version, video_codec, header_size, payload_size, encoded_stream_width, 90 | encoded_stream_height, display_width, display_height, frame_number, timestamp, total_chunks, 91 | chunk_index, frame_type, control, stream_byte_position_lw, stream_byte_position_uw, 92 | stream_id, total_slices, slice_index, header1_size, 93 | header2_size, reserved2, advertised_size, 94 | reserved3) = struct.unpack("<4sBBHIHHHHIIBBBBIIHBBBB2sI12s", 95 | self.buffer[current_index:current_index + self.HEADER_SIZE_SHORT]) 96 | 97 | if (frame_type != 3 or current_index == 0): 98 | eligible_index = current_index 99 | self.payload_size = payload_size 100 | 101 | offset = self.buffer[current_index + 1:].find('PaVE') + 1 102 | if (offset == 0): 103 | break 104 | 105 | current_index += offset 106 | 107 | self.buffer = self.buffer[eligible_index + header_size:] 108 | self.state = self.handle_payload 109 | return True 110 | 111 | 112 | def handle_misalignment(self): 113 | """Sometimes we start of in the middle of frame - look for the PaVE header.""" 114 | IFrame = False 115 | if self.align_on_iframe: 116 | while (not IFrame): 117 | index = self.buffer.find('PaVE') 118 | if index == -1: 119 | return False 120 | 121 | self.buffer = self.buffer[index:] 122 | 123 | if self.fewer_remaining_than(self.HEADER_SIZE_SHORT): 124 | return False 125 | 126 | (signature, version, video_codec, header_size, self.payload_size, encoded_stream_width, 127 | encoded_stream_height, display_width, display_height, frame_number, timestamp, total_chunks, 128 | chunk_index, frame_type, control, stream_byte_position_lw, stream_byte_position_uw, 129 | stream_id, total_slices, slice_index, header1_size, header2_size, reserved2, advertised_size, 130 | reserved3) = struct.unpack("<4sBBHIHHHHIIBBBBIIHBBBB2sI12s", self.buffer[0:self.HEADER_SIZE_SHORT]) 131 | 132 | IFrame = (frame_type == 1 or frame_type == 2) 133 | if not IFrame: 134 | self.buffer = self.buffer[header_size:] 135 | else: 136 | index = self.buffer.find('PaVE') 137 | if index == -1: 138 | return False 139 | self.buffer = self.buffer[index:] 140 | 141 | self.misaligned_frames += 1 142 | self.state = self.handle_header 143 | 144 | return True 145 | 146 | def handle_payload(self): 147 | if self.fewer_remaining_than(self.payload_size): 148 | return False 149 | self.state = self.handle_header 150 | if self.drop_old_frames: 151 | self.state = self.handle_header_drop_frames 152 | 153 | self.outfileobject.write(self.buffer[0:self.payload_size]) 154 | self.buffer = self.buffer[self.payload_size:] 155 | self.payloads += 1 156 | return True 157 | 158 | def fewer_remaining_than(self, desired_size): 159 | return len(self.buffer) < desired_size 160 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy==1.12.1 2 | mock==2.0.0 3 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | 4 | setup( 5 | name='python-ardrone', 6 | author='Brain Corporation', 7 | author_email='passot@braincorporation.com', 8 | url='https://github.com/braincorp/python-ardrone', 9 | long_description='', 10 | version='dev', 11 | packages=find_packages(), 12 | include_package_data=True, 13 | install_requires=[]) 14 | -------------------------------------------------------------------------------- /test/ardrone2_video_example.capture: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adetaylor/python-ardrone/67022e3840e6c4434a7c929e0509e19cc9b83c0e/test/ardrone2_video_example.capture -------------------------------------------------------------------------------- /test/paveparser.output: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/adetaylor/python-ardrone/67022e3840e6c4434a7c929e0509e19cc9b83c0e/test/paveparser.output -------------------------------------------------------------------------------- /test/test_h264_decoder.py: -------------------------------------------------------------------------------- 1 | import os 2 | import mock 3 | from libardrone import paveparser 4 | from libardrone import h264decoder 5 | 6 | 7 | def test_h264_decoder(): 8 | outfileobj = mock.Mock() 9 | decoder = h264decoder.H264Decoder(outfileobj) 10 | example_video_stream = open(os.path.join(os.path.dirname(__file__), 'paveparser.output')) 11 | while True: 12 | data = example_video_stream.read(1000) 13 | if len(data) == 0: 14 | break 15 | decoder.write(data) 16 | 17 | assert outfileobj.image_ready.called 18 | -------------------------------------------------------------------------------- /test/test_libardrone.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2011 Bastian Venthur 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in 11 | # all copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | # THE SOFTWARE. 20 | import unittest 21 | from libardrone import at 22 | 23 | class LibardroneTestCase(unittest.TestCase): 24 | def test_f2i(self): 25 | self.assertEqual(at.f2i(-0.8,), -1085485875) 26 | 27 | if __name__ == "__main__": 28 | unittest.main() 29 | -------------------------------------------------------------------------------- /test/test_losing_connection.py: -------------------------------------------------------------------------------- 1 | import select 2 | import socket 3 | import struct 4 | import time 5 | 6 | DRONE_IP = "192.168.1.1" 7 | ARDRONE_NAVDATA_PORT = 5554 8 | ARDRONE_COMMAND_PORT = 5556 9 | 10 | ''' 11 | Small endless loop to test the robustness of the tcp ip connection (video streaming) 12 | Warning: This test does not stop, it raises an exception when the connection is lost or 13 | if something goes wrong (most likely the drone stops sending video data and 14 | send empty packets on the command port... 15 | ''' 16 | 17 | def at(command, seq, params): 18 | """ 19 | Parameters: 20 | command -- the command 21 | seq -- the sequence number 22 | params -- a list of elements which can be either int, float or string 23 | """ 24 | param_str = '' 25 | for p in params: 26 | if type(p) == int: 27 | param_str += ",%d" % p 28 | elif type(p) == float: 29 | param_str += ",%d" % f2i(p) 30 | elif type(p) == str: 31 | param_str += ',"' + p + '"' 32 | msg = "AT*%s=%i%s\r" % (command, seq, param_str) 33 | sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) 34 | sock.sendto(msg, ("192.168.1.1", ARDRONE_COMMAND_PORT)) 35 | 36 | def f2i(f): 37 | """Interpret IEEE-754 floating-point value as signed integer. 38 | Arguments: 39 | f -- floating point value 40 | """ 41 | return struct.unpack('i', struct.pack('f', f))[0] 42 | 43 | 44 | if __name__ == '__main__': 45 | nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) 46 | nav_socket.connect((DRONE_IP, ARDRONE_NAVDATA_PORT)) 47 | nav_socket.setblocking(0) 48 | nav_socket.send("\x01\x00\x00\x00") 49 | 50 | seq = 1 51 | stopping = 1 52 | while stopping < 100: 53 | inputready, outputready, exceptready = select.select([nav_socket], [], [], 1) 54 | seq += 1 55 | at("COMWDG", seq, []) 56 | if len(inputready) == 0: 57 | print "Connection lost for the %d time !" % stopping 58 | nav_socket.send("\x01\x00\x00\x00") 59 | stopping += 1 60 | for i in inputready: 61 | while 1: 62 | try: 63 | data = nav_socket.recv(500) 64 | except IOError: 65 | break 66 | 67 | raise Exception("Should not get here") 68 | -------------------------------------------------------------------------------- /test/test_paveparser.py: -------------------------------------------------------------------------------- 1 | import os 2 | import mock 3 | from libardrone import paveparser 4 | 5 | 6 | def test_misalignment(): 7 | outfile = mock.Mock() 8 | p = paveparser.PaVEParser(outfile) 9 | example_video_stream = open(os.path.join(os.path.dirname(__file__), 'ardrone2_video_example.capture')) 10 | while True: 11 | data = example_video_stream.read(1000000) 12 | if len(data) == 0: 13 | break 14 | p.write(data) 15 | 16 | assert outfile.write.called 17 | assert p.misaligned_frames < 3 18 | 19 | if __name__ == "__main__": 20 | test_misalignment() 21 | -------------------------------------------------------------------------------- /tox.ini: -------------------------------------------------------------------------------- 1 | [tox] 2 | envlist = py27 3 | 4 | [testenv] 5 | deps = pytest 6 | pytest-cov 7 | -rrequirements.txt 8 | commands = py.test 9 | --------------------------------------------------------------------------------