From e53ac269c06d03b37e9c912202da2fc1a896950c Mon Sep 17 00:00:00 2001 From: Foster McLane Date: Sat, 29 Sep 2018 10:36:45 -0400 Subject: [PATCH] merge from AdeelH/python-ardrone --- ardrone/at.py | 1 + ardrone/drone.py | 51 +++++++++++++------ ardrone/network.py | 122 +++++++++++++++++++-------------------------- ardrone/video.c | 2 +- 4 files changed, 91 insertions(+), 85 deletions(-) diff --git a/ardrone/at.py b/ardrone/at.py index b357aef..0f2a4f5 100644 --- a/ardrone/at.py +++ b/ardrone/at.py @@ -135,6 +135,7 @@ def anim(host, seq, anim, d): """ at(host, 'ANIM', seq, [anim, d]) + def at(host, command, seq, params): """ Parameters: diff --git a/ardrone/drone.py b/ardrone/drone.py index af2fb46..785d048 100644 --- a/ardrone/drone.py +++ b/ardrone/drone.py @@ -4,7 +4,6 @@ import time import threading -import multiprocessing import PIL.Image @@ -28,18 +27,32 @@ def __init__(self, host='192.168.1.1'): self.lock = threading.Lock() self.speed = 0.2 self.at(ardrone.at.config, 'general:navdata_demo', 'TRUE') + self.at(ardrone.at.config, 'control:control_level', '3') self.at(ardrone.at.config, 'control:altitude_max', '20000') - self.video_pipe, video_pipe_other = multiprocessing.Pipe() - self.nav_pipe, nav_pipe_other = multiprocessing.Pipe() - self.com_pipe, com_pipe_other = multiprocessing.Pipe() - self.network_process = ardrone.network.ARDroneNetworkProcess(self.host, nav_pipe_other, video_pipe_other, com_pipe_other) - self.network_process.start() - self.ipc_thread = ardrone.network.IPCThread(self) - self.ipc_thread.start() - self.image = PIL.Image.new('RGB', (640, 360)) - self.navdata = dict() + + self.image = None + self.navdata = None + + self.video_thread = ardrone.network.VidThread( + self.host, + self.image_callback + ) + self.navdata_thread = ardrone.network.NavThread( + self.host, + self.navdata_callback + ) + self.video_thread.start() + self.navdata_thread.start() + self.time = 0 + def image_callback(self, im_data): + w, h, img = im_data + self.image = PIL.Image.frombuffer('RGB', (w, h), img, 'raw', 'RGB', 0, 1) + + def navdata_callback(self, navdata): + self.navdata = navdata + def takeoff(self): """Make the drone takeoff.""" self.at(ardrone.at.ref, True) @@ -140,10 +153,10 @@ def halt(self): """ with self.lock: self.com_watchdog_timer.cancel() - self.ipc_thread.stop() - self.ipc_thread.join() - self.network_process.terminate() - self.network_process.join() + self.video_thread.stop() + self.video_thread.join() + self.navdata_thread.stop() + self.navdata_thread.join() def move(self, lr, fb, vv, va): """Makes the drone move (translate/rotate). @@ -156,3 +169,13 @@ def move(self, lr, fb, vv, va): va -- angular speed: float [-1..1] negative: spin left, positive: spin right""" self.at(ardrone.at.pcmd, True, lr, fb, vv, va) + + def move2(self, vv, va): + """Makes the drone move (up-down and rotate only) while trying + to stay above the same point on the ground. + + Parameters: + vv -- vertical speed: float [-1..1] negative: go down, positive: rise + va -- angular speed: float [-1..1] negative: spin left, positive: spin + right""" + self.at(ardrone.at.pcmd, False, 0, 0, vv, va) diff --git a/ardrone/network.py b/ardrone/network.py index c083809..84fcbf2 100644 --- a/ardrone/network.py +++ b/ardrone/network.py @@ -6,106 +6,88 @@ import socket import struct import threading -import multiprocessing - -import PIL.Image import ardrone.constant import ardrone.navdata import ardrone.video -class ARDroneNetworkProcess(multiprocessing.Process): - """ARDrone Network Process. +class NavThread(threading.Thread): + """Inter Process Communication Thread. - This process collects data from the video and navdata port, converts the - data and sends it to the IPCThread. + This thread collects navdata from the navdata port and makes it available + to the ARDrone. """ - def __init__(self, host, nav_pipe, video_pipe, com_pipe): - multiprocessing.Process.__init__(self) - self.nav_pipe = nav_pipe - self.video_pipe = video_pipe - self.com_pipe = com_pipe + def __init__(self, host, callback): + threading.Thread.__init__(self) self.host = host + self.callback = callback + self.stopping = False def run(self): - video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - video_socket.connect((self.host, ardrone.constant.VIDEO_PORT)) - nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) nav_socket.setblocking(False) nav_socket.bind(('', ardrone.constant.NAVDATA_PORT)) nav_socket.sendto(b'\x01\x00\x00\x00', (self.host, ardrone.constant.NAVDATA_PORT)) - stopping = False - while not stopping: - inputready, outputready, exceptready = select.select([nav_socket, video_socket, self.com_pipe], [], []) - for i in inputready: - if i == video_socket: - # get first few bytes of header - data = video_socket.recv(12, socket.MSG_WAITALL) - if len(data) != 12: - continue - - # decode relevant portions of the header - sig_p, sig_a, sig_v, sig_e, version, codec, header, payload = struct.unpack('4cBBHI', data) - - # check signature (and ignore packet otherwise) - if sig_p != b'P' or sig_a != b'a' or sig_v != b'V' or sig_e != b'E': - continue - - # get remaining frame - data += video_socket.recv(header - 12 + payload, socket.MSG_WAITALL) - - try: - # decode the frame - image = ardrone.video.decode(data) - self.video_pipe.send(image) - except ardrone.video.DecodeError: - pass - elif i == nav_socket: - while 1: - try: - data = nav_socket.recv(65535) - except IOError: - # we consumed every packet from the socket and - # continue with the last one - break - navdata = ardrone.navdata.decode(data) - self.nav_pipe.send(navdata) - elif i == self.com_pipe: - _ = self.com_pipe.recv() - stopping = True + while not self.stopping: + inputready, outputready, exceptready = select.select([nav_socket], [], []) + if len(inputready) < 1: + continue + while True: + try: + data = nav_socket.recv(65535) + except IOError: + # we consumed every packet from the socket and continue with the last one break - video_socket.close() + navdata = ardrone.navdata.decode(data) + self.callback(navdata) nav_socket.close() + def stop(self): + """Stop the IPCThread activity.""" + self.stopping = True -class IPCThread(threading.Thread): + +class VidThread(threading.Thread): """Inter Process Communication Thread. - This thread collects the data from the ARDroneNetworkProcess and forwards - it to the ARDrone. + This thread collects video from the video port and makes it available + to the ARDrone. """ - def __init__(self, drone): + def __init__(self, host, callback): threading.Thread.__init__(self) - self.drone = drone + self.host = host + self.callback = callback self.stopping = False def run(self): + video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + video_socket.connect((self.host, ardrone.constant.VIDEO_PORT)) + while not self.stopping: - inputready, outputready, exceptready = select.select([self.drone.video_pipe, self.drone.nav_pipe], [], [], 1) - for i in inputready: - if i == self.drone.video_pipe: - while self.drone.video_pipe.poll(): - width, height, image = self.drone.video_pipe.recv() - self.drone.image = PIL.Image.frombuffer('RGB', (width, height), image, 'raw', 'RGB', 0, 1) - elif i == self.drone.nav_pipe: - while self.drone.nav_pipe.poll(): - navdata = self.drone.nav_pipe.recv() - self.drone.navdata = navdata + inputready, outputready, exceptready = select.select([video_socket], [], []) + if len(inputready) < 1: + continue + # get first few bytes of header + data = video_socket.recv(12, socket.MSG_WAITALL) + if len(data) != 12: + continue + # decode relevant portions of the header + sig_p, sig_a, sig_v, sig_e, version, codec, header, payload = struct.unpack('4cBBHI', data) + # check signature (and ignore packet otherwise) + if sig_p != b'P' or sig_a != b'a' or sig_v != b'V' or sig_e != b'E': + continue + # get remaining frame + data += video_socket.recv(header - 12 + payload, socket.MSG_WAITALL) + try: + img = ardrone.video.decode(data) + self.callback(img) + except ardrone.video.DecodeError: + pass + video_socket.close() def stop(self): """Stop the IPCThread activity.""" diff --git a/ardrone/video.c b/ardrone/video.c index dad62f5..8daec58 100644 --- a/ardrone/video.c +++ b/ardrone/video.c @@ -34,7 +34,7 @@ struct PaVE { uint8_t reserved1[2]; //padding to align to 48 bytes uint32_t advertised_size; //size of advertised frame uint8_t reserved2[12]; //padding to align to 64 bytes -} __attribute__ ((packed)); +} __attribute__ (packed); static PyObject * VideoDecodeError;