def tcpThread():
	global glider_data, tape_data, tcp_dict, data_init, tape_init, tape_x, tape_y, glider_x, glider_y, glider_heading, glider_roll, glider_altitiude
	s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
	s.connect((tcp_ip, tcp_port))
	while tcp_loop:
		data = s.recv(buffer_size)
		data = data.splitlines()[0]
		try:
			tcp_dict = json.loads(str(data))
			channel = tcp_dict['CHANNEL'].upper()
			if channel == "NEW_TAPE":
				if not tape_init:
					tape_init = True
				tape_data = tcp_dict
				tape_x = [item for sublist in tape_data['x'] for item in sublist]
				tape_y = [item for sublist in tape_data['y'] for item in sublist]
			elif channel == "GLIDER_STATE":
				if not data_init:
					data_init = True
				glider_data = tcp_dict
				glider_x, glider_y = lla2flatearth(glider_data['LAT'], glider_data['LON'])
				glider_heading = glider_data["HEADING"]*180/math.pi
				glider_roll = glider_data["PHI"]*180/math.pi
				glider_altitiude = glider_data["ALT"]
		except:
			pass
	s.close()
	print "Closing Visualization TCP Thread"
示例#2
0
def tcpThread():
	global glider_data, tape_data, tcp_dict, data_init, tape_init, tape_x, tape_y, glider_x, glider_y, glider_heading, glider_roll, glider_altitiude, desired_phi, desired_phi_tape
	global cur_wpt_x, cur_wpt_y, nxt_wpt_x, nxt_wpt_y, wall_l_x, wall_l_y, wall_r_x, wall_r_y, dist_from_line, dist_from_wall, tape_count, glider_vdown, prev_tape_count
	global old_tape_x, old_tape_y, old2_tape_x, old2_tape_y, trigger_wall_l_x, trigger_wall_l_y, trigger_wall_r_x, trigger_wall_r_y
	s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
	s.connect((tcp_ip, tcp_port))
	while tcp_loop:
		data = s.recv(buffer_size)
		data = data.splitlines()[0]
		try:
			tcp_dict = json.loads(str(data))
			channel = tcp_dict['CHANNEL'].upper()
			if channel == "NEW_TAPE":
				if not tape_init:
					tape_init = True
				tape_data = tcp_dict
				tape_count = tape_data['count']
				if prev_tape_count != tape_count:
					prev_tape_count = tape_count
					if tape_count > 1:
						old2_tape_x = old_tape_x
						old2_tape_y = old_tape_y
					if tape_count > 0:
						old_tape_x = tape_x
						old_tape_y = tape_y
				tape_x = [item for sublist in tape_data['x'] for item in sublist]
				tape_y = [item for sublist in tape_data['y'] for item in sublist]
				cur_wpt_x = tape_data['CUR_WPT'][0]
				cur_wpt_y = tape_data['CUR_WPT'][1]
				nxt_wpt_x = tape_data['NXT_WPT'][0]
				nxt_wpt_y = tape_data['NXT_WPT'][1]
				wall_l_x = tape_data['WALL_LEFT'][0]
				wall_l_y = tape_data['WALL_LEFT'][1]
				wall_r_x = tape_data['WALL_RIGHT'][0]
				wall_r_y = tape_data['WALL_RIGHT'][1]
				trigger_wall_l_x = tape_data['TRIGGER_WALL_LEFT'][0]
				trigger_wall_l_y = tape_data['TRIGGER_WALL_LEFT'][1]
				trigger_wall_r_x = tape_data['TRIGGER_WALL_RIGHT'][0]
				trigger_wall_r_y = tape_data['TRIGGER_WALL_RIGHT'][1]
				dist_from_line = tape_data['DIST_FROM_LINE']
				dist_from_wall = tape_data['DIST_FROM_WALL']
				desired_phi = tape_data['DESIRED_PHI']
				desired_phi_tape = tape_data['DESIRED_PHI_TAPE']
			elif channel == "GLIDER_STATE":
				if not data_init:
					data_init = True
				glider_data = tcp_dict
				#glider_x, glider_y = lla2flatearth(glider_data['LAT'], glider_data['LON'])
				glider_x, glider_y = helper.lla2flatearth(glider_data['LAT'], glider_data['LON'])
				glider_heading = glider_data["HEADING"]*180/math.pi
				glider_roll = glider_data["PHI"]*180/math.pi
				glider_altitiude = glider_data["ALT"]
				glider_vdown = glider_data["V_DOWN"]
		except:
			pass
	s.close()
	print "Closing Visualization TCP Thread"
示例#3
0
	def calcCurLine(self, x, y, u):
		x1 = x[self.cur_idx][0]
		y1 = y[self.cur_idx][0]
		x2 = x[self.nxt_idx][0]
		y2 = y[self.nxt_idx][0]
		uav_x, uav_y = lla2flatearth(self.state_estimation.x[0,0], self.state_estimation.x[1,0])
		
		#for calculating the trigger wall (index 29)
		xt = x[29][0]
		yt = y[29][0]
		xt_p = x[28][0]
		yt_p = y[28][0]
		dxt = (xt-xt_p)
		dyt = (yt-yt_p)
		anglet = math.atan2(dyt, dxt) * 180.0/math.pi
		perpanglet = (anglet + 90) * math.pi/180.0
		self.trigger_wall_l_x = xt + 100*math.cos(perpanglet)
		self.trigger_wall_r_x = xt - 100*math.cos(perpanglet)
		self.trigger_wall_l_y = yt + 100*math.sin(perpanglet)
		self.trigger_wall_r_y = yt - 100*math.sin(perpanglet)

		#for calcuating the current line
		dx = (x2-x1)
		dy = (y2-y1)
		
		#for calculating error to and side of current line of tape
		err_num = abs(dy*uav_x - dx*uav_y - x1*y2 + x2*y1)
		err_den = np.sqrt(dx**2 + dy**2)
		self.error_tape = err_num/err_den
		self.side_tape = (uav_x - x1)*dy - (uav_y - y1)*dx
		if self.side_tape > 0:
		    self.side_tape = -1 #right of line
		else:
		    self.side_tape = 1 #left of line
		self.error_tape = self.error_tape*self.side_tape
		 
		#for calculating the current wall
		angle = math.atan2(dy, dx) * 180.0/math.pi
		perpangle = (angle + 90) * math.pi/180.0
		xperpleft = x2 + 50*math.cos(perpangle)
		xperpright = x2 - 50*math.cos(perpangle)
		yperpleft = y2 + 50*math.sin(perpangle)
		yperpright = y2 - 50*math.sin(perpangle)
		
		#for calculating the error to and side of current wall
		dx = (xperpright-xperpleft)
		dy = (yperpright-yperpleft)
		err_num = abs(dy*uav_x - dx*uav_y - xperpleft*yperpright + xperpright*yperpleft)
		err_den = np.sqrt(dx**2 + dy**2)
		self.error_wall = err_num/err_den
		self.side_wall = (uav_x - xperpleft)*dy - (uav_y - yperpleft)*dx
		if self.side_wall > 0:
		    self.side_wall = -1 #behind wall
		else:
		    self.side_wall = 1 #in front of wall
		    if self.cur_idx < (len(x)-2):
				self.cur_idx = self.nxt_idx
				self.nxt_idx = self.cur_idx + 1

		self.cur_wpt_x = x1
		self.cur_wpt_y = y1
		self.nxt_wpt_x = x2
		self.nxt_wpt_y = y2
		self.wall_l_x = xperpleft
		self.wall_l_y = yperpleft
		self.wall_r_x = xperpright
		self.wall_r_y = yperpright

		#index into current control from tape
		
		# TODO make this a linear interpolation look up
		frac = calcPerpendicularFrac([x1,y1],[x2,y2],[uav_x,uav_y])
		interpu =  u[self.cur_idx][0]
		#if (frac<1):
		#	interpu =  u[self.cur_idx][0] + frac*(u[self.nxt_idx][0] - u[self.cur_idx][0])
		if (self.side_wall==1):
			interpu = 0.0
		self.phi_desired_tape = interpu
		
		#calculate delta psi (difference between line angle and uav angle)
		uav_angle = self.heading2XY(self.state_estimation.x[8,0]*180.0/math.pi)
		vd = [0.0,0.0,0.0]
		vr = [0.0,0.0,0.0]
		vd[0] = math.cos(angle*math.pi/180.0)
		vd[1] = math.sin(angle*math.pi/180.0)
		vr[0] = math.cos(uav_angle)
		vr[1] = math.sin(uav_angle)
		#(angle*math.pi/180.0 - uav_angle)
		self.psi_delta = math.acos(vd[0]*vr[0] + vd[1]*vr[1])
		cr = cross(vd,vr)
		if cr[2]>0:
			self.psi_delta = -self.psi_delta