text: #########
  # ps_drone.py
  # (w)+(c) J. Philipp de Graaff, www.playsheep.de, drone@playsheep.de, 2012-2015
  # Project homepage: www.playsheep.de/drone and https://sourceforge.net/projects/ps-drone/
  # Dependencies: a POSIX OS, openCV2 for video-support.
  # Base-program of the PS-Drone API: "An open and enhanced API for universal control of the Parrot AR.Drone 2.0 quadcopter."
  ##########
  # Modified and advanced version, based on a part of the master of computer science degree dissertation "Universelle
  #   Kontrolle und Ueberwachung einer Parrot AR.Drone 2.0 auf Basis eines offenen und erweiterten Toolkits"
  #   by J. Philipp de Graaff, faculty of computer science, Prof. Dr. Hedrich, at the University of Frankfurt / Germany
  #   Linked at http://www.em.cs.uni-frankfurt.de/index.php?id=43&L=1
  # For further details, information, documentation or tutorials visit: www.playsheep.de/drone
  ##########
  # LICENCE:
  #   Artistic License 2.0 as seen on http://opensource.org/licenses/artistic-license-2.0 (retrieved December 2014)
  #   If the terms of this license do not permit the full use that you propose to make of PS-Drone, please contact me for a
  #   different licensing arrangement.
  #   Visit www.playsheep.de/drone or see the PS-Drone-API-documentation for an abstract from the Artistic License 2.0.
  ##########
  # Dedicated to my beloved wife.
  ###########
  
  import threading, select, socket, time, tempfile, multiprocessing, struct, os, sys
  import thread, signal, subprocess
  
  if os.name == 'posix':	import termios, fcntl	# for getKey(), ToDo: Reprogram for Windows
   
  commitsuicideV, showVid, vCruns, lockV, debugV =	False, False, False, threading.Lock(), False	# Global variables for video-decoding
  offsetND, suicideND, commitsuicideND = 0, False, False												# Global variables for NavDava-decoding
  
  
  #Neu:
  #	changeIP
  # Video Detection
  
  
  class Drone(object):
  ######################################=-
  ### Start and stop using the drone ###=-
  ######################################=-
  	###### Bootup and base configuration
  	def __init__(self):
  		self.__Version = 		"2.1.3"
  		self.__lock = 			threading.Lock()	# To prevent semaphores
  		self.__startTime = 		time.time()
  		self.__speed = 			0.2					# Default drone moving speed in percent.
  		self.showCommands = 	False				# Shows all sent commands (but not the keepalives)
  		self.debug = 			False				# Shows some additional debug information
  		self.valueCorrection = 	False
  		self.selfRotation = 	0.0185				# use this value, if not checked by getSelfRotation()
  		self.stopOnComLoss = 	False				# when there is a communication-problem, drone will land or not
  		
  		# Drone communication variables
  		self.DroneIP = 		"192.168.1.1"
  		self.NavDataPort = 	5554
  		self.VideoPort = 	5555
  		self.CmdPort = 		5556
  		self.CTLPort = 		5559
  
  		# NavData variables
  		self.__NavData = 				""
  		self.__State = 					[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
  		self.__NavDataCount = 			0
  		self.__NavDataTimeStamp = 		0.0
  		self.__NavDataDecodingTime = 	0.0
  		self.__NoNavData = 				False
  
  		# Video variables
  		self.__VideoImage = 			None
  		self.__VideoImageCount = 		0
  		self.__VideoDecodeTimeStamp = 	0
  		self.__VideoDecodeTime = 		0
  		self.__VideoReady = 			False
  		self.__vKey =					""
  		self.__SaveVideo = 				False
  		
  		# Config variables
  		self.__ConfigData = 			[]
  		self.__ConfigDataCount = 		0
  		self.__ConfigDataTimeStamp = 	0
  		self.__ConfigSending = 			True
  		self.__ConfigSessionID = 		"03016321"
  		self.__ConfigUserID = 			"0a100407"
  		self.__ConfigApplicationID = 	"03016321"
  		self.sendConfigSaveMode = 		False
  		
  		# Internal variables
  		self.__NavDataProcess =			""
  		self.__VideoProcess =			""
  		self.__vDecodeProcess =			""
  		self.__ConfigQueue =			[]
  		self.__networksuicide =			False
  		self.__receiveDataRunning =		False
  		self.__sendConfigRunning =		False
  		self.__shutdown = 				False
  		self.__pDefaultStr = 			"\033[0m"
  		self.__pRedStr = 				"\033[91m"
  		self.__pGreenStr = 				"\033[92m"
  		self.__pYellowStr = 			"\033[93m"
  		self.__pBlueStr = 				"\033[94m"
  		self.__pPurpleStr = 			"\033[95m"
  		self.__pLineUpStr = 			"\033[1A"
  		
  	###### Connect to the drone and start all procedures
  	def startup(self):
  		# Check for drone in the network and wake it up
  		try:
  			socket.socket().connect((self.DroneIP, 21))
  			socket.socket().close()
  		except:
  			self.printRed()
  			print "Drone is not online"
  			self.printDefault()
  			sys.exit(9)
  
  		# Internal variables
  		self.__CmdCounter = 3											# as there are two raw commands, send next steps
  		self.__calltime = 	0											# to get some time-values to debug
  
  		#send the first four initial-commands to the drone
  		self.__sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)	# Open network connection
  		self.__sock.setblocking(0)										# Network should not block
  		self.__sendrawmsg("\r")											# Wakes up command port
  		time.sleep(0.01)
  		self.__sendrawmsg("AT*PMODE=1,2\rAT*MISC=2,2,20,2000,3000\r")	# Initialising drone as sniffed from datastream demo-tool to AR.Drone
  
  		##### Initialising timed thread(s) for drone communication
  		# Opening NavData- and Video- Processes
  		self.__VidPipePath = tempfile.gettempdir()+"/dronevid-"+str(threading.enumerate()[0])[-12:-2]+"-"+str(time.time())[-7:].replace(".","")+".h264"
  		self.__net_pipes = []
  		self.__NavData_pipe, navdataChild_pipe 		  = multiprocessing.Pipe()
  		self.__Video_pipe,   videoChild_pipe          = multiprocessing.Pipe()
  		self.__vdecode_pipe, self.__vdecodeChild_pipe = multiprocessing.Pipe()
  
  		self.__NavDataProcess = multiprocessing.Process( target=mainloopND, args=(self.DroneIP,self.NavDataPort,navdataChild_pipe,os.getpid()))
  		self.__NavDataProcess.start()
  		self.__VideoProcess =   multiprocessing.Process( target=mainloopV, args=(self.DroneIP,self.VideoPort,self.__VidPipePath,videoChild_pipe,os.getpid()))
  		self.__VideoProcess.start()
  		self.__vDecodeProcess = multiprocessing.Process( target=vDecode, args=(self.__VidPipePath,self.__vdecodeChild_pipe,os.getpid()))
  		# There is a third process called "self.__vDecodeProcess" for decoding video, initiated and started around line 880
  
  		# Final settings
  		self.useDemoMode(True) 		# This entry is necessary for the drone's firmware, otherwise the NavData contains just header and footer
  		self.setConfig("custom:session_id","-all")	
  		self.getNDpackage(["demo"])
  
  		time.sleep(1)
  		#setup Network-thread
  		while not self.__receiveDataRunning or not self.__sendConfigRunning or len(self.__ConfigQueue):		# sometimes they would not start why ever, so TK has to double-check
  			if not self.__receiveDataRunning:
  				self.__threadReceiveData=threading.Thread(target=self.__receiveData)
  				self.__threadReceiveData.start()
  				time.sleep(0.05)
  			if not self.__sendConfigRunning:
  				self.__threadSendConfig=threading.Thread(target=self.__sendConfig)
  				self.__threadSendConfig.start()
  				time.sleep(0.05)
  			time.sleep(0.01)
  		
  	###### Clean Shutdown
  	def shutdown(self):								
  		if self.__shutdown:	sys.exit()
  		self.__shutdown = True
  		if self.debug: print "Shutdown..."
  		self.land()
  		self.thrust(0,0,0,0)
  		try:	self.__NavData_pipe.send("die!")
  		except:	pass
  		self.__Video_pipe.send("uninit")
  		t=time.time()
  		while	self.__VideoReady and (time.time()-t)<5:	time.sleep(0.1)
  		try:	self.__Video_pipe.send("die!")
  		except:	pass
  
  		time.sleep(0.5)
  		try:	self.__VideoProcess.terminate()
  		except:	pass
  		try:	self.__vDecodeProcess.terminate()
  		except:	pass
  		try:	self.__NavDataProcess.terminate()
  		except:	pass
  
  		self.__stopnetwork()
  		try:	self.__threadSendConfig.join()
  		except:	pass
  		try:	self.__threadReceiveData.join()
  		except:	pass
  		self.__keepalive.cancel()
  		sys.exit()
  
  ##############################################################=-
  ### Make internal variables to external read-only variables ###=-
  ##############################################################=-
  	@property
  	def Version(self):				return self.__Version
  	@property
  	def startTime(self):			return self.__startTime
  	@property
  	def speed(self):				return self.__speed
  	@property
  	def NavData(self):				return self.__NavData
  	@property
  	def State(self):				return self.__State
  	@property
  	def NavDataCount(self):			return self.__NavDataCount
  	@property
  	def NavDataTimeStamp(self):		return self.__NavDataTimeStamp
  	@property
  	def NavDataDecodingTime(self):	return self.__NavDataDecodingTime
  	@property
  	def NoNavData(self):			return self.__NoNavData
  	@property
  	def VideoImage(self):			return self.__VideoImage
  	@property
  	def VideoImageCount(self):		return self.__VideoImageCount
  	@property
  	def VideoDecodeTimeStamp(self):	return self.__VideoDecodeTimeStamp
  	@property
  	def VideoDecodeTime(self):		return self.__VideoDecodeTime
  	@property
  	def VideoReady(self):			return self.__VideoReady
  	@property
  	def SaveVideo(self):			return self.__SaveVideo
  	@property
  	def ConfigData(self):			return self.__ConfigData
  	@property	
  	def ConfigDataCount(self):		return self.__ConfigDataCount
  	@property
  	def ConfigDataTimeStamp(self):	return self.__ConfigDataTimeStamp
  	@property
  	def ConfigSending(self):		return self.__ConfigSending
  	@property
  	def ConfigSessionID(self):		return self.__ConfigSessionID
  	@property
  	def ConfigUserID(self):			return self.__ConfigUserID
  	@property	
  	def ConfigApplicationID(self):	return self.__ConfigApplicationID
  		
  ######################=-
  ### Drone commands ###=-
  ######################=-
  	###### Commands for configuration
  	# change some value
  	def setConfig(self, name, value):								# e.g. drone.setConfig(control:altitude_max","5000")
  		self.__ConfigQueue.append([str(name), str(value), False])	# Note: changes are not immediately and could take some time
  
  	# change some value and send the configuration Identifier (sendConfigIDs) ahead
  	def setMConfig(self, name, value):								# Usage like setConfig
  		self.__ConfigQueue.append([str(name), str(value), True])	# Note: changes are not immediately and could take some time
  
  	# get actual configuration
  	def getConfig(self):											# Stored in "ConfigData"
  		self.at("CTRL", [5,0])										# Wow, that is new, was not necessary before
  		self.at("CTRL", [4,0])										# Note: Actual configuration data will be received after setting...
  		if self.showCommands:		self.__calltime = time.time()	# 	... automatically. An update will take up to 0.015 sec)
  
  	# setting IDs to store Konfigurations for later
  	def setConfigSessionID(self, *args):
  		try:
  			value = float(*args[0])
  			self.__ConfigSessionID = normalLen8(value)
  			self.setConfig("custom:session_id", self.__ConfigSessionID)		
  		except:		return (self.__ConfigSessionID)
  
  	def setConfigUserID(self, *args):
  		try:
  			value = float(*args[0])
  			self.__ConfigUserID = normalLen8(value)
  			self.setConfig("custom:profile_id", self.__ConfigUserID)
  		except:		return (self.__ConfigUserID)
  
  	def setConfigApplicationID(self, *args):
  		try:
  			value = float(*args[0])
  			self.__ConfigApplicationID = normalLen8(value)
  			self.setConfig("custom:application_id", self.__ConfigApplicationID)
  		except:		return (self.__ConfigApplicationID)
  
  	def setConfigAllID(self):
  		self.setConfig("custom:session_id", self.__ConfigSessionID)		
  		self.setConfig("custom:profile_id", self.__ConfigUserID)
  		self.setConfig("custom:application_id", self.__ConfigApplicationID)
  
  	# Reminds the drone which IDs it has to use (important for e.g. switch cameras)
  	def sendConfigIDs(self):
  		self.at("CONFIG_IDS", [self.__ConfigSessionID,self.__ConfigUserID,self.__ConfigApplicationID])
  
  	###### Calibration
  	def trim(self):
  		self.at("FTRIM", [])
  
  	def mtrim(self):
  		self.at("CALIB", [0])
  	
  	def mantrim(self, thetaAngle, phiAngle, yawAngle):		# manual Trim
  		if self.valueCorrection:
  			try:		thetaAngle = 		float(thetaAngle)
  			except:		thetaAngle = 		0.0
  			try:		phiAngle = 			float(phiAngle)
  			except:		phiAngle = 			0.0
  			try:		yawAngle = 			float(yawAngle)
  			except:		yawAngle = 			0.0
  		self.at("MTRIM", [thetaAngle,phiAngle,yawAngle])	# floats
  
  	def getSelfRotation(self, wait):
  		if self.valueCorrection:
  			try:		wait = float(wait)
  			except:		wait = 1.0
  		reftime = time.time()
  		oangle = self.__NavData["demo"][2][2]				# detects the self-rotation-speed of the yaw-sensor
  		time.sleep(wait)
  		self.selfRotation = (self.__NavData["demo"][2][2]-oangle)/(time.time()-reftime)
  		return self.selfRotation
  
  	###### Movement
  	# Default speed of movement
  	def setSpeed(self, *speed):
  		try:	self.__speed = self.__checkSpeedValue(*speed)
  		except: pass
  		return self.__speed
  
  	# Absolute movement in x, y and z-direction and rotation
  	def move(self, leftright, backwardforward, downup, turnleftright):	# Absolute movement in x, y and z-direction and rotation
  		if self.valueCorrection:
  			try:		leftright = 		float(leftright)
  			except:		leftright = 		0.0
  			try:		backwardforward = 	float(backwardforward)
  			except:		backwardforward = 	0.0
  			try:		downup = 			float(downup)
  			except:		downup = 			0.0
  			try:		turnleftright = 	float(turnleftright)
  			except:		turnleftright = 	0.0
  		if leftright >  1.0:			leftright =	 		 1.0
  		if leftright < -1.0:			leftright =			-1.0
  		if backwardforward >  1.0:		backwardforward =	 1.0
  		if backwardforward < -1.0:		backwardforward =	-1.0
  		if downup >  1.0:				downup =			 1.0
  		if downup < -1.0:				downup =			-1.0
  		if turnleftright >  1.0:		turnleftright =		 1.0
  		if turnleftright < -1.0:		turnleftright =		-1.0
  		self.at("PCMD", [3 ,leftright, -backwardforward, downup, turnleftright])
  
  	# Relative movement to controller in x, y and z-direction and rotation
  	def relMove(self, leftright, backwardforward, downup, turnleftright, eastwest, northturnawayaccuracy):
  		if self.valueCorrection:
  			try:		leftright = 		float(leftright)
  			except:		leftright = 		0.0
  			try:		backwardforward = 	float(backwardforward)
  			except:		backwardforward = 	0.0
  			try:		downup = 			float(downup)
  			except:		downup = 			0.0
  			try:		turnleftright = 	float(turnleftright)
  			except:		turnleftright = 	0.0
  		if leftright >  1.0:			leftright = 		 1.0
  		if leftright < -1.0:			leftright =			-1.0
  		if backwardforward >  1.0:		backwardforward =	 1.0
  		if backwardforward < -1.0:		backwardforward =	-1.0
  		if downup >  1.0:				downup =			 1.0
  		if downup < -1.0:				downup =			-1.0
  		if turnleftright >  1.0:		turnleftright =		 1.0
  		if turnleftright < -1.0:		turnleftright =		-1.0
  		self.at("PCMD_MAG", [1 ,leftright, -backwardforward, downup, turnleftright, eastwest, northturnawayaccuracy])
  
  	# Stop moving
  	def hover(self):
  		self.at("PCMD", [0,0.0,0.0,0.0,0.0])
  	def stop(self):	# Hammertime !
  		self.hover()
  
  	# Basic movements
  	def moveLeft(self,*args):
  		try:	speed=args[0]
  		except:	speed=self.__speed
  		self.move(-self.__checkSpeedValue(speed),0.0,0.0,0.0)
  
  	def moveRight(self,*args):
  		try:	speed=args[0]
  		except:	speed=self.__speed
  		self.move( self.__checkSpeedValue(speed),0.0,0.0,0.0)
  
  	def moveForward(self,*args):
  		try:	speed=args[0]
  		except:	speed=self.__speed
  		self.move(0.0, self.__checkSpeedValue(speed),0.0,0.0)
  
  	def moveBackward(self,*args):
  		try:	speed=args[0]
  		except:	speed=self.__speed
  		self.move(0.0,-self.__checkSpeedValue(speed),0.0,0.0)
  
  	def moveUp(self,*args):
  		try:	speed=args[0]
  		except:	speed=self.__speed
  		self.move(0.0,0.0, self.__checkSpeedValue(speed),0.0)
  
  	def moveDown(self,*args):
  		try:	speed=args[0]
  		except:	speed=self.__speed
  		self.move(0.0,0.0,-self.__checkSpeedValue(speed),0.0)
  
  	def turnLeft(self,*args):
  		try:	speed=args[0]
  		except:	speed=self.__speed
  		self.move(0.0,0.0,0.0,-self.__checkSpeedValue(speed))
  
  	def turnRight(self,*args):
  		try:	speed=args[0]
  		except:	speed=self.__speed
  		self.move(0.0,0.0,0.0, self.__checkSpeedValue(speed))
  
  	# Lets the drone rotate defined angle
  	# BUG: does not work with 180deg. turns
  	# ToDo: Should be able to stop in case of failures
  	def turnAngle(self,ndir,speed,*args):
  		opos = 		self.__NavData["demo"][2][2]			# get the source/current (original) angle
  		npos = 		opos+ndir								# calculate the destination (new) angle
  		minaxis = 	opos									# to make sure, that the jump from -180 to 180 will...
  		maxaxis = 	opos									# ...be correctly handled
  		speed = 	self.__checkSpeedValue(speed)
  		ospeed = 	speed									# stores the given speed-value
  		reftime = 	time.time()
  		accurateness = 0		
  		try:	accurateness = args[0]
  		except:	pass
  		if accurateness<=0:
  			accurateness = 0.005							# Destination angle can differ +/- this value (not demo-mode)
  			if self.__State[10]:	accurateness = 0.1		# Destination angle can differ +/- this value in demo-mode
  		stop =		False
  		counter =	0
  		direction =	0										# -1 = left | 1 = right     To prevent endless loops (happends sometimes, whyever)
  		while not stop and counter<=5:
  			ndc = self.__NavDataCount						# wait for the next NavData-package
  			while ndc == self.__NavDataCount:		time.sleep(0.001)
  			kalib = (time.time()-reftime)*self.selfRotation	# trys to recalibrate, causing moving sensor-values around 0.0185 deg/sec
  			cpos = self.__NavData["demo"][2][2]				# get the current angle
  			if minaxis > cpos:			minaxis = cpos		# set the minimal seen angle
  			if maxaxis < cpos:			maxaxis = cpos		# set the maximal seen angle
  			if cpos-minaxis >= 180:		cpos = cpos-360		# correct the angle-value if necessary...
  			elif maxaxis-cpos >= 180:	cpos = cpos+360		# ...for an easier calculation
  			speed = abs(cpos-npos+kalib) / 10.0				# the closer to the destination the slower the drone turns
  			if speed > ospeed:			speed = ospeed		# do not turn faster than recommended
  			if speed < 0.05:			speed = 0.05		# too slow turns causes complications with calibration
  			self.__speed = speed
  			if cpos > (npos+kalib):							# turn left, if destination angle is lower
  				self.turnLeft()
  				if direction   ==  0:	direction = -1
  				elif direction ==  1:	direction, counter = -1, counter+1
  			else:											# turn right if destination angle is higher
  				self.turnRight()
  				if direction   ==  0:	direction = 1
  				elif direction == -1:	direction, counter = 1, counter+1
  
  			if cpos < (npos+kalib+accurateness) and cpos > (npos+kalib-accurateness):# if angle is reached...
  				self.stop()									# ...stop turning
  				time.sleep(0.01)
  				stop = True
  		return(True)
  
  	def takeoff(self):
  		self.at("REF", [290718208]) #290718208=10001010101000000001000000000
  
  	def land(self):
  		self.at("REF", [290717696]) #290717696=10001010101000000000000000000
  
  	###### NavData commands
  	# Switches to Demo- or Full-NavData-mode
  	def useDemoMode(self,value):
  		if value:	self.setConfig("general:navdata_demo", "TRUE")
  		else:		self.setConfig("general:navdata_demo", "FALSE")
  
  	def useMDemoMode(self,value):
  		if value:	self.setMConfig("general:navdata_demo", "TRUE")
  		else:		self.setMConfig("general:navdata_demo", "FALSE")
  
  	def getNDpackage(self,packets):
  		self.__NavData_pipe.send(("send",packets))
  
  	def addNDpackage(self,packets):
  		self.__NavData_pipe.send(("add",packets))
  
  	def delNDpackage(self,packets):
  		self.__NavData_pipe.send(("block",packets))
  
  	def reconnectNavData(self):
  		self.__NavData_pipe.send("reconnect")
  
  	###### Video & Marker commands
  	# This makes the drone fly around and follow 2D tags which the camera is able to detect.
  	def aflight(self, flag):
  	    self.at("AFLIGHT", [flag])	#Integer: 1: start flight, 0: stop flight
  
  	def slowVideo(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:	self.__Video_pipe.send("slowVideo")
  		else:	self.__Video_pipe.send("fastVideo")
  
  	def midVideo(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:	self.__Video_pipe.send("midVideo")
  		else:	self.__Video_pipe.send("fastVideo")
  
  	def fastVideo(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:	self.__Video_pipe.send("fastVideo")
  		else:	self.__Video_pipe.send("slowVideo")
  
  	def saveVideo(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:	self.__Video_pipe.send("saveVideo")
  		else:	self.__Video_pipe.send("unsaveVideo")
  
  	def startVideo(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:	self.__Video_pipe.send("init")
  		else:	self.stopVideo()
  
  	def stopVideo(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:	self.__Video_pipe.send("uninit")
  		else:	self.startVideo()
  
  	def showVideo(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:
  				self.__Video_pipe.send("init")
  				self.__Video_pipe.send("show")
  		else:	self.hideVideo()
  
  	def hideVideo(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:
  				self.__Video_pipe.send("init")
  				self.__Video_pipe.send("hide")
  		else:	self.showVideo()
  
  	# Selects which video stream to send on the video UDP port.
  	def hdVideo(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:	self.setMConfig("video:video_codec","131")
  		else:	self.setMConfig("video:video_codec","129")
  
  	def sdVideo(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:	self.setMConfig("video:video_codec","129")
  		else:	self.setMConfig("video:video_codec","131")
  
  	def mp4Video(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:	self.setMConfig("video:video_codec","128")
  		else:	self.setMConfig("video:video_codec","129")
  
  	# Selects which video-framerate (in frames per second) to send on the video UDP port.
  	def videoFPS(self, fps):
  		try:
  			int(fps)
  			if fps>60:	fps = 60
  			elif fps<1:	fps = 1
  			self.setMConfig("video:codec_fps",fps)
  		except:		pass
  
  	# Selects which video-bitrate (in kilobit per second) to send on the video UDP port.
  	def videoBitrate(self, bitrate):
  		try:
  			int(bitrate)
  			if bitrate > 20000:	bitrate = 20000
  			if bitrate < 250:	bitrate = 250
  			self.setMConfig("video:bitrate",bitrate)
  		except:		pass
  
  	# Selects which video stream to send on the video UDP port.
  	def frontCam(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:	self.setMConfig("video:video_channel","0")
  		else:	self.setMConfig("video:video_channel","1")
  
  	def groundCam(self, *args):
  		try:	do = args[0]
  		except:	do = True
  		if do:	self.setMConfig("video:video_channel","1")
  		else:	self.setMConfig("video:video_channel","0")
  
  ### Misc commands
  	def reset(self):
  		if self.NavDataCount>0 and self.State[31]==1:
  			self.at("REF", [290717952]) #290717952=10001010101000000000100000000
  
  	def thrust(self, fl, fr, rl, rr):	# Controls engines directly, overriding control loops.
  		fl *= 2
  		if fl > 64000:	fl = 64000
  		elif fl < 0:	fl = 0
  		fr *= 2
  		if fr > 64000:	fr = 64000
  		elif fr < 0:	fr = 0
  		rl *= 2
  		if rl > 64000:	rl = 64000
  		elif rl < 0:	rl = 0
  		rr *= 2
  		if rr > 64000:	rr = 64000
  		elif rr < 0:	rr = 0
  
  
  		self.at("PWM", [int(fl), int(fr), int(rr), int(rl)])
  		# Seems that integer-values could be between 0 (stop) to 511 (full); more than 511 seem to have no effect.
  		# Beware: if using too high values (e.g. floats (>64k ?)), there will be side-effects like restarting other motors, etc.
  		# Drone will shut down, if its flight-angle is more than set.
  
  #   Control the drone's LED.
  	def led(self, animation, frequency, duration):	
  		if animation < 21 and frequency > 0 and duration >= 0:
  			self.at("LED", [animation, float(frequency), duration])
  
  #   Makes the drone execute a predefined movement (animation).
  	def anim(self, animation, duration):
  		if animation < 20 and duration >= 0:
  			self.at("ANIM", [animation, duration])
  
  	def changeIP(self, IP):
  		try:
  			import telnetlib
  			tnet	= telnetlib.Telnet(self.DroneIP)
  			tnet.write("ifconfig ath0 down;ifconfig ath0 "+IP+" netmask 255.255.255.0;ifconfig ath0 up\n")
  			tnet.close()
  			self.DroneIP=IP
  			OK		= True
  		except:
  			OK		= False
  		return OK
  
  #########################=-
  ### Low-level Commands ###=-
  #########################=-
  
  	# Upgrading the basic drone commands to low-level drone commands:vid
  	# Adding command-number, checking the values, convert 32-bit float to 32-bit integer and put it in quotes
  	def at(self, command, params):
  		self.__lock.acquire()
  		paramLn = ""
  		if params:	
  			for p in params:
  				if type(p) 	 == int:	paramLn += ","+str(p)
  				elif type(p) == float:	paramLn += ","+str(struct.unpack("i", struct.pack("f", p))[0])
  				elif type(p) == str:	paramLn += ",\""+p+"\""
  		msg = "AT*"+command+"="+str(self.__CmdCounter)+paramLn+"\r"
  		self.__CmdCounter += 1
  		self.__sendrawmsg(msg)
  		self.__lock.release()
  
  
  	# Sending the low-level drone-readable commands to the drone...better do not use
  	def __sendrawmsg(self, msg):
  		try:		self.__keepalive.cancel()
  		except:		pass
  		if self.showCommands:
  			if msg.count("COMWDG") < 1:	print msg
  		self.__sock.sendto(msg, (self.DroneIP, self.CmdPort))
  		self.__keepalive = threading.Timer(0.1, self.__heartbeat)
  		self.__keepalive.start()
  
  
  #############################=-
  ###  Convenient Commands  ###=-
  #############################=-
  #    Just add water
  	# Checks the battery-status
  	def getBattery(self):
  		batStatus =	"OK"
  		batValue =	0
  		if self.__State[15] == 1:	batStatus = "empty"
  		try:	batValue = self.__NavData['demo'][1]
  		except:	batValue = -1
  		return (batValue,batStatus)	# Percent & status ("OK", "empty")
  
  	# Calculates the minor difference between two angles as the drone gives values from -180 to 180...
  	# ...so e.g. 170 and -160 are +30 difference and drone will turn to the correct direction
  	def angleDiff(self, base, value):
  		adiff = ((base+180)-(value+180)) %360
  		if adiff>180: adiff-=360
  		return adiff
  
  	# Grabs the pressed key (not yet for Windows)
  	# ToDo: Reprogram for Windows
  	def getKey(self):
  		key =	""
  		fd =	sys.stdin.fileno()
  		if os.name == 'posix':
  			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)
  			try:        
  				try:	key = sys.stdin.read(1)
  				except IOError: pass
  			finally:
  				termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm)
  				fcntl.fcntl(fd, fcntl.F_SETFL, oldflags)
  		if os.name == 'nt':
  			if msvcrt.kbhit():	key = msvcrt.getch()
  		key += self.__vKey
  		self.__vKey = ""
  		return key
  
  	# Drone hops like an excited dog
  	def doggyHop(self):
  		ospeed = self.__speed
  		self.__speed = 1
  		for i in range (0,4,1):
  			self.moveUp()
  			time.sleep(0.20)
  			self.moveDown()
  			time.sleep(0.20)
  		self.hover()
  		self.__speed = ospeed
  
  	# Drone wags like a happy dog
  	def doggyWag(self):
  		ospeed = self.__speed
  		self.__speed = 1
  		for i in range (0,4,1):
  			self.moveLeft()
  			time.sleep(0.25)
  			self.moveRight()
  			time.sleep(0.25)
  		self.hover()
  		self.__speed = ospeed
  
  	# Drone nods
  	def doggyNod(self):
  		ospeed = self.__speed
  		self.__speed = 1
  		for i in range (0,4,1):
  			self.moveForward()
  			time.sleep(0.25)
  			self.moveBackward()
  			time.sleep(0.25)
  		self.hover()
  		self.__speed = ospeed
  
  	def printDefault(self, *args):
  		if os.name == 'posix':
  			print self.__pDefaultStr,
  			try:
  				if len(*args) > 0:
  					for i in args:	print i,
  					print self.__pDefaultStr
  			except: pass
  
  	def printRed(self, *args):
  		if os.name == 'posix':
  			print self.__pRedStr,
  			try:
  				if len(*args) > 0:
  					for i in args:	print i,
  					print self.__pDefaultStr
  			except: pass
  
  	def printGreen(self, *args):
  		if os.name == 'posix':
  			print self.__pGreenStr,
  			try:
  				if len(*args) > 0:
  					for i in args:	print i,
  					print self.__pDefaultStr
  			except: pass
  
  	def printYellow(self, *args):
  		if os.name == 'posix':
  			print self.__pYellowStr,
  			try:
  				if len(*args) > 0:
  					for i in args:	print i,
  					print self.__pDefaultStr
  			except: pass
  
  	def printBlue(self, *args):
  		if os.name == 'posix':
  			print self.__pBlueStr,
  			try:
  				if len(*args) > 0:
  					for i in args:	print i,
  					print self.__pDefaultStr
  			except: pass
  
  	def printPurple(self, *args):
  		if os.name == 'posix':
  			print self.__pPurpleStr,
  			try:
  				if len(*args) > 0:
  					for i in args:	print i,
  					print self.__pDefaultStr
  			except: pass
  
  	def printLineUp(self):
  		if os.name == 'posix':	print self.__pLineUpStr,
  
  
  ##################################=-
  ### Threads & Thread-Sidekicks ###=-
  ##################################=-
  # Idea: the network thread listens to the given network-stream and communication-pipes of other processes, such as for video or navdata-decoding.
  # 		In case the connection to the drone is cut off for more than 2 seconds (so no keep-alive-command has been sent) the network
  #		needs to reconnect. In order to do so the (private) function "__netrecon" starts after 0.1 seconds of no incoming navdata-datapacket to
  #		reconnect all given network-sockets.
  	def __heartbeat(self):
  	# If the drone does not get a command, it will mutter after 50ms (CTRL watchdog / state[28] will set to 1)
  	# and panic after 2 seconds and abort data-communication on port 5554 (then you have to initialize the network again).
  	# Heartbeat will reset the watchdog and, by the way, the ACK_BIT (state[6], to accept any other AT*CONFIG command)
  	# If mainthread isn't alive anymore (because program crashed or whatever), heartbeat will initiate the shutdown.
  		if str(threading.enumerate()).count("MainThread, stopped") or str(threading.enumerate()).count("MainThread")==0:	self.shutdown()
  		else:	self.at("COMWDG",[])
  
  	# CheckAndReact is periodically called by the receiveData-Thread to check for mainly for critical status-error(s) and
  	# changed debug-modes.
  	def __checkAndReact(self, debug, showCommands):
  		# Automatic process-commands, used for syncing debugging-bits to child-processes
  		if debug != self.debug:
  			debug = self.debug
  			if debug:
  				self.__NavData_pipe.send("debug")
  				self.__Video_pipe.send("debug")
  			else:
  				self.__NavData_pipe.send("undebug")
  				self.__Video_pipe.send("undebug")
  		if showCommands != self.showCommands:
  			showCommands = self.showCommands
  			if showCommands:
  				self.__NavData_pipe.send("showCommands")
  				self.__Video_pipe.send("showCommands")
  			else:
  				self.__NavData_pipe.send("hideCommands")
  				self.__Video_pipe.send("hideCommands")
  		# Communication problem, shutting down
  		if self.stopOnComLoss and self.__State[30]:
  			self.shutdown()
  			sys.exit()
  		return (debug,showCommands)
  			
  	# Thread for sending the configuration. It is asynchronous but save.
  	# The configuration-requests are in a queue, the first entry is sent. NavData will contain a "Control command ACK" status-bit,...
  	# ...that configuration is ready to be set. This will be confirmed and the procedure waits until this bit is 0 again; then the next entry will be processed.
  	# In savemode, there is a check whether the configuration has been changed correctly by requesting the current/latest configuration and double-checking this value. 
  	def __sendConfig(self):
  		sleeptime, getconfigtag, self.__sendConfigRunning = 0.001, False, True
  		while not self.__networksuicide:
  			if len(self.__ConfigQueue):										# If there is something in the queue...
  				if self.__ConfigQueue[0][-1]:	self.sendConfigIDs()		# ...check for multiuserconfig-request (and send it)					
  				self.__ConfigSending = True									# Set tag, to show sending is in process
  				qlen = len(self.__ConfigQueue)
  				if qlen > 1:												# Testing for double entries, preventing a ping-pong in save-mode
  					i = 1
  					while True:									   			
  						if i >= qlen:	break
  						if self.__ConfigQueue[0][0].lower() == self.__ConfigQueue[i][0].lower():
  							self.__ConfigQueue.remove(self.__ConfigQueue[0])# Delete double entries
  							qlen = len(self.__ConfigQueue)
  						else: 			i+=1
  				self.at("CONFIG",self.__ConfigQueue[0][:-1])				# Send the first entry in queue
  				getconfigtag, configconfirmed, configreconfirmed = False, False, False
  				while not configconfirmed and not self.__networksuicide:	# Wait for confirmation-bit from drone...
  					if self.__State[6] and not configreconfirmed and not self.__networksuicide:
  						self.at("CTRL",[5,0])								# ...and send reset the confirmation-bit
  						configreconfirmed = True
  					if not self.__State[6] and configreconfirmed and not self.__networksuicide:
  						configconfirmed = True								# Wait for the reset of the confirmation-bit
  					time.sleep(sleeptime)
  				# It seems that the drone stores configurations not always correctly; therfore, here is a save-mode:
  				if self.sendConfigSaveMode and not self.__networksuicide:
  					lastConfigDataCount = self.__ConfigDataCount			# Wait for the next configuration-list
  					self.getConfig()
  					while lastConfigDataCount == self.__ConfigDataCount and not self.__networksuicide:	time.sleep(sleeptime)
  				# New & Optimized
  					for i in range (0,len(self.__ConfigData),1):
  						if self.__ConfigData[i][0].find(self.__ConfigQueue[0][0]) > -1:
  							if self.__ConfigData[i][1] != self.__ConfigQueue[0][1]:
  								if self.debug:
  									print "   Configuration missmatched, resending !"
  									print "   "+self.__ConfigData[i][0]+" should be \""+self.__ConfigQueue[0][1]+"\" is \""+self.__ConfigData[i][1]+"\""
  									self.__ConfigQueue.append(self.__ConfigQueue[0])		# If value is not correctly set, requeue !
  				self.__ConfigQueue.remove(self.__ConfigQueue[0])							# Configuration has been (correctly) set, delete request from queue and go on
  				if self.__networksuicide:	self.__ConfigQueue=[]
  			if not len(self.__ConfigQueue):
  				if not getconfigtag:
  					self.getConfig()
  					getconfigtag = True
  					self.__ConfigSending = False
  				else:	time.sleep(sleeptime)
  		if self.debug:	print "sendConfig-Tread :   committed suicide"
  
  	def __receiveData(self):
  		self.__net_pipes=[]
  		self.__net_pipes.append(self.__NavData_pipe)	
  		self.__net_pipes.append(self.__Video_pipe)
  		self.__Config_pipe = socket.socket(socket.AF_INET, socket.SOCK_STREAM)	#TCP
  		self.__Config_pipe.setblocking(0)
  		self.__Config_pipe.connect_ex((self.DroneIP, self.CTLPort))
  		self.__net_pipes.append(self.__Config_pipe)
  		VideoIsDead, configdata, cfgdata, cmd = False, [], "", ""
  		self.__vDecodeRunning, debug, showCommands, self.__receiveDataRunning = False, False, False, True
  
  		while not self.__networksuicide:
  			in_pipe, dummy1, dummy2 = select.select(self.__net_pipes, [], [], 0.1)	# When something is in a pipe...
  			for ip in in_pipe:														# ...go and get it
  				if ip == self.__NavData_pipe:		### Receiving sensor-values from NavData-process
  					self.__NavData, self.__State, self.__NavDataCount, self.__NavDataTimeStamp, self.__NavDataDecodingTime, self.__NoNavData = self.__NavData_pipe.recv()
  				if ip == self.__vdecode_pipe:		### Receiving imagedata and feedback from videodecode-process
  					cmd, VideoImageCount, VideoImage, VideoDecodeTime = self.__vdecode_pipe.recv()	# Imagedata
  					if self.showCommands and cmd!="Image" :	print "** vDec -> Com :",cmd	
  					if cmd == "suicided":				self.__Video_pipe.send("vd died")		# videodecode-process died
  					if cmd == "foundCodec":				self.__Video_pipe.send("foundCodec")	# the codec of the videostream has been found, do not flood anymore
  					if cmd == "VideoUp":				self.__VideoReady = True				# Imagedata is available
  					if cmd == "keypressed":				self.__vKey = VideoImage				# Pressed key on window
  					if cmd == "reset":					self.__Video_pipe.send(cmd)				# proxy to videodecode-process
  					if cmd == "Image":															# Imagedata !
  						self.__VideoImageCount =		VideoImageCount
  						self.__VideoImage =				VideoImage
  						self.__VideoDecodeTime =		VideoDecodeTime
  						self.__VideoDecodeTimeStamp =	time.time()-self.__startTime
  				if ip == self.__Video_pipe:		### Receiving feedback from videostream-process
  					cmd = self.__Video_pipe.recv()
  					if self.showCommands and cmd != "":	print "** Vid -> Com : ",cmd
  					if cmd == "vDecProc":											# videodecode-process should start
  						if not self.__vDecodeRunning:
  							self.__vDecodeProcess = multiprocessing.Process( target=vDecode, args=(self.__VidPipePath,self.__vdecodeChild_pipe,os.getpid()))
  							self.__vDecodeProcess.start()
  							self.__net_pipes.append(self.__vdecode_pipe)
  							self.__vDecodeRunning = True
  
  						self.__Video_pipe.send("vDecProcON")
  #					else:						self.__vdecode_pipe.send(cmd)		# If / elif / else is somehow not working here...whyever
  					if cmd == "VideoDown":		self.__VideoReady=False				# videodecode-process stopped
  					if cmd == "saveVideo":		self.__SaveVideo=True				# no preprocessing of the video
  					if cmd == "unsaveVideo":	self.__SaveVideo=False				# preprocessing activated again
  					if cmd == "debug":			self.__vdecode_pipe.send(cmd)		# proxy to videodecode-process
  					if cmd == "showCommands":	self.__vdecode_pipe.send(cmd)		# proxy to videodecode-process
  					if cmd == "hideCommands":	self.__vdecode_pipe.send(cmd)		# proxy to videodecode-process
  					if cmd == "show":			self.__vdecode_pipe.send(cmd)		# proxy to videodecode-process
  					if cmd == "hide":			self.__vdecode_pipe.send(cmd)		# proxy to videodecode-process
  					if cmd == "vDecProcKill":
  						self.__vdecode_pipe.send("die!")	# videodecode-process should switch off
  						vDecodeRunning = False
  				if ip==self.__Config_pipe and not self.__networksuicide:	### Receiving drone-configuration
  					try:
  						if self.__networksuicide:	  break								# Does not stop sometimes, so the loop will be forced to stop
  						cfgdata = cfgdata+self.__Config_pipe.recv(65535)				# Data comes in two or three packages
  						if cfgdata.count("\x00"):										# Last byte of sent config-file, everything was received
  							if self.__networksuicide:	break
  							configdata = (cfgdata.split("\n"))							# Split the huge package into a configuration-list
  							for i in range(0, len(configdata), 1):
  								configdata[i] = configdata[i].split(" = ")				# Split the single configuration-lines into configuration and value
  							self.__ConfigData = configdata[:-1]   						# Last value is "\x00"
  							self.__ConfigDataTimeStamp = time.time()-self.__startTime	# Set a timestamp for a better coordination
  							self.__ConfigDataCount+=1									# Alters the count of received Configdata for a better coordination
  							configdata, cfgdata = [], ""
  							if self.showCommands:	print "Got "+str(len(self.__ConfigData))+" Configdata "+str(time.time()-self.__calltime)
  							self.__calltime=0
  					except IOError:	pass
  				debug, showCommands = self.__checkAndReact(debug, showCommands)		# Check for errors and things to change
  		if self.debug:	print "receiveData-Thread : committed suicide"
  		# TestMe
  		try:	self.__Config_pipe.close()
  		except:	pass
  
  	def __stopnetwork(self):
  		self.__networksuicide = True
  
  #############################=-
  ### Compatibility Commands ###=-
  #############################=-
  # While programming this API I changed some command-names
  # This section converts the old commands into the new ones
  	def pwm(self, fl, fr, rl, rr):		# Controls engines directly, overriding control loops.
  		if fl > 64000:	fl = 64000
  		if fr > 64000:	fr = 64000
  		if rl > 64000:	rl = 64000
  		if rr > 64000:	rr = 64000
  		self.at("PWM", [int(fl), int(fr), int(rr), int(rl)])
  
  	def groundVideo(self, *args):	self.groundCam(*args)
  	def frontVideo(self, *args):	self.frontCam(*args)
  
  ###############################################################################
  ### Internal Subfunctions
  ###############################################################################
  	def	__checkSpeedValue(self,value):
  		try:
  			speed = float(value)
  			if self.valueCorrection:
  				speed = max(-1.0,speed)
  				speed = min( 1.0,speed)
  		except:	speed = self.__speed
  		return speed
  
  # Checks the inputs for the right length
  def normalLen8(value):
  	value, zero =	str(value), "00000000"
  	vlen =			min(len(value),8)
  	normal = 		zero[0:8-vlen] + value[0:8]
  	return normal[0:8].lower()
  	
  ##################################################################################################
  ###### Receive and Decode Video																######
  ##################################################################################################
  # If the ps_drone-process has crashed, recognize it and kill yourself
  def watchdogV(parentPID, ownPID):
  	global commitsuicideV
  	while not commitsuicideV:
  		time.sleep(1)
  		try : 		os.getpgid(parentPID)
  		except:
  			try:	subprocess.Popen(["kill",str(os.getpid())],stdin=subprocess.PIPE,stdout=subprocess.PIPE,stderr=subprocess.PIPE)
  			except:	pass
  
  # Thread to capture, decode and display the video-stream			
  def vCapture(VidPipePath, parent_pipe):
  	import cv2
  	global vCruns, commitsuicideV, showVid, lockV, debugV
  
  	show = 		False
  	hide =		True
  	vCruns =	True
  	t = 		time.time()
  	parent_pipe.send(("VideoUp",0,0,0))
  	capture = 	cv2.VideoCapture(VidPipePath)
  	ImgCount =	0
  	if debugV:	print "CAPTURE: "+str(time.time()-t)
  	time.sleep(0.2)
  	parent_pipe.send(("foundCodec",0,0,0))
  	time.sleep(0.2)
  	declag =	time.time()
  	count =		-3
  	imageXsize = 	0
  	imageYsize = 	0
  	windowName = 	"PS-Drone"
  	codecOK = 		False
  	lastKey =		""
  	cc=0
  
  	while not commitsuicideV:
  		decTimeRev = 		time.time()
  		receiveWatchdog = threading.Timer(2.0, VideoReceiveWatchdog, [parent_pipe,"vCapture", debugV])	# Resets video if something hangs
  		receiveWatchdog.start()
  		success, image = 	capture.read()
  		cc	+=1
  		receiveWatchdog.cancel()
  		decTime =			decTimeRev-time.time()
  		tlag =				time.time()-declag
  
  		if not codecOK and success:
  			try:
  				if image.shape[:2]==(360,640) or image.shape[:2]==(368,640) or image.shape[:2]==(720,1280) or image.shape[:2]==(1080,1920):
  					codecOK = True
  					if debugV:	print "Codec seems OK"
  				else:
  					if debugV:	print "Codec failure"
  					parent_pipe.send(("reset",0,0,0))
  					commitsuicideV = True
  			except:
  					if debugV:	print "Codec failure"
  					parent_pipe.send(("reset",0,0,0))
  					commitsuicideV = True
  		if codecOK:
  			if not (imageXsize == image.shape[1]) or not (imageYsize == image.shape[0]):
  				cv2.destroyAllWindows()
  				imageYsize, imageXsize = image.shape[:2]
  				windowName = "PS-Drone - "+str(imageXsize)+"x"+str(imageYsize)
  			if success:
  				if tlag > 0.02:	count+=1
  				if count > 0:
  					ImgCount+=1
  					if not show and not hide:
  						cv2.destroyAllWindows()
  						hide = True
  					if show:
  						cv2.imshow(windowName, image)
  						key=cv2.waitKey(1)
  						if key>-1:	parent_pipe.send(("keypressed",0,chr(key%256),0))
  					parent_pipe.send(("Image",ImgCount,image,decTime))
  			else:	time.sleep(0.01)
  			declag = time.time()
  
  			if showVid:
  				if not show:
  					show=True
  					cv2.destroyAllWindows()
  			else:
  				if show:
  					show=False
  					cv2.destroyAllWindows()
  	vCruns = False
  	cv2.destroyAllWindows()
  	capture.release()
  	if debugV:	print "vCapture-Thread :    committed suicide"
  
  ### Process to decode the videostream in the FIFO-Pipe, stored there from main-loop.
  # Storing and decoding must not be processed in the same process, thats why decoding is external.
  # vDecode controls the vCapture-thread which captures and decodes finally the videostream.
  def vDecode(VidPipePath, parent_pipe, parentPID):
  	global vCruns, commitsuicideV, showVid, lockV, debugV
  	showCommands = 		False
  	Thread_vCapture =	threading.Thread(target=vCapture, args=(VidPipePath,parent_pipe))
  	Thread_vCapture.start()
  	Thread_watchdogV =	threading.Thread(target=watchdogV, args=[parentPID,os.getpid()])
  	Thread_watchdogV.start()
  
  	while not commitsuicideV:
  		in_pipe, out_pipe, dummy2 = select.select([parent_pipe], [], [], 0.1)		# When something is in a pipe...
  		cmd = parent_pipe.recv()
  		if showCommands:	print "** Com -> vDec : ",cmd
  		if cmd == "die!":			commitsuicideV = True
  		elif cmd == "reset":		commitsuicideV = True
  		elif cmd == "show":			showVid = 		True
  		elif cmd == "hide":			showVid =		False
  		elif cmd == "debug":
  			debugV = True
  			print "vDecode-Process :    running"
  			if vCruns:	print "vCapture-Thread :    running"
  		elif cmd == "undebug":		debugV =		False
  		elif cmd == "showCommands":	showCommands =	True
  		elif cmd == "hideCommands":	showCommands =	False
  	Thread_vCapture.join()
  	parent_pipe.send(("suicided",0,0,0))
  	time.sleep(0.1)
  	if debugV:	print "vDecode-Process :    committed suicide"
  
  #####################################################
  def VideoReceiveWatchdog(parent_pipe,name, debugV):
  	if debugV:	print "WHATCHDOG reset von",name
  	parent_pipe.send(("reset",0,0,0))
  
  def mainloopV(DroneIP, VideoPort, VidPipePath, parent_pipe, parentPID):
  	inited, preinited, suicide, debugV, showCommands, slowVideo = False, False, 0, False, False, False
  	rawVideoFrame, VidStreamSnippet, VidStreamSnippetAvalible, iFrame, lastIFrame = "", "", False, False, False
  	saveVideo, unsureMode, searchCodecTime, frameRepeat, burstFrameCount = False, True, 0, 1, 0
  	FrameCount, reset, resetCount, commitsuicideV, foundCodec = 0,False, 0, False, False
  
  	vstream_pipe, pipes = None, [parent_pipe]
  	vdecode_pipe, vdecode_childpipe = multiprocessing.Pipe()
  	pipes.append(vdecode_pipe)
  	Thread_watchdogV = threading.Thread(target=watchdogV, args=[parentPID,os.getpid()])
  	Thread_watchdogV.start()
  
  	while not commitsuicideV:
  		in_pipe, out_pipe, dummy2 = select.select(pipes, [], [], 0.1)		# When something is in a pipe...
  		for ip in in_pipe:	
  			if ip == parent_pipe:
  				cmd = parent_pipe.recv()
  				if showCommands:			print "** Com -> Vid : ",cmd
  				if cmd == "die!":
  					if inited:
  						suicide = True
  						parent_pipe.send("vDecProcKill")
  						dummy = 0
  					else:	commitsuicideV = True
  				elif cmd == "foundCodec":
  					foundCodec		= True
  					burstFrameCount = 0
  				elif cmd == "reset" and not reset:# and resetCount<3:
  					inited, preinited, foundCodec	= False, False, False
  					rawVideoFrame, VidStreamSnippet	= "", ""
  					VidStreamSnippetAvalible		= False
  					iFrame, lastIFrame				= False, False
  					FrameCount, reset				= False, 0, True
  					unsureMode, searchCodecTime		= True, 0
  					burstFrameCount					= 0
  					resetCount						+= 1
  					parent_pipe.send("vDecProcKill")
  				elif cmd == "slowVideo":
  					slowVideo = True
  					frameRepeat = 1
  				elif cmd == "midVideo":
  					slowVideo = True
  					frameRepeat = 5
  				elif cmd == "fastVideo":
  					slowVideo = False
  					frameRepeat = 1
  				elif cmd == "saveVideo":
  					saveVideo = True
  					parent_pipe.send("saveVideo")
  				elif cmd == "unsaveVideo":
  					saveVideo = False
  					parent_pipe.send("unsaveVideo")
  				elif cmd == "showCommands":
  					showCommands = True
  					parent_pipe.send("showCommands")
  				elif cmd == "hideCommands":
  					showCommands = False
  					parent_pipe.send("hideCommands")
  				elif cmd == "debug":
  					debugV = True
  					print "Video-Process :      running"
  					parent_pipe.send("debug")
  				elif cmd == "undebug":
  					debugV = False
  					parent_pipe.send("undebug")
  				elif cmd == "init" and not inited and not preinited:
  					preinited = True
  					try:
  						os.mkfifo(VidPipePath)
  						os.chmod(VidPipePath, 1411)		# dec => oct : 777 => 1411 | 666 => 1232
  					except:
  						saveVideo = True						# ... fall back to savemode
  						parent_pipe.send("saveVideo")			# Inform the main process
  						unsureMode = False
  						foundCodec = True	
  					parent_pipe.send("vDecProc")
  				elif cmd == "vDecProcON":
  					rawVideoFrame		= ""
  					VidStreamSnippet	= ""
  					iFrame, lastIFrame	= False, False
  					FrameCount			= 0
  					foundCodec			= False
  					searchCodecTime		= 0
  					if not vstream_pipe:
  						vstream_pipe = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
  						vstream_pipe.setblocking(0)
  						vstream_pipe.connect_ex((DroneIP,VideoPort))
  						pipes.append(vstream_pipe)
  					write2pipe = open(VidPipePath,"w+")
  					suicide = False
  					inited = True
  					preinited = False
  					unsureMode = True
  				elif cmd == "uninit" and inited:
  					parent_pipe.send("vDecProcKill")
  				elif cmd == "vd died":
  					if inited and not reset:
  						pipes.remove(vstream_pipe)
  						vstream_pipe.shutdown(socket.SHUT_RDWR)
  						vstream_pipe.close()
  						write2pipe.close()
  						inited = False
  						if suicide:		commitsuicideV = True
  						parent_pipe.send("VideoDown")
  					try:			os.remove(VidPipePath)
  					except:			pass
  					if not inited and reset:
  						try:
  							os.mkfifo(VidPipePath)
  							os.chmod(VidPipePath, 1411)		# dec => oct : 777 => 1411 | 666 => 1232
  						except:
  							saveVideo = True						# ... fall back to savemode
  							parent_pipe.send("saveVideo")			# Inform the main process
  							unsureMode = False
  							foundCodec = True	
  						parent_pipe.send("VideoDown")
  						parent_pipe.send("vDecProc")
  						parent_pipe.send("debug")
  						reset = 		  False
  						burstFrameCount	= 0
  				else:
  					parent_pipe.send(cmd)
  
  			### Grabs the Videostream and store it in a fifo-pipe for decoding.
  			# The decoder has to guess the videostream-format which takes around 266 video-frames.
  			#    So the stream is preprocessed, I-Frames will cut out while initiation and a flood of copies 
  			#	 will be send to the decoder, till the proper decoder for the videostream is found.
  			# In case of a slow or midspeed-video, only a single or a few copied I-frames are sent to the decoder.
  			if ip == vstream_pipe:
  				receiveWatchdog = threading.Timer(2.0, VideoReceiveWatchdog, [parent_pipe,"Video Mainloop", debugV,])	# Resets video if something hangs
  				receiveWatchdog.start()
  				videoPackage	= vstream_pipe.recv(65535)
  				receiveWatchdog.cancel()
  				lenVideoPackage	=len(videoPackage)
  				if lenVideoPackage == 0:		commitsuicideV = True
  				else:
  					if inited and not reset:
  						### Analyze raw datastream
  						frameStart = False
  						if not saveVideo and lenVideoPackage>45 and videoPackage[36:40]=="\x00\x00\x00\x00" and videoPackage[41:44]=="\x00\x01\x00":
  							frameStart		= True
  							rawVideoFrame	= VidStreamSnippet
  							lastIFrame		= iFrame
  							if videoPackage[30] == "\x01":			# I-Frame
  								iFrame, unsureMode	= True, False
  								VidStreamSnippet	= videoPackage
  								FrameCount += 1
  							elif videoPackage[30] == "\x03":		# P-Frame
  								iFrame = False
  								if not unsureMode:
  									if foundCodec:		VidStreamSnippet	= videoPackage
  #									else:				VidStreamSnippet   += videoPackage
  								FrameCount += 1
  							else:	
  								iFrame				= False
  								VidStreamSnippet	= videoPackage
  								if debugV:
  									print "*** Odd h264 Frametype: ",FrameCount,
  									print videoPackage[30:50].encode("hex")
  									print " - ",videoPackage[31:40].find("\x00\x00\x00"),ord(videoPackage[30])
  						elif not unsureMode:
  							VidStreamSnippet+=videoPackage		# Merging video-snippets
  
  
  						### Procress last frame
  						# An MPEG4-Stream is not confirmed. Boost or fallback to savemode.
  						lenRawVideoFrame	= len(rawVideoFrame)
  
  #						print slowVideo, frameStart, saveVideo, foundCodec, lenRawVideoFrame
  
  						if not saveVideo and unsureMode and frameStart and lastIFrame and lenRawVideoFrame>0:
  							if not searchCodecTime:	
  								searchCodecTime = time.time()			# Video is freshly initiated
  							elif (time.time()-searchCodecTime) > 2.0:	# Waited too long for an MPEG4 stream confirmation...
  								saveVideo = True						# ... fall back to savemode
  								parent_pipe.send("saveVideo")			# Inform the main process
  								unsureMode = False
  								foundCodec = True						# switch off codec guess speed-up
  						elif not saveVideo and frameStart and not unsureMode and not foundCodec and lenRawVideoFrame>0:
  							# Boost Frames
  							boost=((1024*512)/len(VidStreamSnippet))+1
  #							print "Boost IN", len(VidStreamSnippet)
  							for i in range(0,boost):
  								try: write2pipe.write(rawVideoFrame)
  								except: print "Boost ERROR"
  							burstFrameCount+=1
  							# Give up after to much tries
  							if burstFrameCount>10:		
  								parent_pipe.send(("reset",0,0,0))
  								time.sleep(0.2)
  								burstFrameCount=0
  								if debugV: print "To many pictures send while guessing the codec. Resetting."
  
  						# Normal Pipeing
  						elif not slowVideo and frameStart and (saveVideo or foundCodec) and lenRawVideoFrame>0:
  							if burstFrameCount==0 and lastIFrame:		burstFrameCount = 1
  							if burstFrameCount==1:
  								try:	write2pipe.write(rawVideoFrame)
  								except: print "Pipe Error"
  
  						# Just show the I-Frame for slow-video-mode (and repeat for less delay in midVideo()-mode)
  						elif not saveVideo and foundCodec and slowVideo and lastIFrame and lenRawVideoFrame>0:
  							for i in range(0,frameRepeat):			write2pipe.write(rawVideoFrame)	
  						# Save-Mode
  						elif saveVideo:								write2pipe.write(videoPackage)
  
  
  	try:
  		vstream_pipe.shutdown(socket.SHUT_RDWR)
  		vstream_pipe.close()
  	except:	pass
  	try:	write2pipe.close()
  	except:	pass
  	try:	vstream_pipe.close()
  	except:	pass
  	try:
  		VidPipe=open(VidPipePath,"r")
  		r = "1"
  		while len(r):	r=VidPipe.read()
  		FIFO.close()
  	except:	pass
  	try:	os.remove(VidPipePath)
  	except:	pass
  	if debugV:	print "Video-Process :      committed suicide"
  	try:	vstream_pipe.close()
  	except:	pass
  	
  
  	
  ##################################################################################################
  ###### Receive and Decode NavData															######
  ##################################################################################################
  ### Description:
  ### It follows lousy code for abetter documentation! Later there will be lousy code because of laziness; I will correct it later....maybe.
  ### You will (normally) find the names of the official AR.drone SDK 2.0, some comments and the official data type of that value.
  ### A lot of entries are reversed engineered; for some, I have no idea what they are doing or what their meaning is.
  ### It would be nice if you could give me a hint if you have some further information.
  
  ##### Header ##################################################################
  def decode_Header(data):
  #Bit 00-07: FLY_MASK, VIDEO_MASK, VISION_MASK, CONTROL_MASK, ALTITUDE_MASK, USER_FEEDBACK_START, COMMAND_MASK, CAMERA_MASK
  #Bit 08-15: TRAVELLING_MASK, USB_MASK, NAVDATA_DEMO_MASK, NAVDATA_BOOTSTRAP, MOTORS_MASK, COM_LOST_MASK, SOFTWARE_FAULT, VBAT_LOW
  #Bit 16-23: USER_EL, TIMER_ELAPSED, MAGNETO_NEEDS_CALIB, ANGLES_OUT_OF_RANGE, WIND_MASK, ULTRASOUND_MASK, CUTOUT_MASK, PIC_VERSION_MASK
  #Bit 24-31: ATCODEC_THREAD_ON, NAVDATA_THREAD_ON, VIDEO_THREAD_ON, ACQ_THREAD_ON, CTRL_WATCHDOG_MASK, ADC_WATCHDOG_MASK, COM_WATCHDOG_MASK, EMERGENCY_MASK
  	stateBit = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
  	stateBit[ 0] = data[1]    &1	#  0: FLY MASK :					(0) ardrone is landed, (1) ardrone is flying
  	stateBit[ 1] = data[1]>> 1&1	#  1: VIDEO MASK :					(0) video disable, (1) video enable
  	stateBit[ 2] = data[1]>> 2&1	#  2: VISION MASK :					(0) vision disable, (1) vision enable
  	stateBit[ 3] = data[1]>> 3&1	#  3: CONTROL ALGO :				(0) euler angles control, (1) angular speed control
  	stateBit[ 4] = data[1]>> 4&1	#  4: ALTITUDE CONTROL ALGO :	 	(0) altitude control inactive (1) altitude control active
  	stateBit[ 5] = data[1]>> 5&1	#  5: USER feedback : 				Start button state
  	stateBit[ 6] = data[1]>> 6&1	#  6: Control command ACK : 		(0) None, (1) one received
  	stateBit[ 7] = data[1]>> 7&1	#  7: CAMERA MASK : 				(0) camera not ready, (1) Camera ready
  	stateBit[ 8] = data[1]>> 8&1	#  8: Travelling mask : 			(0) disable, (1) enable
  	stateBit[ 9] = data[1]>> 9&1	#  9: USB key : 					(0) usb key not ready, (1) usb key ready
  	stateBit[10] = data[1]>>10&1	# 10: Navdata demo : 				(0) All navdata, (1) only navdata demo
  	stateBit[11] = data[1]>>11&1	# 11: Navdata bootstrap : 			(0) options sent in all or demo mode, (1) no navdata options sent
  	stateBit[12] = data[1]>>12&1	# 12: Motors status : 				(0) Ok, (1) Motors problem
  	stateBit[13] = data[1]>>13&1	# 13: Communication Lost : 			(0) Com is ok, (1) com problem
  	stateBit[14] = data[1]>>14&1	# 14: Software fault detected - user should land as quick as possible (1)
  	stateBit[15] = data[1]>>15&1	# 15: VBat low : 					(0) Ok, (1) too low
  	stateBit[16] = data[1]>>16&1	# 16: User Emergency Landing :		(0) User EL is OFF, (1) User EL is ON
  	stateBit[17] = data[1]>>17&1	# 17: Timer elapsed : 				(0) not elapsed, (1) elapsed
  	stateBit[18] = data[1]>>18&1	# 18: Magnetometer calib state :	(0) Ok, no calibration needed, (1) not ok, calibration needed
  	stateBit[19] = data[1]>>19&1	# 19: Angles :						(0) Ok,	(1) out of range
  	stateBit[20] = data[1]>>20&1	# 20: WIND MASK:					(0) Ok, (1) Too much wind
  	stateBit[21] = data[1]>>21&1	# 21: Ultrasonic sensor :			(0) Ok, (1) deaf
  	stateBit[22] = data[1]>>22&1	# 22: Cutout system detection :		(0) Not detected, (1) detected
  	stateBit[23] = data[1]>>23&1	# 23: PIC Version number OK :		(0) a bad version number, (1) version number is OK
  	stateBit[24] = data[1]>>24&1	# 24: ATCodec thread ON : 			(0) thread OFF, (1) thread ON
  	stateBit[25] = data[1]>>25&1	# 25: Navdata thread ON : 			(0) thread OFF, (1) thread ON
  	stateBit[26] = data[1]>>26&1	# 26: Video thread ON : 			(0) thread OFF, (1) thread ON
  	stateBit[27] = data[1]>>27&1	# 27: Acquisition thread ON : 		(0) thread OFF, (1) thread ON
  	stateBit[28] = data[1]>>28&1	# 28: CTRL watchdog : 				(0) control is well scheduled, (1) delay in control execution (> 5ms)
  	stateBit[29] = data[1]>>29&1	# 29: ADC Watchdog :				(0) uart2 is good, (1) delay in uart2 dsr (> 5ms)
  	stateBit[30] = data[1]>>30&1	# 30: Communication Watchdog :		(0) Com is ok, (1) com problem
  	stateBit[31] = data[1]>>31&1	# 31: Emergency landing : 			(0) no emergency, (1) emergency
  	stateBit[32] = data[2]
  	stateBit[33] = data[3]
  	# Alternative code:
  	#	for i in range (0,32,1):	arState[i]=data>>i&1
  	return (stateBit)
  
  ##### ID = 0 ### "demo" #######################################################
  def decode_ID0(packet):		# NAVDATA_DEMO_TAG
  	dataset = struct.unpack_from("HHIIfffifffIffffffffffffIIffffffffffff", packet, 0)
  	if dataset[1] != 148:		print "*** ERROR : Navdata-Demo-Options-Package (ID=0) has the wrong size !!!"
  	demo=[[0,0,0,0,0,0,0,0,0,0,0,0],0,[0,0,0],0,[0,0,0],0,[0,0,0,0,0,0,0,0,0],[0,0,0],0,0,[0,0,0,0,0,0,0,0,0],[0,0,0]]
  	demo[0][ 0] = dataset[2]>>15&1	# DEFAULT			(bool)
  	demo[0][ 1] = dataset[2]>>16&1	# INIT				(bool)
  	demo[0][ 2] = dataset[2]>>17&1	# LANDED			(bool)
  	demo[0][ 3] = dataset[2]>>18&1	# FLYING			(bool)
  	demo[0][ 4] = dataset[2]>>19&1	# HOVERING			(bool)  (Seems like landing)
  	demo[0][ 5] = dataset[2]>>20&1	# TEST				(bool)
  	demo[0][ 6] = dataset[2]>>21&1	# TRANS_TAKEOFF		(bool)
  	demo[0][ 7] = dataset[2]>>22&1	# TRANS_GOFIX		(bool)
  	demo[0][ 8] = dataset[2]>>23&1	# TRANS_LANDING		(bool)
  	demo[0][ 9] = dataset[2]>>24&1	# TRANS_LOOPING		(bool)
  	demo[0][10] = dataset[2]>>25&1	# TRANS_NO_VISION	(bool)
  	demo[0][11] = dataset[2]>>26&1	# NUM_STATE			(bool)
  	demo[1]		=dataset[3]			# vbat_flying_percentage	battery voltage (filtered) in percent	(uint32)
  	demo[2][0]	=dataset[4]/1000.0	# theta						pitch in degrees						(float)
  	demo[2][1]	=dataset[5]/1000.0	# phi						roll  in degrees						(float)
  	demo[2][2]	=dataset[6]/1000.0	# psi						yaw   in degrees						(float)
  	demo[3]		=dataset[7]/10.0	# altitude					altitude in centimetres					(int32)
  	demo[4][0]	=dataset[8]			# vx						estimated speed in X in mm/s			(float)	
  	demo[4][1]	=dataset[9]			# vy						estimated speed in Y in mm/s			(float)
  	demo[4][2]	=dataset[10]		# vz						estimated speed in Z in mm/s			(float)
  	demo[5]		=dataset[11]		# num_frames				streamed frame index 					(uint32) (Not used to integrate in video stage)
  	for i in range (0,9,1):	demo[6][i]	= dataset[12+i]	# detection_camera_rot		Camera parameters compute by detection	(float matrix33)
  	for i in range (0,3,1):	demo[7][i]	= dataset[21+i]	# detection_camera_trans	Deprecated ! Don't use !				(float vector31)
  	demo[8]								= dataset[24]	# detection_tag_index		Deprecated ! Don't use !				(uint32)
  	demo[9]								= dataset[25]	# detection_camera_type   	Type of tag								(uint32)
  	for i in range (0,9,1):	demo[10][i]	= dataset[26+i]	# drone_camera_rot			Camera parameters computed by drone		(float matrix33)
  	for i in range (0,3,1):	demo[11][i]	= dataset[35+i]	# drone_camera_trans		Deprecated ! Don't use !				(float vector31)
  	return(demo)
  	
  ##### ID = 1 ### "time" #######################################################
  def decode_ID1(packet):			#NAVDATA_TIME_TAG
  	dataset = struct.unpack_from("HHI", packet, 0)
  	if dataset[1] != 8:		print "*** ERROR : navdata-time-Options-Package (ID=1) has the wrong size !!!"
  	time=[0.0]
  	# Value: 11 most significant bits represent the seconds, and the 21 least significant bits represent the microseconds.
  	for i in range(0,21,1):		time[0] += ((dataset[2]>>i&1)*(2**i))		# Calculating the millisecond-part
  	time[0] /= 1000000
  	for i in range(21,32,1):	time[0] += (dataset[2]>>i&1)*(2**(i-21))	# Calculating second-part
  	return(time)
  	
  ##### ID = 2 ### "raw_measures" ################################################
  def decode_ID2(packet):			#NAVDATA_RAW_MEASURES_TAG
  	dataset = struct.unpack_from("HHHHHhhhhhIHHHHHHHHHHHHhh", packet, 0)
  	if dataset[1] != 52:		print "*** ERROR : navdata-raw_measures-Options-Package (ID=2) has the wrong size !!!"
  	raw_measures = [[0,0,0],[0,0,0],[0,0],0,0,0,0,0,0,0,0,0,0,0,0,0]
  	for i in range(0,3,1):	raw_measures[0][i] = dataset[2+i]	# raw_accs[xyz]			filtered accelerometer-datas [LSB]	(uint16)
  	for i in range(0,3,1):	raw_measures[1][i] = dataset[5+i]	# raw_gyros[xyz]		filtered gyrometer-datas [LSB]		(int16)
  	for i in range(0,2,1):	raw_measures[2][i] = dataset[8+i]	# raw_gyros_110[xy]		gyrometers  x/y 110 deg/s [LSB]		(int16)
  	raw_measures[ 3] = dataset[10]		# vbat_raw				battery voltage raw (mV)			(uint)
  	raw_measures[ 4] = dataset[11]		# us_debut_echo			[LSB]								(uint16)
  	raw_measures[ 5] = dataset[12]		# us_fin_echo			[LSB]								(uint16)
  	raw_measures[ 6] = dataset[13]		# us_association_echo	[LSB]								(uint16)
  	raw_measures[ 7] = dataset[14]		# us_distance_echo		[LSB]								(uint16)
  	raw_measures[ 8] = dataset[15]		# us_courbe_temps		[LSB]								(uint16)
  	raw_measures[ 9] = dataset[16]		# us_courbe_valeur		[LSB]								(uint16)
  	raw_measures[10] = dataset[17]		# us_courbe_ref			[LSB]								(uint16)
  	raw_measures[11] = dataset[18]		# flag_echo_ini			[LSB]								(uint16)
  	raw_measures[12] = dataset[19]		# nb_echo				[LSB]								(uint16)
  	raw_measures[13] = dataset[21]		# sum_echo				juRef_st lower 16Bit, upper 16Bit=tags?	(uint32)
  	raw_measures[14] = dataset[23]		# alt_temp_raw			in Milimeter	(just lower 16Bit)	(int32)
  	raw_measures[15] = dataset[24]		# gradient				[LSB]								(int16)
  	return(raw_measures)
  
  ##### ID = 3 ### "phys_measures" ##############################################
  def decode_ID3(packet):  		#NAVDATA_PHYS_MEASURES_TAG
  	dataset = struct.unpack_from("HHfHffffffIII", packet, 0)
  	if dataset[1] != 46:		print "*** ERROR : navdata-phys_measures-Options-Package (ID=3) has the wrong size !!!"
  	phys_measures = [0,0,[0,0,0],[0,0,0],0,0,0]
  	phys_measures[0] = dataset[2]	#float32   accs_temp
  	phys_measures[1] = dataset[3]	#uint16    gyro_temp
  	phys_measures[4] = dataset[10]	#uint32    alim3V3              3.3volt alim [LSB]
  	phys_measures[5] = dataset[11]	#uint32    vrefEpson            ref volt Epson gyro [LSB]
  	phys_measures[6] = dataset[12]	#uint32    vrefIDG              ref volt IDG gyro [LSB]
  	dataset = struct.unpack_from(">HHfHffffffIII", packet, 0) 	#switch from little to big-endian
  	for i in range(0,3,1):	phys_measures[2][i] = dataset[4+i]	#float32   phys_accs[xyz] 
  	for i in range(0,3,1):	phys_measures[3][i] = dataset[7+i]	#float32   phys_gyros[xyz]
  	return(phys_measures)
  
  ##### ID = 4 ### "gyros_offsets" ##############################################
  def decode_ID4(packet):  		#NNAVDATA_GYROS_OFFSETS_TAG
  	dataset = struct.unpack_from("HHfff", packet, 0)
  	if dataset[1] != 16:		print "*** ERROR : navdata-gyros_offsets-Options-Package (ID=4) has the wrong size !!!"
  	gyros_offsets = [0,0,0]
  	for i in range (0,3,1):		gyros_offsets[i]=dataset[i+2]	# offset_g[xyz]				in deg/s					(float)
  	return(gyros_offsets)
  
  ##### ID = 5 ### "euler_angles" ###############################################
  def decode_ID5(packet):			#NAVDATA_EULER_ANGLES_TAG
  	dataset = struct.unpack_from("HHff", packet, 0)
  	if dataset[1] != 12:		print "*** ERROR : navdata-euler_angles-Options-Package (ID=5) has the wrong size !!!"
  	euler_angles = [0,0]
  	euler_angles[0] = dataset[2]	#float32   theta_a (head/back)
  	euler_angles[1] = dataset[3]	#float32   phi_a   (sides)
  	return(euler_angles)
  
  ##### ID = 6 ### "references" #################################################
  def decode_ID6(packet):			#NAVDATA_REFERENCES_TAG
  	dataset = struct.unpack_from("HHiiiiiiiiffffffIfffffI", packet, 0)
  	if dataset[1] != 88:		print "*** ERROR : navdata-references-Options-Package (ID=6) has the wrong size !!!"
  	references = [[0,0,0],[0,0],[0,0,0],[0.0,0.0],[0.0,0.0],[0.0,0.0],0,[0.0,0.0,0.0,0.0,0.0,0]]
  	references[0][0] = dataset[2]		#ref_theta  	Theta_ref_embedded [milli-deg]	(int32)
  	references[0][1] = dataset[3]		#ref_phi		Phi_ref_embedded [milli-deg]	(int32)
  	references[0][2] = dataset[9]		#ref_psi		Psi_ref_embedded [milli-deg]	(int32)
  	references[1][0] = dataset[4]		#ref_theta_I	Theta_ref_int [milli-deg]		(int32)
  	references[1][1] = dataset[5]		#ref_phi_I		Phi_ref_int [milli-deg]			(int32)
  	references[2][0] = dataset[6]		#ref_pitch		Pitch_ref_embedded [milli-deg]	(int32)
  	references[2][1] = dataset[7]		#ref_roll		Roll_ref_embedded [milli-deg]	(int32)
  	references[2][2] = dataset[8]		#ref_yaw		Yaw_ref_embedded [milli-deg/s]	(int32)
  	references[3][0] = dataset[10]		#vx_ref			Vx_Ref_[mm/s]					(float)
  	references[3][1] = dataset[11]		#vy_ref			Vy_Ref_[mm/s]					(float)
  	references[4][0] = dataset[12]		#theta_mod		Theta_modele [radian]			(float)
  	references[4][1] = dataset[13]		#phi_mod		Phi_modele [radian]				(float)
  	references[5][0] = dataset[14]		#k_v_x											(float)
  	references[5][1] = dataset[15]		#k_v_y											(float)
  	references[6]    = dataset[16]		#k_mode											(uint32)
  	references[7][0] = dataset[17]		#ui_time										(float)
  	references[7][1] = dataset[18]		#ui_theta										(float)
  	references[7][2] = dataset[19]		#ui_phi											(float)
  	references[7][3] = dataset[20]		#ui_psi											(float)
  	references[7][4] = dataset[21]		#ui_psi_accuracy								(float)
  	references[7][5] = dataset[22]		#ui_seq											(int32)
  	return(references)
  
  ##### ID = 7 ### "trims" ######################################################
  def decode_ID7(packet):			#NAVDATA_TRIMS_TAG
  	dataset = struct.unpack_from("HHfff", packet, 0)
  	if dataset[1] != 16:		print "*** ERROR : navdata-trims-Options-Package (ID=7) has the wrong size !!!"
  	trims = [0,0,0]
  	trims[0] = dataset[2]	#  angular_rates_trim									(float)
  	trims[1] = dataset[3]	#  euler_angles_trim_theta	[milli-deg]					(float)
  	trims[2] = dataset[4]	#  euler_angles_trim_phi	[milli-deg]					(float)
  	return(trims)
  
  ##### ID = 8 ### "rc_references" ##############################################
  def decode_ID8(packet):			#NAVDATA_RC_REFERENCES_TAG
  	dataset = struct.unpack_from("HHiiiii", packet, 0)
  	if dataset[1] != 24:		print "*** ERROR : navdata-rc_references-Options-Package (ID=8) has the wrong size !!!"
  	rc_references = [0,0,0,0,0]
  	rc_references[0] = dataset[2]	#  rc_ref_pitch		Pitch_rc_embedded			(int32)
  	rc_references[1] = dataset[3]	#  rc_ref_roll		Roll_rc_embedded			(int32)
  	rc_references[2] = dataset[4]	#  rc_ref_yaw		Yaw_rc_embedded				(int32)
  	rc_references[3] = dataset[5]	#  rc_ref_gaz		Gaz_rc_embedded				(int32)
  	rc_references[4] = dataset[6]	#  rc_ref_ag		Ag_rc_embedded				(int32)
  	return(rc_references)
  
  ##### ID = 9 ### "pwm" ########################################################
  def decode_ID9(packet):			#NAVDATA_PWM_TAG
  	dataset = struct.unpack_from("HHBBBBBBBBffffiiifiiifHHHHff", packet, 0)
  	if dataset[1] != 76 and dataset[1] != 92:   #92 since firmware 2.4.8 ?
  		print "*** ERROR : navdata-navdata_pwm-Options-Package (ID=9) has the wrong size !!!"
  		#print "Soll: 76     Ist:",dataset[1]
  	pwm = [[0,0,0,0],[0,0,0,0],0.0,0.0,0.0,0.0,[0,0,0],0.0,[0,0,0,0.0],[0,0,0,0],0.0,0.0]
  	for i in range(0,4,1):	pwm[0][i] = dataset[2+i]	#  motor1/2/3/4		[Pulse-width mod]	(uint8)
  	for i in range(0,4,1):	pwm[1][i] = dataset[6+i]	#  sat_motor1/2/3/4	[Pulse-width mod]	(uint8)
  	pwm[2]    = dataset[10]			# gaz_feed_forward		[Pulse-width mod]	(float)
  	pwm[3]    = dataset[11]			# gaz_altitud			[Pulse-width mod]	(float)
  	pwm[4]    = dataset[12]			# altitude_integral		[mm/s]				(float)
  	pwm[5]    = dataset[13]			# vz_ref				[mm/s]				(float)
  	pwm[6][0] = dataset[14]			# u_pitch				[Pulse-width mod]	(int32)
  	pwm[6][1] = dataset[15]			# u_roll				[Pulse-width mod]	(int32)
  	pwm[6][2] = dataset[16]			# u_yaw					[Pulse-width mod]	(int32)
  	pwm[7]    = dataset[17]			# yaw_u_I				[Pulse-width mod]	(float)
  	pwm[8][0] = dataset[18]			# u_pitch_planif		[Pulse-width mod]	(int32)
  	pwm[8][1] = dataset[19]			# u_roll_planif			[Pulse-width mod]	(int32)
  	pwm[8][2] = dataset[20]			# u_yaw_planif			[Pulse-width mod]	(int32)
  	pwm[8][3] = dataset[21]			# u_gaz_planif			[Pulse-width mod]	(float)
  	for i in range(0,4,1):
  		pwm[9][i] = dataset[22+i]	# current_motor1/2/3/4	[mA]				(uint16)
  	pwm[10]   = dataset[26]			# altitude_prop			[Pulse-width mod]	(float)
  	pwm[11]   = dataset[27]			# altitude_der			[Pulse-width mod]	(float)
  	return(pwm)
  
  ##### ID = 10 ### "altitude" ###################################################
  def decode_ID10(packet):		#NAVDATA_ALTITUDE_TAG
  	dataset = struct.unpack_from("HHifiiffiiiIffI", packet, 0)
  	if dataset[1] != 56:		print "*** ERROR : navdata-navdata_altitude-Options-Package (ID=10) has the wrong size !!!"
  	altitude = [0,0.0,0,0,0.0,0.0,[0,0,0],0,[0,0],0]
  	altitude[0] = dataset[2]			# altitude_vision	[mm]					(int32)
  	altitude[1] = dataset[3]			# altitude_vz		[mm/s]					(float)
  	altitude[2] = dataset[4]			# altitude_ref		[mm]					(int32)
  	altitude[3] = dataset[5]			# altitude_raw		[mm]					(int32)
  	altitude[4] = dataset[6]			# obs_accZ			Observer AccZ [m/s2]	(float)
  	altitude[5] = dataset[7]			# obs_alt			Observer altitude US [m](float)
  	for i in range (0,3,1):
  		altitude[6][i] = dataset[8+i]	# obs_x				3-Vector				(int32)
  	altitude[7] = dataset[11]			# obs_state			Observer state [-]		(uint32)
  	for i in range (0,2,1):
  		altitude[8][i] = dataset[12+i]	# est_vb			2-Vector				(float)
  	altitude[9] = dataset[14]			# est_state			Observer flight state 	(uint32)
  	return(altitude)
  
  ##### ID = 11 ### "vision_raw" #################################################
  def decode_ID11(packet):		#NAVDATA_VISION_RAW_TAG
  	dataset = struct.unpack_from("HHfff", packet, 0)
  	if dataset[1] != 16:	print "*** ERROR : navdata-vision_raw-Options-Package (ID=11) has the wrong size !!!"
  	vision_raw = [0,0,0]
  	for i in range (0,3,1):		vision_raw[i] = dataset[2+i]	#  vision_tx_raw (xyz)				(float)
  	return(vision_raw)
  
  ##### ID = 12 ### "vision_of" #################################################
  def decode_ID12(packet):		#NAVDATA_VISION_OF_TAG
  	dataset = struct.unpack_from("HHffffffffff", packet, 0)
  	if dataset[1] != 44:	print "*** ERROR : navdata-vision_of-Options-Package (ID=12) has the wrong size !!!"
  	vision_of = [[0.0,0.0,0.0,0.0,0.0],[0.0,0.0,0.0,0.0,0.0]]
  	for i in range (0,5,1):		vision_of[0][i] = dataset[2+i]	#  of_dx[5]							(float)
  	for i in range (0,5,1):		vision_of[1][i] = dataset[7+i]	#  of_dy[5]							(float)
  	return(vision_of)
  
  ##### ID = 13 ### "vision" #####################################################
  def decode_ID13(packet):		#NAVDATA_VISION_TAG
  	dataset = struct.unpack_from("HHIiffffifffiIffffffIIff", packet, 0)
  	if dataset[1] != 92:	print "*** ERROR : navdata-vision-Options-Package (ID=13) has the wrong size !!!"
  	vision=[0,0,0.0,0.0,0.0,0.0,0,[0.0,0.0,0.0],0,0.0,[0.0,0.0,0.0],[0.0,0.0,0.0],0,0,[0.0,0.0]]
  	vision[0] = dataset[2]				# vision_state FIXME: What are the meanings of the tags ?
  	vision[1] = dataset[3]				# vision_misc							(int32)	
  	vision[2] = dataset[4]				# vision_phi_trim						(float)
  	vision[3] = dataset[5]				# vision_phi_ref_prop					(float)
  	vision[4] = dataset[6]				# vision_theta_trim						(float)
  	vision[5] = dataset[7]				# vision_theta_ref_prop					(float)
  	vision[6] = dataset[8]				# new_raw_picture						(int32)
  	for i in range (0,3,1):
  		vision[7][i] = dataset[9+i]		# theta/phi/psi_capture					(float)
  	vision[8] = dataset[12]				# altitude_capture						(int32)
  	for i in range (0,21,1):						# Calculating milisecond-part
  		vision[9] += ((dataset[13]>>i&1)*(2**i))
  	vision[9] /= 1000000
  	for i in range (21,32,1):						# Calculating second-part
  		vision[9] += (dataset[13]>>i&1)*(2**(i-21))	# time_capture			(float)
  	for i in range (0,3,1):
  		vision[10][i] = dataset[14+i]	#  velocities[xyz]						(float)
  	for i in range (0,3,1):
  		vision[11][i] = dataset[17+i]	#  delta_phi/theta/psi					(float)
  	vision[12] =    dataset[20]			# gold_defined							(uint32)
  	vision[13] =    dataset[21]			# gold_reset							(uint32)	
  	vision[14][0] = dataset[22]			# gold_x								(float)
  	vision[14][1] = dataset[23]			# gold_y								(float)
  	return(vision)
  
  ##### ID = 14 ### "vision_perf" ###############################################
  def decode_ID14(packet):		#NAVDATA_VISION_PERF_TAG
  	dataset = struct.unpack_from("HHffffffffffffffffffffffffff", packet, 0)
  	if dataset[1] != 108:		print "*** ERROR : navdata-vision_of-Options-Package (ID=14) has the wrong size !!!"
  	vision_perf=[0.0,0.0,0.0,0.0,0.0,0.0,[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]]
  	vision_perf[0] = dataset[2]				# time_szo								(float)
  	vision_perf[1] = dataset[3]				# time_corners							(float)
  	vision_perf[2] = dataset[4]				# time_compute							(float)
  	vision_perf[3] = dataset[5]				# time_tracking							(float)
  	vision_perf[4] = dataset[6]				# time_trans							(float)
  	vision_perf[5] = dataset[7]				# time_update							(float)
  	for i in range (0,20,1):
  		vision_perf[6][i] = dataset[8+i]	# time_custom[20]						(float)
  	return(vision_perf)
  
  ##### ID = 15 ### "trackers_send" #############################################
  def decode_ID15(packet):  		#NAVDATA_TRACKERS_SEND_TAG
  	dataset = struct.unpack_from("HHiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiii", packet, 0)
  	if dataset[1] != 364:		print "*** ERROR : navdata-trackers_send-Options-Package (ID=15) has the wrong size !!!"
  	DEFAULT_NB_TRACKERS_WIDTH  = 6
  	DEFAULT_NB_TRACKERS_HEIGHT = 5
  	limit = DEFAULT_NB_TRACKERS_WIDTH*DEFAULT_NB_TRACKERS_HEIGHT
  	trackers_send = [[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0],[[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0]]]
  	for i in range (0, limit, 1):
  		trackers_send[0][i] = dataset[2+i]			#  locked[limit]				(int32)
  	for i in range (0, limit, 1):
  		trackers_send[1][i][0] = dataset[32+(i*2)]	#  point[x[limit],y[limit]]		(int32)
  		trackers_send[1][i][1] = dataset[33+(i*2)]
  	return(trackers_send)
  
  ##### ID = 16 ### "vision_detect" #############################################
  def decode_ID16(packet):  		#NAVDATA_VISION_DETECT_TAG
  	dataset = struct.unpack_from("HHIIIIIIIIIIIIIIIIIIIIIIIIIffffIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII", packet, offsetND)
  	if dataset[1] != 328:	print "*** ERROR : navdata-vision_detect-Package (ID=16) has the wrong size !!!"
  	vision_detect = [0,[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,0],[0.0,0.0,0.0,0.0],[[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]],[[0.0,0.0,0.0],[0.0,0.0,0.0],[0.0,0.0,0.0],[0.0,0.0,0.0]],[0,0,0,0]]
  	#Max marker detection in one picture: 4
  	vision_detect[0] = dataset[2]									 		# nb_detected						(uint32)
  	for i in range (0,4,1):		vision_detect[1][i] = dataset[3+i]			# type[4]							(uint32)
  	for i in range (0,4,1):		vision_detect[2][i] = dataset[7+i]			# xc[4]								(uint32)
  	for i in range (0,4,1):		vision_detect[3][i] = dataset[11+i]			# yc[4]								(uint32)
  	for i in range (0,4,1):		vision_detect[4][i] = dataset[15+i]			# width[4]							(uint32)
  	for i in range (0,4,1):		vision_detect[5][i] = dataset[19+i]			# height[4]							(uint32)
  	for i in range (0,4,1):		vision_detect[6][i] = dataset[23+i]			# dist[4]							(uint32)
  	for i in range (0,4,1):		vision_detect[7][i] = dataset[27+i]			# orientation_angle[4]				(float)
  	for i in range (0,4,1):
  		for j in range (0,9,1):	vision_detect[8][i][j] = dataset[31+i+j]	# rotation[4]						(float 3x3 matrix (11,12,13,21,...)
  	for i in range (0,4,1):
  		for j in range (0,3,1):	vision_detect[9][i][j] = dataset[67+i+j]	# rotation[4]						(float 3 vector)
  	for i in range (0,4,1):		vision_detect[10][i] = dataset[79+i]		# camera_source[4]					(uint32)
  	return(vision_detect)
  
  ##### ID = 17 ### "watchdog" ###################################################
  def decode_ID17(packet):  		#NAVDATA_WATCHDOG_TAG
  	dataset = struct.unpack_from("HHI", packet, offsetND)
  	if dataset[1] != 8:	print "*** ERROR : navdata-watchdog-Package (ID=17) has the wrong size !!!"
  	watchdog = dataset[2]		# watchdog			Watchdog controll [-]				(uint32)
  	return(watchdog)
  
  ##### ID = 18 ### "adc_data_frame" #############################################
  def decode_ID18(packet):  		#NAVDATA_ADC_DATA_FRAME_TAG
  	dataset = struct.unpack_from("HHIBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBBB", packet, offsetND)
  	if dataset[1] != 40:	print "*** ERROR : navdata-adc_data_frame-Package (ID=18) has the wrong size !!!"
  	adc_data_frame = [0,[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]]
  	adc_data_frame[0] = dataset[2]									# version								(uint32)
  	for i in range (0,32,1):	adc_data_frame[1][i] = dataset[3+i]	# data_frame[32]						(uint8)
  	return(adc_data_frame)
  
  ##### ID = 19 ### "video_stream" ###############################################
  def decode_ID19(packet):		#NAVDATA_VIDEO_STREAM_TAG
  	dataset = struct.unpack_from("HHBIIIIfIIIiiiiiII", packet, offsetND)
  	if dataset[1] != 65:	print "*** ERROR : navdata-video_stream-Package (ID=19) has the wrong size !!!"
  	video_stream = [0,0,0,0,0,0.0,0,0,0,[0,0,0,0,0],0,0]
  	video_stream[0] = dataset[2]	# quant   		quantizer reference used to encode [1:31]   				(uint8)
  	video_stream[1] = dataset[3]	# frame_size	frame size in bytes   										(uint32)
  	video_stream[2] = dataset[4]	# frame_number	frame index   												(uint32)
  	video_stream[3] = dataset[5]	# atcmd_ref_seq	atmcd ref sequence number   								(uint32)
  	video_stream[4] = dataset[6]	# atcmd_mean_ref_gap	mean time between two consecutive atcmd_ref (ms)	(uint32)
  	video_stream[5] = dataset[7]	# atcmd_var_ref_gap															(float)
  	video_stream[6] = dataset[8]	# atcmd_ref_quality		estimator of atcmd link quality   					(uint32)
  	#Drone 2.0:
  	video_stream[7] = dataset[9]	# out_bitrate			measured out throughput from the video tcp socket	(uint32)
  	video_stream[8] = dataset[10]	# desired_bitrate		last frame size generated by the video encoder		(uint32)
  	for i in range (0,5,1):		video_stream[9][i] = dataset[11+i]	# data		misc temporary data				(int32)
  	video_stream[10] = dataset[16]	# tcp_queue_level		queue usage											(uint32)
  	video_stream[11] = dataset[17]	# fifo_queue_level		queue usage											(uint32)
  	return(video_stream)
  
  ##### ID = 20 ### "games" ######################################################
  def decode_ID20(packet):  		#NAVDATA_GAMES_TAG
  	dataset = struct.unpack_from("HHII", packet, offsetND)
  	if dataset[1] != 12:	print "*** ERROR : navdata-games-Package (ID=20) has the wrong size !!!"
  	games = [0,0]
  	games[0] = dataset[2]	# double_tap_counter 			   							(uint32)
  	games[1] = dataset[3]	# finish_line_counter										(uint32)
  	return(games)
  
  ##### ID = 21 ### "pressure_raw" ###############################################
  def decode_ID21(packet):  		#NAVDATA_PRESSURE_RAW_TAG
  	dataset = struct.unpack_from("HHihii", packet, offsetND)
  	if dataset[1] != 18:	print "*** ERROR : navdata-pressure_raw-Package (ID=21) has the wrong size !!!"
  	pressure_raw = [0,0,0,0]
  	pressure_raw[0] = dataset[2]	# up 			   									(int32)
  	pressure_raw[1] = dataset[3]	# ut												(int16)
  	pressure_raw[2] = dataset[4]	# Temperature_meas 									(int32)
  	pressure_raw[3] = dataset[5]	# Pression_meas										(int32)
  	return(pressure_raw)
  
  ##### ID = 22 ### "magneto" ####################################################
  def decode_ID22(packet):		#NAVDATA_MAGNETO_TAG
  	dataset = struct.unpack_from("HHhhhffffffffffffBifff", packet, offsetND)
  	if dataset[1] != 83:	print "*** ERROR : navdata-magneto-Package (ID=22) has the wrong size !!!"
  	magneto = [[0,0,0],[0.0,0.0,0.0],[0.0,0.0,0.0],[0.0,0.0,0.0],0.0,0.0,0.0,0,0,0.0,0.0,0.0]
  	for i in range (0,3,1):		magneto[0][i]=dataset[2+i]	# mx/my/mz											(int16)
  	for i in range (0,3,1):		magneto[1][i]=dataset[5+i]	# magneto_raw		magneto in the body frame [mG]	(vector float)
  	for i in range (0,3,1):		magneto[2][i]=dataset[8+i]	# magneto_rectified									(vector float)
  	for i in range (0,3,1):		magneto[3][i]=dataset[11+i]	# magneto_offset									(vector float)
  	magneto[ 4] = dataset[14]								# heading_unwrapped 								(float)
  	magneto[ 5] = dataset[15]								# heading_gyro_unwrapped							(float)
  	magneto[ 6] = dataset[16]								# heading_fusion_unwrapped 							(float)
  	magneto[ 7] = dataset[17]								# magneto_calibration_ok							(char)
  	magneto[ 8] = dataset[18]								# magneto_state 									(uint32)
  	magneto[ 9] = dataset[19]								# magneto_radius									(float)
  	magneto[10] = dataset[20]								# error_mean 										(float)
  	magneto[11] = dataset[21]								# error_var											(float)
  	return(magneto)
  
  ##### ID = 23 ### "wind_speed" ################################################
  def decode_ID23(packet):		#NAVDATA_WIND_TAG
  	dataset = struct.unpack_from("HHfffffffffffff", packet, offsetND)
  	if dataset[1] != 56 and dataset[1] != 64:
  		print "*** ERROR : navdata-wind_speed-Package (ID=23) has the wrong size !!!"
  	wind_speed = [0.0,0.0,[0.0,0.0],[0.0,0.0,0.0,0.0,0.0,0.0],[0.0,0.0,0.0]]
  	wind_speed[0]    = dataset[2]							# wind_speed 			   					(float)
  	wind_speed[1]    = dataset[3]							# wind_angle								(float)
  	wind_speed[2][0] = dataset[4]							# wind_compensation_theta 					(float)
  	wind_speed[2][1] = dataset[5]							# wind_compensation_phi						(float)
  	for i in range (0,6,1):	wind_speed[3][i]=dataset[6+i]	# state_x[1-6]								(float)
  	for i in range (0,3,1):	wind_speed[4][i]=dataset[7+i]	# magneto_debug[1-3]						(float)
  	return(wind_speed)
  
  ##### ID = 24 ### "kalman_pressure" ###########################################
  def decode_ID24(packet):  		#NAVDATA_KALMAN_PRESSURE_TAG
  	dataset = struct.unpack_from("HHffffffffff?f?ff??", packet, offsetND)
  	if dataset[1] != 72:	print "*** ERROR : navdata-wind_speed-Package (ID=24) has the wrong size !!!"
  	kalman_pressure = [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0.0,False,0.0,0.0,False,False]
  	kalman_pressure[ 0] = dataset[2]	# offset_pressure 			   					(float)
  	kalman_pressure[ 1] = dataset[3]	# est_z											(float)
  	kalman_pressure[ 2] = dataset[4]	# est_zdot 										(float)
  	kalman_pressure[ 3] = dataset[5]	# est_bias_PWM 									(float)
  	kalman_pressure[ 4] = dataset[6]	# est_biais_pression							(float)
  	kalman_pressure[ 5] = dataset[7]	# offset_US 			   						(float)
  	kalman_pressure[ 6] = dataset[8]	# prediction_US									(float)
  	kalman_pressure[ 7] = dataset[9]	# cov_alt 										(float)
  	kalman_pressure[ 8] = dataset[10]	# cov_PWM										(float)
  	kalman_pressure[ 9] = dataset[11]	# cov_vitesse									(float)
  	kalman_pressure[10] = dataset[12]	# bool_effet_sol								(bool)
  	kalman_pressure[11] = dataset[13]	# somme_inno									(float)
  	kalman_pressure[12] = dataset[14]	# flag_rejet_US									(bool)
  	kalman_pressure[13] = dataset[15]	# u_multisinus									(float)
  	kalman_pressure[14] = dataset[16]	# gaz_altitude									(float)
  	kalman_pressure[15] = dataset[17]	# Flag_multisinus								(bool)
  	kalman_pressure[16] = dataset[18]	# Flag_multisinus_debut							(bool)
  	return(kalman_pressure)
  
  ##### ID = 25 ### "hdvideo_stream" ############################################
  def decode_ID25(packet):		#NAVDATA_HDVIDEO-TAG
  	dataset = struct.unpack_from("HHfffffff", packet, offsetND)
  	if dataset[1] != 32:	print "*** ERROR : navdata-hdvideo_stream-Package (ID=25) has the wrong size !!!"
  	hdvideo_stream = [0.0,0.0,0.0,0.0,0.0,0.0,0.0]
  	hdvideo_stream[0] = dataset[2]	# hdvideo_state 			   					(float)
  	hdvideo_stream[1] = dataset[3]	# storage_fifo_nb_packets						(float)
  	hdvideo_stream[2] = dataset[4]	# storage_fifo_size 							(float)
  	hdvideo_stream[3] = dataset[5]	# usbkey_size 			USB key in kb (no key=0)(float)
  	hdvideo_stream[4] = dataset[6]	# usbkey_freespace		USB key in kb (no key=0)(float)
  	hdvideo_stream[5] = dataset[7]	# frame_number 			PaVE field of the frame starting to be encoded for the HD stream (float)
  	hdvideo_stream[6] = dataset[8]	# usbkey_remaining_time	[sec]					(float)
  	return(hdvideo_stream)
  
  ##### ID = 26 ### "wifi" ######################################################
  def decode_ID26(packet):		#NAVDATA_WIFI_TAG
  	dataset = struct.unpack_from("HHI", packet, offsetND)
  	if dataset[1] != 8:	print "*** ERROR : navdata-wifi-Package (ID=26) has the wrong size !!!"
  	wifi = dataset[2]	# link_quality 			   								(uint32)
  	return(wifi)
  
  ##### ID = 27 ### "zimmu_3000" ################################################
  def decode_ID27(packet):  #NAVDATA_ZIMU_3000_TAG
  	dataset = struct.unpack_from("HHif", packet, offsetND)
  	if dataset[1] != 12 and dataset[1] != 216:		# 216 since firmware 2.4.8 ?
  		print "*** ERROR : navdata-zimmu_3000-Package (ID=27) has the wrong size !!!"
  	zimmu_3000 = [0,0.0]
  	zimmu_3000[0] = dataset[2]	# vzimmuLSB 			   							(int32)
  	zimmu_3000[1] = dataset[3]	# vzfind 			   								(float)
  	return(zimmu_3000)
  
  ##### Footer ### "chksum" #####################################################
  def decode_Footer(packet,allpacket):   ### Decode Checksum options-package ID=65535
  	dataset = struct.unpack_from("HHI", packet, offsetND)
  	if dataset[1] != 8:	print "*** ERROR : Checksum-Options-Package (ID=65535) has the wrong size !!!"
  	chksum = 	[0,False]
  	chksum[0] = dataset[2]
  	sum, plen =	0, len(allpacket)-8
  	for i in range (0,plen,1):	sum += ord(allpacket[i])		# Slows down this Navdata-subprocess massivly
  	if sum == chksum[0]:	chksum[1] = True
  	return(chksum)
  
  
  ###############################################################################
  ### Navdata-Decoding
  ###############################################################################
  def getDroneStatus(packet):
  	arState =	[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
  	checksum = 	(0,False)
  	length =	len(packet)
  	dataset = 	struct.unpack_from("IIII", packet, 0)	# Reading (Header, State, Sequence, Vision)
  	offsetND =	struct.calcsize("IIII")
  	
  ###############################=-
  ###	Decode Options-Packages ###=-
  ###############################=-
  def getNavdata(packet,choice):
  	navdata =	{}
  	length =	len(packet)
  	dataset = 	struct.unpack_from("IIII", packet, 0)	# Reading (Header, State, Sequence, Vision)
  	navdata["state"] = decode_Header(dataset)
  	offsetND = 	struct.calcsize("IIII")
  	#Demo-mode contains normally Option-Packages with ID=0 (_navdata_demo_t), ID=16 (seems empty) and ID=65535 (checksum)
  	# Full Mode contains
  	while offsetND < length:
  		dataset =  struct.unpack_from("HH", packet, offsetND)	# Reading (Header, Length)
  		if dataset[0]== 0 and choice[ 0]: navdata["demo"]				= decode_ID0(packet[offsetND:])
  		if dataset[0]== 1 and choice[ 1]: navdata["time"] 				= decode_ID1(packet[offsetND:])
  		if dataset[0]== 2 and choice[ 2]: navdata["raw_measures"]		= decode_ID2(packet[offsetND:])
  		if dataset[0]== 3 and choice[ 3]: navdata["phys_measures"] 		= decode_ID3(packet[offsetND:])
  		if dataset[0]== 4 and choice[ 4]: navdata["gyros_offsets"] 		= decode_ID4(packet[offsetND:])
  		if dataset[0]== 5 and choice[ 5]: navdata["euler_angles"]		= decode_ID5(packet[offsetND:])
  		if dataset[0]== 6 and choice[ 6]: navdata["references"] 		= decode_ID6(packet[offsetND:])
  		if dataset[0]== 7 and choice[ 7]: navdata["trims"]				= decode_ID7(packet[offsetND:])
  		if dataset[0]== 8 and choice[ 8]: navdata["rc_references"] 		= decode_ID8(packet[offsetND:])
  		if dataset[0]== 9 and choice[ 9]: navdata["pwm"]				= decode_ID9(packet[offsetND:])
  		if dataset[0]==10 and choice[10]: navdata["altitude"]			= decode_ID10(packet[offsetND:])
  		if dataset[0]==11 and choice[11]: navdata["vision_raw"] 		= decode_ID11(packet[offsetND:])
  		if dataset[0]==12 and choice[12]: navdata["vision_of"] 			= decode_ID12(packet[offsetND:])
  		if dataset[0]==13 and choice[13]: navdata["vision"]				= decode_ID13(packet[offsetND:])
  		if dataset[0]==14 and choice[14]: navdata["vision_perf"]		= decode_ID14(packet[offsetND:])
  		if dataset[0]==15 and choice[15]: navdata["trackers_send"] 		= decode_ID15(packet[offsetND:])
  		if dataset[0]==16 and choice[16]: navdata["vision_detect"]		= decode_ID16(packet[offsetND:])
  		if dataset[0]==17 and choice[17]: navdata["watchdog"] 			= decode_ID17(packet[offsetND:])
  		if dataset[0]==18 and choice[18]: navdata["adc_data_frame"] 	= decode_ID18(packet[offsetND:])
  		if dataset[0]==19 and choice[19]: navdata["video_stream"] 		= decode_ID19(packet[offsetND:])
  		if dataset[0]==20 and choice[20]: navdata["games"]				= decode_ID20(packet[offsetND:])
  		if dataset[0]==21 and choice[21]: navdata["pressure_raw"]		= decode_ID21(packet[offsetND:])
  		if dataset[0]==22 and choice[22]: navdata["magneto"]			= decode_ID22(packet[offsetND:])
  		if dataset[0]==23 and choice[23]: navdata["wind_speed"] 		= decode_ID23(packet[offsetND:])
  		if dataset[0]==24 and choice[24]: navdata["kalman_pressure"]	= decode_ID24(packet[offsetND:])
  		if dataset[0]==25 and choice[25]: navdata["hdvideo_stream"]		= decode_ID25(packet[offsetND:])
  		if dataset[0]==26 and choice[26]: navdata["wifi"]				= decode_ID26(packet[offsetND:])
  		if dataset[0]==27 and choice[27]: navdata["zimmu_3000"]			= decode_ID27(packet[offsetND:])
  		if dataset[0]==65535 and choice[28]: navdata["chksum"]			= decode_Footer(packet[offsetND:],packet)
  		offsetND += dataset[1]
  	return(navdata)
  
  ###############################=-
  ###	Threads					###=- 
  ###############################=-
  def reconnect(navdata_pipe, commitsuicideND, DroneIP,NavDataPort):
  	if not commitsuicideND:		navdata_pipe.sendto("\x01\x00\x00\x00", (DroneIP, NavDataPort))
  
  def watchdogND(parentPID):
  	global commitsuicideND
  	while not commitsuicideND:
  		time.sleep(1)
  		try : 		os.getpgid(parentPID)
  		except:		commitsuicideND=True
  # It seems that you just have to reinitialize the network-connection once and the drone keeps on sending forever then.
  
  def mainloopND(DroneIP,NavDataPort,parent_pipe,parentPID):
  	global commitsuicideND
  	something2send, MinimalPacketLength, timetag =	False, 30, 0
  	packetlist =		["demo","time","raw_measures","phys_measures","gyros_offsets","euler_angles","references","trims","rc_references","pwm","altitude","vision_raw","vision_of","vision","vision_perf","trackers_send","vision_detect","watchdog","adc_data_frame","video_stream","games","pressure_raw","magneto","wind_speed","kalman_pressure","hdvideo_stream","wifi","zimmu_3000","chksum","state"]
  	choice =			[False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,False,True]
  	overallchoice =		False	# This and oneTimeFailOver is necessary because of a bug (?) of AR.Drone sending NavData in DemoMode...
  	oneTimeFailOver =	True 	# ...while setting a configuration the drone sends the next DemoMode-package with just its status.
  	debug =				False
  	showCommands =		False
  
  	# Checks if the main-process is running and sends ITS own PID back
  	ThreadWatchdogND = threading.Thread(target=watchdogND,args=[parentPID])
  	ThreadWatchdogND.start()
  
  	# Prepare communication-pipes
  	pipes = []
  	pipes.append(parent_pipe)
  
  	navdata_pipe = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
  	navdata_pipe.setblocking(0)
  	navdata_pipe.bind(('', NavDataPort))
  	pipes.append(navdata_pipe)
  	
  	# start connection
  	reconnect(navdata_pipe, commitsuicideND, DroneIP, NavDataPort)
  	netHeartbeat = threading.Timer(2.0, reconnect, [navdata_pipe,commitsuicideND,DroneIP,NavDataPort,])	# Inits the first Network-Heartbeat (2 secs after disconnection the drone stops sending)
  	netHeartbeat.start()
  
  	if choice.count(True) > 0: overallchoice = True
  
  	while not commitsuicideND:
  		in_pipe, out_pipe, dummy2 = select.select(pipes, [], [], 0.5)		# When something is in a pipe...
  		for ip in in_pipe:	
  			if ip == parent_pipe:
  				cmd = parent_pipe.recv()
  				if showCommands:		print "** Com -> Nav : ",cmd
  				# Signal to stop this process and all its threads
  				if cmd == "die!":				commitsuicideND = True
  				# Enables/disables Debug-bit
  				elif cmd == "debug":
  					debug = True
  					print "NavData-Process :    running"
  				elif cmd == "undebug":
  					debug = False
  				# Enables/disables Debug-bit
  				elif cmd == "showCommands":	showCommands = True
  				elif cmd == "hideCommands":	showCommands = False
  				elif cmd == "reconnect":		reconnect(navdata_pipe, commitsuicideND, DroneIP, NavDataPort)
  				# Sets explicitly the value-packages which shall be decoded
  				elif cmd[0] == "send":
  					if cmd[1].count("all"):
  						for i in range (0,len(choice),1):		choice[i] = True
  					else:
  						for i in range (0,len(packetlist),1):
  							if cmd[1].count(packetlist[i]):	choice[i] = True
  							else:								choice[i] = False
  					if choice.count(True) > 0:		overallchoice = True
  					else: 								overallchoice = False
  				# Adds value-packages to the other which shall be decoded
  				elif cmd[0] == "add":
  					for i in range (0,len(packetlist),1):
  						if cmd[1].count(packetlist[i]):		choice[i] = True
  					if cmd[1].count("all"):
  						for i in range (0,len(choice),1):		choice[i] = True
  					if choice.count(True)>0: 			overallchoice = True
  					else: 								overallchoice = False
  				# Deletes packages from the value-package-list which shall not be decoded anymore
  				elif cmd[0] == "block":
  					if cmd[1].count("all"):
  						for i in range (0,len(packetlist),1):	choice[i] = False
  					else:
  						for i in range (0,len(packetlist),1):
  							if cmd.count(packetlist[i]):		choice[i] = False
  					if choice.count(True) > 0:			overallchoice = True
  					else: 								overallchoice = False
  			if ip == navdata_pipe:
  				try:
  					netHeartbeat.cancel()									# Connection is alive, Network-Heartbeat not necessary for a moment
  					Packet = 		navdata_pipe.recv(65535)				# Receiving raw NavData-Package
  					netHeartbeat = 	threading.Timer(2.1,reconnect,[navdata_pipe,commitsuicideND,DroneIP,NavDataPort])
  					netHeartbeat.start()									# Network-Heartbeat is set here, because the drone keeps on sending NavData (vid, etc you have to switch on)
  					timestamp =		timetag									# Setting up decoding-time calculation
  					timetag = 		time.time()
  					if overallchoice:
  						try:		lastdecodedNavData=decodedNavData
  						except:		lastdecodedNavData={}
  					decodedNavData = getNavdata(Packet,choice)
  					state = decodedNavData["state"]
  					# If there is an abnormal small NavPacket, the last NavPacket will be sent out with an error-tag
  					NoNavData = False
  					if len(Packet)<MinimalPacketLength and overallchoice:	decodedNavData, NoNavData = lastdecodedNavData, True
  					dectime = time.time()-timetag
  					# Sends all the data to the mainprocess
  					parent_pipe.send((decodedNavData, state[0:32], state[32], timestamp, dectime, NoNavData))
  				except IOError:	pass
  	suicideND = True
  	netHeartbeat.cancel()
  	if debug:	print "NavData-Process :    committed suicide"
  	# TestMe
  	try:	navdata_pipe.close()
  	except:	pass
  	
  	
  ##################################################################################################
  ###### Playground																			######
  ##################################################################################################
  if __name__ == "__main__":
  ###
  ### Here you can write your first test-codes and play around with them
  ###
  
  	import time
  	import ps_drone
  
  	drone = ps_drone.Drone()								# Start using drone					
  	drone.printBlue("Battery: ")
  
  	drone.startup()											# Connects to drone and starts subprocesses
  	drone.reset()											# Always good, at start
  
  	while drone.getBattery()[0] == -1:	time.sleep(0.1)		# Waits until the drone has done its reset
  	time.sleep(0.5)											# Give it some time to fully awake 
  
  	drone.printBlue("Battery: "+str(drone.getBattery()[0])+"%  "+str(drone.getBattery()[1]))	# Gives a battery-status
  	
  	stop = False
  	while not stop:
  		key = drone.getKey()
  		if key == " ":
  			if drone.NavData["demo"][0][2] and not drone.NavData["demo"][0][3]:	drone.takeoff()
  			else:																drone.land()
  		elif key == "0":	drone.hover()
  		elif key == "w":	drone.moveForward()
  		elif key == "s":	drone.moveBackward()
  		elif key == "a":	drone.moveLeft()
  		elif key == "d":	drone.moveRight()
  		elif key == "q":	drone.turnLeft()
  		elif key == "e":	drone.turnRight()
  		elif key == "7":	drone.turnAngle(-10,1)
  		elif key == "9":	drone.turnAngle( 10,1)
  		elif key == "4":	drone.turnAngle(-45,1)
  		elif key == "6":	drone.turnAngle( 45,1)
  		elif key == "1":	drone.turnAngle(-90,1)
  		elif key == "3":	drone.turnAngle( 90,1)
  		elif key == "8":	drone.moveUp()
  		elif key == "2":	drone.moveDown()
  		elif key == "*":	drone.doggyHop()
  		elif key == "+":	drone.doggyNod()
  		elif key == "-":	drone.doggyWag()
  		elif key != "":		stop = True
  
  	print "Batterie: "+str(drone.getBattery()[0])+"%  "+str(drone.getBattery()[1])	# Gives a battery-status
  