def run(self):
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
nav_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
nav_socket.setblocking(0)
nav_socket.bind(('', ARDRONE_NAVDATA_PORT))
try:
nav_socket.sendto("\x01\x00\x00\x00",
(ARDRONE_DIRECCION, ARDRONE_NAVDATA_PORT))
except:
print "arnetwork: error al inicializar puerto navdata"
stopping = False
while not stopping:
inputready, outputready, exceptready = select.select(
[nav_socket, self.com_pipe], [], [])
for i in inputready:
# Recibo datos de navegacion del drone.
if 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 = libardrone.decode_navdata(data)
self.nav_pipe.send(navdata)
# Me avisaron que tengo que irme.
elif i == self.com_pipe:
_ = self.com_pipe.recv()
stopping = True
break
nav_socket.close()
def run(self):
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
nav_socket.setblocking(0)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT))
stopping = False
while not stopping:
try:
inputready, outputready, exceptready = select.select([nav_socket, self.com_pipe], [], [])
for i in inputready:
if 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 = libardrone.decode_navdata(data)
self.nav_pipe.send(navdata)
elif i == self.com_pipe:
_ = self.com_pipe.recv()
stopping = True
break
except:
pass
nav_socket.close()
def run(self):
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
nav_socket.setblocking(0)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00",
('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT))
stopping = False
while not stopping:
inputready, outputready, exceptready = select.select(
[nav_socket, self.com_pipe], [], [])
for i in inputready:
if 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 = libardrone.decode_navdata(data)
self.nav_pipe.send(navdata)
elif i == self.com_pipe:
_ = self.com_pipe.recv()
stopping = True
break
nav_socket.close()
def run(self):
if self.is_ar_drone_2:
video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
video_socket.connect(
('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
video_socket.setblocking(0)
else:
video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
video_socket.setblocking(0)
video_socket.bind(('', libardrone.ARDRONE_VIDEO_PORT))
video_socket.sendto("\x01\x00\x00\x00",
('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
nav_socket.setblocking(0)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00",
('192.168.1.1', libardrone.ARDRONE_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:
while 1:
try:
data = video_socket.recv(65535)
except IOError:
# we consumed every packet from the socket and
# continue with the last one
break
if self.is_ar_drone_2:
self.ar2video.write(data)
# Sending is taken care of by the decoder
else:
w, h, image, t = arvideo.read_picture(data)
self.video_pipe.send(image)
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 = libardrone.decode_navdata(data)
self.nav_pipe.send(navdata)
elif i == self.com_pipe:
_ = self.com_pipe.recv()
stopping = True
break
video_socket.close()
nav_socket.close()
def run(self):
if self.is_ar_drone_2:
video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
video_socket.connect(('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
video_socket.setblocking(0)
else:
video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
video_socket.setblocking(0)
video_socket.bind(('', libardrone.ARDRONE_VIDEO_PORT))
video_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
nav_socket.setblocking(0)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_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:
while 1:
try:
data = video_socket.recv(65535)
except IOError:
# we consumed every packet from the socket and
# continue with the last one
break
if self.is_ar_drone_2:
self.ar2video.write(data)
# Sending is taken care of by the decoder
else:
w, h, image, t = arvideo.read_picture(data)
self.video_pipe.send(image)
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 = libardrone.decode_navdata(data)
self.nav_pipe.send(navdata)
elif i == self.com_pipe:
_ = self.com_pipe.recv()
stopping = True
break
video_socket.close()
nav_socket.close()
def run(self):
'''
video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
video_socket.setblocking(0)
video_socket.bind(('', libardrone.ARDRONE_VIDEO_PORT))
video_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
'''
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
nav_socket.setblocking(0)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT))
stopping = False
while not stopping:
inputready, outputready, exceptready = select.select(
[nav_socket, self.com_pipe], [], [])
for i in inputready:
'''
if i == video_socket:
while 1:
try:
data = video_socket.recv(65535)
except IOError as err:
print err
# we consumed every packet from the socket and
# continue with the last one
break
w, h, image, t = arvideo.read_picture(data)
self.video_pipe.send(image)
'''
if 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 = libardrone.decode_navdata(data)
self.nav_pipe.send(navdata)
elif i == self.com_pipe:
_ = self.com_pipe.recv()
stopping = True
break
# video_socket.close()
nav_socket.close()
def run(self):
video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
video_socket.setblocking(0)
video_socket.bind(('', libardrone.ARDRONE_VIDEO_PORT))
video_socket.sendto("\x01\x00\x00\x00",
(self.ip, libardrone.ARDRONE_VIDEO_PORT))
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
nav_socket.setblocking(0)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00",
(self.ip, libardrone.ARDRONE_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:
while 1:
try:
data = video_socket.recv(65535)
except IOError:
# we consumed every packet from the socket and
# continue with the last one
break
w, h, image, t = arvideo.read_picture(data)
self.video_pipe.send(image)
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 = libardrone.decode_navdata(data)
# print 'navdata being sent'
self.nav_pipe.send(navdata)
elif i == self.com_pipe:
_ = self.com_pipe.recv()
stopping = True
break
video_socket.close()
nav_socket.close()
def run(self):
print "Nav data thread ready"
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
nav_socket.setblocking(1)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00",
('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT))
data = ""
while not self.stopping:
while 1:
try:
data = nav_socket.recv(65535)
if data is not None:
navdata = libardrone.decode_navdata(data)
if 0 in navdata:
self.drone.navdata = navdata
#self.onNavdataReceive(self.drone.navdata)
except IOError:
break
nav_socket.close()
def decode(self, q_nav, frame, s, time1, time2, cross_program, frame_cross_counter, cross_state):
#MK_default nav data
#cross_state = [False, [0,0]]
state = False
cross_past_states = []
cross_past_states_x = []
cross_past_states_y = []
str_battery = "battery level: " + str("")
str_altitude = "altitude: " + str("")
str_vx = "x-velocity: " + str("")
str_vy = "y-velocity: " + str("")
str_vz = "z-velocity: " + str("")
str_s = "integrated way: " + str("")
str_time = "time: " + str("")
str_phi = "phi: " + str("")
str_psi = "psi: " + str("")
str_theta = "theta: " + str("")
str_cross = "cross identification: " + str("")
str_frames = "frame_cross_counter: " + str("")
nav_info = []
if q_nav.full():
state = True
nav_info = q_nav.get()
nav_info = libardrone.decode_navdata(nav_info)
try:
battery_state = nav_info[0]['battery']
altitude_state = 0.001 * nav_info[0]['altitude']
vx_state = 0.001 * nav_info[0]['vx']
vy_state = 0.001 * nav_info[0]['vy']
vz_state = 0.001 * nav_info[0]['vz']
phi_state = nav_info[0]['phi']
psi_state = nav_info[0]['psi']
theta_state = nav_info[0]['theta']
#MK_printing navigation state
str_battery = "battery level: " + str(battery_state)
str_altitude = "altitude: " + str(altitude_state)
str_vx = "x-velocity: " + str(vx_state)
str_vy = "y-velocity: " + str(vy_state)
str_vz = "z-velocity: " + str(vz_state)
str_s = "integrated way: " + str(s)
str_time = "time: " + str(time1)
str_phi = "phi: " + str(phi_state)
str_psi = "psi: " + str(psi_state)
str_theta = "theta: " + str(theta_state)
str_cross = "cross identification: " + str(cross_program)
str_frames = "frame_cross_counter: " + str(frame_cross_counter)
except:
print "Fail to decode nav_data.."
cv2.putText(frame,str(str_battery),(20,20), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
cv2.putText(frame,str(str_altitude),(20,40), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
cv2.putText(frame,str(str_vx),(20,60), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
cv2.putText(frame,str(str_vy),(20,80), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
cv2.putText(frame,str(str_vz),(20,100), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
cv2.putText(frame,str(str_s),(20,120), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
cv2.putText(frame,str(str_time),(20,140), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
cv2.putText(frame,str(str_phi),(20,160), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
cv2.putText(frame,str(str_psi),(20,180), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
cv2.putText(frame,str(str_theta),(20,200), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
cv2.putText(frame,str(str_cross),(20,220), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
cv2.putText(frame,str(str_frames),(20,240), cv2.FONT_HERSHEY_PLAIN, 1.0,(0,255,0))
#print cross_state[1][0],cross_state[1][1]
cv2.circle(frame, (cross_state[1][0], cross_state[1][1]), 10, (255, 0, 0), -1)
return frame, nav_info, state
def run(self):
def _connect():
logging.warn('Connection to ardrone')
if self.is_ar_drone_2:
video_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
video_socket.connect(('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
video_socket.setblocking(0)
else:
video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
video_socket.setblocking(0)
video_socket.bind(('', libardrone.ARDRONE_VIDEO_PORT))
video_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
nav_socket.setblocking(0)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT))
control_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
control_socket.connect(('192.168.1.1', libardrone.ARDRONE_CONTROL_PORT))
control_socket.setblocking(0)
time.sleep(1.0)
com_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
com_socket.connect(('127.0.0.1', 17482))
com_socket.setblocking(0)
logging.warn('Connection established')
return video_socket, nav_socket, control_socket, com_socket
def _disconnect(video_socket, nav_socket, control_socket):
logging.warn('Disconnection to ardrone streams')
video_socket.close()
nav_socket.close()
control_socket.close()
video_socket, nav_socket, control_socket, com_socket = _connect()
self.stopping = False
connection_lost = 1
reconnection_needed = False
while not self.stopping:
if reconnection_needed:
_disconnect(video_socket, nav_socket, control_socket)
video_socket, nav_socket, control_socket, com_socket = _connect()
reconnection_needed = False
inputready, outputready, exceptready = select.select([nav_socket, video_socket, com_socket, control_socket], [], [], 1.)
if len(inputready) == 0:
connection_lost += 1
reconnection_needed = True
for i in inputready:
if i == video_socket:
while 1:
try:
data = video_socket.recv(65536)
if self.is_ar_drone_2:
self.ar2video.write(data)
except IOError:
# we consumed every packet from the socket and
# continue with the last one
break
# Sending is taken care of by the decoder
if not self.is_ar_drone_2:
w, h, image, t = arvideo.read_picture(data)
self._drone.set_image(image)
elif i == nav_socket:
while 1:
try:
data = nav_socket.recv(500)
except IOError:
# we consumed every packet from the socket and
# continue with the last one
break
navdata, has_information = libardrone.decode_navdata(data)
if (has_information):
self._drone.set_navdata(navdata)
elif i == self.com_pipe:
_ = self.com_pipe.recv()
self.stopping = True
break
elif i == com_socket:
# print "got die!"
self.stopping = True
break
elif i == control_socket:
reconnection_needed = False
while not reconnection_needed:
try:
data = control_socket.recv(65536)
if len(data) == 0:
logging.warning('Received an empty packet on control socket')
reconnection_needed = True
else:
logging.warning("Control Socket says : %s", data)
except IOError:
break
_disconnect(video_socket, nav_socket, control_socket)
def run(self):
def _connect():
logging.warn('Connection to ardrone')
if self.is_ar_drone_2:
video_socket = socket.socket(socket.AF_INET,
socket.SOCK_STREAM)
video_socket.connect(
('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
video_socket.setblocking(0)
else:
video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
video_socket.setblocking(0)
video_socket.bind(('', libardrone.ARDRONE_VIDEO_PORT))
video_socket.sendto(
"\x01\x00\x00\x00",
('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
nav_socket.setblocking(0)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00",
('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT))
control_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
control_socket.connect(
('192.168.1.1', libardrone.ARDRONE_CONTROL_PORT))
control_socket.setblocking(0)
logging.warn('Connection established')
return video_socket, nav_socket, control_socket
def _disconnect(video_socket, nav_socket, control_socket):
logging.warn('Disconnection to ardrone streams')
video_socket.close()
nav_socket.close()
control_socket.close()
video_socket, nav_socket, control_socket = _connect()
stopping = False
connection_lost = 1
reconnection_needed = False
while (not stopping) and (not self.stopped()):
if reconnection_needed:
_disconnect(video_socket, nav_socket, control_socket)
video_socket, nav_socket, control_socket = _connect()
reconnection_needed = False
inputready, outputready, exceptready = select.select(
[nav_socket, video_socket, self.com_pipe, control_socket], [],
[], 1.)
if len(inputready) == 0:
connection_lost += 1
reconnection_needed = True
for i in inputready:
if i == video_socket:
while 1:
try:
data = video_socket.recv(65536)
if self.is_ar_drone_2:
self.ar2video.write(data)
except IOError:
# we consumed every packet from the socket and
# continue with the last one
break
# Sending is taken care of by the decoder
if not self.is_ar_drone_2:
w, h, image, t = arvideo.read_picture(data)
self._drone.set_image(image)
elif i == nav_socket:
while 1:
try:
data = nav_socket.recv(500)
except IOError:
# we consumed every packet from the socket and
# continue with the last one
break
navdata, has_information = libardrone.decode_navdata(data)
if (has_information):
self._drone.set_navdata(navdata)
elif i == self.com_pipe:
_ = self.com_pipe.recv()
stopping = True
break
elif i == control_socket:
reconnection_needed = False
while not reconnection_needed:
try:
data = control_socket.recv(65536)
if len(data) == 0:
logging.warning(
'Received an empty packet on control socket'
)
reconnection_needed = True
else:
logging.warning("Control Socket says : %s",
data)
except IOError:
break
_disconnect(video_socket, nav_socket, control_socket)
def run(self):
def _connect():
logging.warn('Connexion vers AR Drone en cours ...')
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
nav_socket.setblocking(0)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT))
control_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
control_socket.connect(('192.168.1.1', libardrone.ARDRONE_CONTROL_PORT))
control_socket.setblocking(0)
logging.warn('Connection etablie')
return nav_socket, control_socket
def _disconnect(nav_socket, control_socket):
logging.warn('Deconnection AR Drone')
nav_socket.close()
control_socket.close()
nav_socket, control_socket = _connect()
stopping = False
connection_lost = 1
reconnection_needed = False
while not stopping:
if reconnection_needed:
_disconnect(nav_socket, control_socket)
nav_socket, control_socket = _connect()
reconnection_needed = False
inputready, outputready, exceptready = select.select([nav_socket, self.com_pipe, control_socket], [], [], 1.)
if len(inputready) == 0:
connection_lost += 1
reconnection_needed = True
for i in inputready:
if i == nav_socket:
while 1:
try:
data = nav_socket.recv(500)
except IOError:
# we consumed every packet from the socket and
# continue with the last one
break
navdata, has_information = libardrone.decode_navdata(data)
if (has_information):
self._drone.set_navdata(navdata)
elif i == self.com_pipe:
_ = self.com_pipe.recv()
stopping = True
break
elif i == control_socket:
reconnection_needed = False
while not reconnection_needed:
try:
data = control_socket.recv(65536)
if len(data) == 0:
logging.warning('Received an empty packet on control socket')
reconnection_needed = True
else:
logging.warning("Control Socket says : %s", data)
except IOError:
break
_disconnect(nav_socket, control_socket)
def run(self):
def _connect():
logging.warn('Connexion vers AR Drone en cours ...')
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM,
socket.IPPROTO_UDP)
nav_socket.setblocking(0)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00",
('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT))
control_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
control_socket.connect(
('192.168.1.1', libardrone.ARDRONE_CONTROL_PORT))
control_socket.setblocking(0)
logging.warn('Connection etablie')
return nav_socket, control_socket
def _disconnect(nav_socket, control_socket):
logging.warn('Deconnection AR Drone')
nav_socket.close()
control_socket.close()
nav_socket, control_socket = _connect()
stopping = False
connection_lost = 1
reconnection_needed = False
while not stopping:
if reconnection_needed:
_disconnect(nav_socket, control_socket)
nav_socket, control_socket = _connect()
reconnection_needed = False
inputready, outputready, exceptready = select.select(
[nav_socket, self.com_pipe, control_socket], [], [], 1.)
if len(inputready) == 0:
connection_lost += 1
reconnection_needed = True
for i in inputready:
if i == nav_socket:
while 1:
try:
data = nav_socket.recv(500)
except IOError:
# we consumed every packet from the socket and
# continue with the last one
break
navdata, has_information = libardrone.decode_navdata(data)
if (has_information):
self._drone.set_navdata(navdata)
elif i == self.com_pipe:
_ = self.com_pipe.recv()
stopping = True
break
elif i == control_socket:
reconnection_needed = False
while not reconnection_needed:
try:
data = control_socket.recv(65536)
if len(data) == 0:
logging.warning(
'Received an empty packet on control socket'
)
reconnection_needed = True
else:
logging.warning("Control Socket says : %s",
data)
except IOError:
break
_disconnect(nav_socket, control_socket)
def run(self):
video_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
video_socket.setblocking(0)
video_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
video_socket.bind(('', libardrone.ARDRONE_VIDEO_PORT))
video_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_VIDEO_PORT))
try:
nav_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
nav_socket.setblocking(0)
nav_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
nav_socket.bind(('', libardrone.ARDRONE_NAVDATA_PORT))
nav_socket.sendto("\x01\x00\x00\x00", ('192.168.1.1', libardrone.ARDRONE_NAVDATA_PORT))
except:
pass
video_dump_file = open("/tmp/arvideo_dump.mjpeg", 'w')
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:
while 1:
try:
data = video_socket.recv(65535)
except IOError:
# we consumed every packet from the socket and
# continue with the last one
break
# AMN temporary dump to file to see what's what in video decoding land...
# video_dump_file.write(data)
#w, h, image, t = arvideo.read_picture(data)
#self.video_pipe.send(image)
# AMN removed video handling for now, as it's slow as treacle and I'm not using it yet!
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 = libardrone.decode_navdata(data)
self.nav_pipe.send(navdata)
elif i == self.com_pipe:
_ = self.com_pipe.recv()
stopping = True
break
try:
video_socket.shutdown(socket.SHUT_RDWR)
video_dump_file.close()
except:
pass
video_socket.close()
try:
nav_socket.shutdown(socket.SHUT_RDWR)
except:
pass
nav_socket.close()
