summaryrefslogtreecommitdiff
path: root/libardrone/libardrone.py
diff options
context:
space:
mode:
Diffstat (limited to 'libardrone/libardrone.py')
-rw-r--r--libardrone/libardrone.py690
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()
+
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-05 02:24:09 +0000