server2

1 files changed, 690 insertions, 0 deletions

diff --git a/libardrone/libardrone.py b/libardrone/libardrone.py
new file mode 100644
index 0000000..348ce0f
--- /dev/null
+++ b/libardrone/libardrone.py
@@ -0,0 +1,690 @@
+# 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()
+