def run(self): try: print "starting web server" # initialise web server which will display final image web = Webserver(balloon_config.config.parser, (lambda: self.frame_filtered)) print "initialising camera" # initialise video capture balloon_video.init_camera() print "Ready to go!" while (True): # get a frame frame = balloon_video.capture_image() # Convert BGR to HSV hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # get latest colour filters self.read_config_file(False) # use trackbar positions to filter image colour_low = np.array([self.h_low, self.s_low, self.v_low]) colour_high = np.array([self.h_high, self.s_high, self.v_high]) # Threshold the HSV image mask = cv2.inRange(hsv_frame, colour_low, colour_high) #PAPOU_MOD Blur & Erode blurred = cv2.GaussianBlur(mask, (3, 3), 0) # Erode erode_kernel = np.ones((3, 3), np.uint8) eroded_img = cv2.erode(blurred, erode_kernel, iterations=2) # dilate dilate_kernel = np.ones((10, 10), np.uint8) dilated_img = cv2.dilate(eroded_img, dilate_kernel, iterations=1) # Bitwise-AND mask and original image self.frame_filtered = cv2.bitwise_and(frame, frame, mask=dilated_img) # handle interrupts except: print "interrupted, exiting" # exit and close web server print "exiting..." web.close()
def run(self): try: print "starting web server" # initialise web server which will display final image web = Webserver(balloon_config.config.parser, (lambda : self.frame_filtered)) print "initialising camera" # initialise video capture balloon_video.init_camera() print "Ready to go!" while(True): # get a frame frame = balloon_video.capture_image() # Convert BGR to HSV hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # get latest colour filters self.read_config_file(False) # use trackbar positions to filter image colour_low = np.array([self.h_low,self.s_low,self.v_low]) colour_high = np.array([self.h_high,self.s_high,self.v_high]) # Threshold the HSV image mask = cv2.inRange(hsv_frame, colour_low, colour_high) # Erode erode_kernel = np.ones((3,3),np.uint8); eroded_img = cv2.erode(mask,erode_kernel,iterations = 1) # dilate dilate_kernel = np.ones((10,10),np.uint8); dilated_img = cv2.dilate(eroded_img,dilate_kernel,iterations = 1) # Bitwise-AND mask and original image self.frame_filtered = cv2.bitwise_and(frame,frame, mask=dilated_img) # handle interrupts except: print "interrupted, exiting" # exit and close web server print "exiting..." web.close()
def main(self): web = Webserver(balloon_config.config.parser, (lambda: self.frame)) # initialise camera balloon_video.init_camera() video_writer = balloon_video.open_video_writer() # get start time start_time = time.time() # loop for 10 seconds looking for circles while (time.time() - start_time < 20): # Take each frame frame = balloon_video.capture_image() self.frame = frame # is there the x & y position in frame of the largest balloon found_in_image, xpos, ypos, size = self.analyse_frame(frame) # display image cv2.imshow('frame', frame) # write the frame video_writer.write(frame) # exit if user presses ESC k = cv2.waitKey(5) & 0xFF if k == 27: break print "exiting..." web.close() # uncomment line below if window with real-time video was displayed cv2.destroyAllWindows() # release camera balloon_video.close_camera()
def main(self): web = Webserver(balloon_config.config.parser, (lambda : self.frame)) # initialise camera balloon_video.init_camera() video_writer = balloon_video.open_video_writer() # get start time start_time = time.time() # loop for 10 seconds looking for circles while(time.time() - start_time < 20): # Take each frame frame = balloon_video.capture_image() self.frame = frame # is there the x & y position in frame of the largest balloon found_in_image, xpos, ypos, size = self.analyse_frame(frame) # display image cv2.imshow('frame',frame) # write the frame video_writer.write(frame) # exit if user presses ESC k = cv2.waitKey(5) & 0xFF if k == 27: break print "exiting..." web.close() # uncomment line below if window with real-time video was displayed cv2.destroyAllWindows() # release camera balloon_video.close_camera()
def run(self): # yawangle=0.0 # imgnum=0 # bal_rc1=self.vehicle.parameters['RC1_TRIM'] # bal_rc2=self.vehicle.parameters['RC2_TRIM'] RC7MAX=self.vehicle.parameters['RC7_MAX'] #for tuning RC7MIN=self.vehicle.parameters['RC7_MIN'] RC6MIN=self.vehicle.parameters['RC6_MIN'] RC6MAX=self.vehicle.parameters['RC6_MAX'] RC6MID=(RC6MIN+RC6MAX)/2 # print "RC6MIN=",RC6MIN,",RC6MAX=",RC6MAX,",RC6MID=",RC6MID # last_mode=None gain=1.0 last_gain=1.0 # deltasatx=0 # deltasaty=0 try: web=Webserver(balloon_config.config.parser,(lambda:self.webframe),(lambda:self.webframe_masked)) #Shang uncommented here. #web=Webserver(balloon_config.config.parser,(lambda:self.frame)) fourcc = cv2.cv.CV_FOURCC(*'XVID') video_name='FOPD_%s.avi' % (time.time()) video_writer = cv2.VideoWriter(video_name,fourcc, 1.0, (640,480)) print "Initialising camera." # initialise video capture camera = balloon_video.get_camera() log_gname='FOPD_%s.txt' % (time.time()) gf = open(log_gname, mode='a')#w gf.write("Guided mode log DATA %s\n"%(time.localtime())) gf.write("time\t RC5VAL\t yaw_angle\t objposx\t objposy\t RC7VAL\t vx\t vy\t gain\t size\t roll\t pitch\t RC6VAL\n") #coeffx\t coeffy\t # gf.close cycle_num=0 coeffx=1 coeffy=1 previous_time=0 yprevious_error=0 xprevious_error=0 upx9=0 upx8=0 upx7=0 upx6=0 upx5=0 upx4=0 upx3=0 upx2=0 upx1=0 deltay9=0 deltay8=0 deltay7=0 deltay6=0 deltay5=0 deltay4=0 deltay3=0 deltay2=0 deltay1=0 upy9=0 upy8=0 upy7=0 upy6=0 upy5=0 upy4=0 upy3=0 upy2=0 upy1=0 deltax9=0 deltax8=0 deltax7=0 deltax6=0 deltax5=0 deltax4=0 deltax3=0 deltax2=0 deltax1=0 print "Ready to go!" while(True): #print(self.vehicle) if self.vehicle is None: #print('0.5') self.vehicle=self.api.get_vehicles()[0] print('no vehicle') #print('1.5') # get a frame _, frame = camera.read() newx,newy=frame.shape[1],frame.shape[0] newframe=cv2.resize(frame,(newx,newy)) self.frame=newframe self.max=web.my_static.max; self.min=web.my_static.min; self.smax=web.my_static.smax self.smin=web.my_static.smin self.vmax=web.my_static.vmax self.vmin=web.my_static.vmin #self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(frame) self.object_found,xpos,ypos,size=self.object_find(newframe) #self.object_found,xpos,ypos,size=self.object_find_hough(frame) #if self.object_found: #adding text to image font = cv2.FONT_HERSHEY_SIMPLEX #Python:#cv2.putText(img, text, org, fontFace, fontScale, color[, thickness[, lineType[,bottomLeftOrigin]]]) cv2.putText(newframe,'Object Found? ' + str(self.object_found)[0],(10,20), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Mode: ' + self.vehicle.mode.name,(10,40), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Time: ' + str(time.time()),(10,60), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Alt: ' + str(self.vehicle.location.global_relative_frame),(10,100), font, 0.3,(255,255,255),1) cv2.putText(newframe,'RC5: ' + str(self.vehicle.channels['5'])+' 6: '+ str(self.vehicle.channels['5'])+' 7: '+ str(self.vehicle.channels['7']),(10,120), font, 0.3,(255,255,255),1) cv2.putText(newframe,'max: ' + str(self.max),(10,240), font, 0.3,(255,255,255),1) #print 'New video frame. Time: %s' % (time.time()) #print xpos print self.vehicle.channels['5'] if self.vehicle.channels['5'] is not None: RC5VAL=float(self.vehicle.channels['5']) else: RC5VAL=1400 if self.vehicle.channels['6'] is not None: RC6VAL=float(self.vehicle.channels['6']) else: RC6VAL=1400 if self.vehicle.channels['7'] is not None: RC7VAL=self.vehicle.channels['7'] else: RC7VAL=1400 if RC7VAL==0 or RC7MAX is None: gain=last_gain #else: # gain=7.0*(RC7VAL-RC7MIN)/(RC7MAX-RC7MIN) if RC7VAL>=1099 and RC7VAL<=1150: gain=0 elif RC7VAL>=1150 and RC7VAL<=1200: gain=0.05 elif RC7VAL>=1200 and RC7VAL<=1250: gain=0.1 elif RC7VAL>=1250 and RC7VAL<=1300: gain=0.2 elif RC7VAL>=1300 and RC7VAL<=1350: gain=0.3 elif RC7VAL>=1350 and RC7VAL<=1400: gain=0.4 elif RC7VAL>=1400 and RC7VAL<=1450: gain=0.5 elif RC7VAL>=1450 and RC7VAL<=1500: gain=0.6 elif RC7VAL>=1500 and RC7VAL<=1550: gain=0.7 elif RC7VAL>=1550 and RC7VAL<=1600: gain=0.8 elif RC7VAL>=1600 and RC7VAL<=1650: gain=0.9 elif RC7VAL>=1650: gain=1.0 last_gain=gain cv2.putText(newframe,'Gain: ' + str(gain),(10,140), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Roll: ' + str(self.vehicle.attitude.roll),(10,160), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Pitch: ' + str(self.vehicle.attitude.pitch),(10,180), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Battery: ' + str(self.vehicle.battery),(10,200), font, 0.3,(255,255,255),1) cv2.putText(newframe,'[vx, vy, vz ](in m/s): ' + str(self.vehicle.velocity),(10,220), font, 0.3,(255,255,255),1) #gain1=0.05*gain #gain=gain1 #print 'gain is:',gain,'RC7=',RC7VAL #RC6VAL=1 #if RC6VAL==1: #if RC6VAL<RC6MID:#vision-controlled if 1: #print 'vision-controlled.' yaw_origin=self.vehicle.attitude.yaw roll_origin=self.vehicle.attitude.roll pitch_origin=self.vehicle.attitude.pitch #attitude_origin=self.vehicle.attitude # print "Heading: %s" % self.vehicle.heading cv2.line(newframe,(320,240),(int(320+100*math.sin(-yaw_origin)),int(240-100*math.cos(-yaw_origin))),(255,255,255),1) cv2.putText(newframe,"N: " + str(math.degrees(yaw_origin))[0:5],(int(320+100*math.sin(-yaw_origin)),int(240-100*math.cos(-yaw_origin))),font,0.3,(255,255,255),1) #print 'yaw_origin=', yaw_origin if self.vehicle.mode.name!="LOITER":#visual-guided #if 1:#visual-guided ''' if last_mode!="GUIDED": self.condition_yaw(0) last_mode="GUIDED" ''' #print "---------------New period-----------------" #print 'vehicle.mode = Guided. Gain =',gain if self.object_found: #if 1: # print('object found') deltax=xpos-320 #cv2.line(frame,(320,240),320) deltay=240-ypos #print 'yaw-origin: deltaxpos deltaypos ', yaw_origin,deltax,deltay #print 'roll, pitch=', roll_origin,pitch_origin #Kd=0.013; #dt=time.time()-previous_time #get the sampling time #if previous_time==0: # dt=0.58 #previous_time=time.time() upx=0.6708*upx1 + 3.159*upx2 - 1.998*upx3 - 3.618*upx4+ 2.099*upx5 + 1.753*upx6 - 0.882*upx7 - 0.2949*upx8+ 0.1106*upx9+5.268*deltay - 8.506*deltay1 - 8.5*deltay2 + 18.26*deltay3 + 1.801*deltay4- 12.23*deltay5 + 2.027*deltay6 + 2.468*deltay7 - 0.5658*deltay8- 0.03059*deltay9 if upx>gain: upx=gain elif upx<-gain: upx=-gain upx9=upx8 upx8=upx7 upx7=upx6 upx6=upx5 upx5=upx4 upx4=upx3 upx3=upx2 upx2=upx1 upx1=upx deltay9=deltay8 deltay8=deltay7 deltay7=deltay6 deltay6=deltay5 deltay5=deltay4 deltay4=deltay3 deltay3=deltay2 deltay2=deltay1 deltay1=deltay #print "dt:",dt upy=0.6708*upy1 + 3.159*upy2 - 1.998*upy3 - 3.618*upy4+ 2.099*upy5 + 1.753*upy6 - 0.882*upy7 - 0.2949*upy8+ 0.1106*upy9+5.268*deltax - 8.506*deltax1 - 8.5*deltax2 + 18.26*deltax3 + 1.801*deltax4- 12.23*deltax5 + 2.027*deltax6 + 2.468*deltax7 - 0.5658*deltax8- 0.03059*deltax9 if upy>gain: upy=gain elif upy<-gain: upy=-gain upy9=upy8 upy8=upy7 upy7=upy6 upy6=upy5 upy5=upy4 upy4=upy3 upy3=upy2 upy2=upy1 upy1=upy deltax9=deltax8 deltax8=deltax7 deltax7=deltax6 deltax6=deltax5 deltax5=deltax4 deltax4=deltax3 deltax3=deltax2 deltax2=deltax1 deltax1=deltax print 'upx, upy=', upx,upy self.vel_control(upx,upy) cv2.putText(newframe,'upx(N): ' + str(upx) + ', upy(E): ' + str(upy),(10,80), font, 0.3,(255,255,255),1) # time.sleep(0.1) #deltatime=time.time()-self.timelast # gf = open(log_gname, mode='a') gf.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n"%(time.time(),RC5VAL,yaw_origin,xpos,ypos,RC7VAL,upx,upy,gain,size,roll_origin,pitch_origin,RC6VAL)) # gf.close() else:#not-found print "Guided mode,object not found. Gain = %f; Time = %f" % (gain,time.time()) # self.vel_control(0,0) #time.sleep(0.5) # RC6VAL=float(self.vehicle.channels['6']) #deltatime=time.time()-self.timelast # gf = open(log_gname, mode='a') gf.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n"%(time.time(),0,0,-566,-566,0,0,0,0,size,roll_origin,pitch_origin,RC6VAL)) # gf.close() else:#non-guided print "Vision control but non-guided. Yaw=", math.degrees(yaw_origin), ", Time=", time.time() # RC6VAL=float(self.vehicle.channels['6']) #deltatime=time.time()-self.timelast # gf = open(log_gname, mode='a') # gf.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n"%(time.time(),0,0,-566,-566,0,0,0,0,size,roll_origin,pitch_origin,RC6VAL)) # gf.close() # if last_mode=="GUIDED": # self.vel_control(0,0) # last_mode=self.vehicle.mode.name # time.sleep(0.5) # else:#non-vision-controlled # print "non-vision-controlled,landing......" #deltatime=time.time()-self.timelast # gf = open(log_gname, mode='a') # gf.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n"%(time.time(),yaw_origin,angle_yaw,xpos,ypos,error_total,upx,upy,gain,size,RC6VAL,RC6VAL,RC6VAL)) # gf.close() ''' while(self.vehicle.mode.name=="GUIDED"): self.vehicle.mode.name="LAND" self.vehicle.mode = VehicleMode("LAND") self.vehicle.flush() time.sleep(1) while(self.vehicle.armed is True): self.vehicle.armed=False self.vehicle.flush() time.sleep(1) ''' #if RC6VAL<RC6MID: video_writer.write(newframe) #print 'Video recorded.' #print 'RC6VAL:',RC6VAL #print 'RC6MID:',RC6MID self.webframe=newframe #time.sleep(1) # handle interrupts except KeyboardInterrupt: print 'interrupted, exiting' # exit and close web server print "exiting..." gf.close() web.close() #Shang uncommented here. video_writer.release() bus.exit()
def run(self): yawangle=0.0 imgnum=0 bal_rc1=self.vehicle.parameters['RC1_TRIM'] bal_rc2=self.vehicle.parameters['RC2_TRIM'] RC7MAX=self.vehicle.parameters['RC7_MAX'] RC7MIN=self.vehicle.parameters['RC7_MIN'] RC6MIN=self.vehicle.parameters['RC6_MIN'] RC6MAX=self.vehicle.parameters['RC6_MAX'] RC6MID=(RC6MIN+RC6MAX)/2 # print "RC6MIN=",RC6MIN,",RC6MAX=",RC6MAX,",RC6MID=",RC6MID last_mode=None gain=1.0 last_gain=1.0 deltasatx=0 deltasaty=0 try: web=Webserver(balloon_config.config.parser,(lambda:self.webframe)) #Shang uncommented here. #web=Webserver(balloon_config.config.parser,(lambda:self.frame)) fourcc = cv2.cv.CV_FOURCC(*'XVID') video_name='video_%s.avi' % (time.time()) video_writer = cv2.VideoWriter(video_name,fourcc, 1.0, (640,480)) print "Initialising camera." # initialise video capture camera = balloon_video.get_camera() log_gname='BoTest%s.txt' % (time.time()) gf = open(log_gname, mode='w') gf.write("Guided velocity mode log DATA %s\n"%(time.localtime())) gf.write("time\t yaw(org,radians)\t yaw_angle\t objposx\t objposy\t error_total\t vx\t vy\t gain\t imgradius\t roll\t pitch\t RC6VAL\n") #coeffx\t coeffy\t gf.close cycle_num=0 coeffx=1 coeffy=1 print "Ready to go!" while(True): #print(self.vehicle) if self.vehicle is None: #print('0.5') self.vehicle=self.api.get_vehicles()[0] print('no vehicle') #print('1.5') # get a frame _, frame = camera.read() newx,newy=frame.shape[1],frame.shape[0] newframe=cv2.resize(frame,(newx,newy)) self.frame=newframe #self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(frame) self.object_found,xpos,ypos,size=self.object_find(newframe) #self.object_found,xpos,ypos,size=self.object_find_hough(frame) #if self.object_found: #adding text to image font = cv2.FONT_HERSHEY_SIMPLEX #Python:#cv2.putText(img, text, org, fontFace, fontScale, color[, thickness[, lineType[,bottomLeftOrigin]]]) cv2.putText(newframe,'Object Found? ' + str(self.object_found)[0],(10,20), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Mode: ' + self.vehicle.mode.name,(10,40), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Time: ' + str(time.time()),(10,60), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Alt: ' + str(self.vehicle.location.global_relative_frame),(10,100), font, 0.3,(255,255,255),1) cv2.putText(newframe,'RC5: ' + str(self.vehicle.channels['5']),(10,120), font, 0.3,(255,255,255),1) #print 'New video frame. Time: %s' % (time.time()) #print xpos RC6VAL=float(self.vehicle.channels['6']) RC7VAL=self.vehicle.channels['7'] if RC7VAL==0 or RC7MAX is None: gain=last_gain #else: # gain=7.0*(RC7VAL-RC7MIN)/(RC7MAX-RC7MIN) if RC7VAL>=1099 and RC7VAL<=1150: gain=0 elif RC7VAL>=1150 and RC7VAL<=1200: gain=0.05 elif RC7VAL>=1200 and RC7VAL<=1250: gain=0.1 elif RC7VAL>=1250 and RC7VAL<=1300: gain=0.2 elif RC7VAL>=1300 and RC7VAL<=1350: gain=0.3 elif RC7VAL>=1350 and RC7VAL<=1400: gain=0.4 elif RC7VAL>=1400 and RC7VAL<=1450: gain=0.5 elif RC7VAL>=1450 and RC7VAL<=1500: gain=0.6 elif RC7VAL>=1500 and RC7VAL<=1550: gain=0.7 elif RC7VAL>=1550 and RC7VAL<=1600: gain=0.8 elif RC7VAL>=1600 and RC7VAL<=1650: gain=0.9 elif RC7VAL>=1650: gain=1.0 last_gain=gain cv2.putText(newframe,'Gain: ' + str(gain),(10,140), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Roll: ' + str(self.vehicle.attitude.roll),(10,160), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Pitch: ' + str(self.vehicle.attitude.pitch),(10,180), font, 0.3,(255,255,255),1) cv2.putText(newframe,'Battery: ' + str(self.vehicle.battery),(10,200), font, 0.3,(255,255,255),1) cv2.putText(newframe,'[vx, vy, vz ](in m/s): ' + str(self.vehicle.velocity),(10,220), font, 0.3,(255,255,255),1) #gain1=0.05*gain #gain=gain1 #print 'gain is:',gain,'RC7=',RC7VAL #RC6VAL=1 #if RC6VAL==1: if RC6VAL<RC6MID:#vision-controlled #print 'vision-controlled.' yaw_origin=self.vehicle.attitude.yaw roll_origin=self.vehicle.attitude.roll pitch_origin=self.vehicle.attitude.pitch #attitude_origin=self.vehicle.attitude print "Heading: %s" % self.vehicle.heading cv2.line(newframe,(320,240),(int(320+100*math.sin(-yaw_origin)),int(240-100*math.cos(-yaw_origin))),(255,255,255),1) cv2.putText(newframe,"N: " + str(math.degrees(yaw_origin))[0:5],(int(320+100*math.sin(-yaw_origin)),int(240-100*math.cos(-yaw_origin))),font,0.3,(255,255,255),1) #print 'yaw_origin=', yaw_origin if self.vehicle.mode.name=="GUIDED":#visual-guided if last_mode!="GUIDED": self.condition_yaw(0) last_mode="GUIDED" #print "---------------New period-----------------" #print 'vehicle.mode = Guided. Gain =',gain if self.object_found: print('object found') pixelsize=0.03 deltax=xpos-320 #cv2.line(frame,(320,240),320) deltay=240-ypos #print 'yaw-origin: deltaxpos deltaypos ', yaw_origin,deltax,deltay print 'roll, pitch=', roll_origin,pitch_origin #print'deltax y' #print deltax,deltay angle_yawtmp=0.5*3.14-math.atan2(deltay,deltax)+yaw_origin#radians angle_yaw=wrap_PI(angle_yawtmp) #print'angle1' angle_yawdeg=math.degrees(angle_yaw) angle_pitch=0 speedSet=0.15 #spdfactor=math.sqrt((deltax*daltax+deltay*deltay))*0.1 realrx=deltax*pixelsize realry=deltay*pixelsize #spdfactor=((rx**2+ry**2)**0.5*0.01 spddist=math.sqrt(realrx**2+realry**2) #print "dist:",spddist #if spddist<=0.5: # self.vehicle.mode=VehicleMode("LAND") # self.vehicle.flush() spdfactor=spddist*0.2 ''' # get time since last time velocity pid controller was run dt = self.vel_xy_pid.get_dt(2.0) # get speed towards balloon based on balloon distance #speed = self.balloon_distance * self.vel_dist_ratio speed=spdfactor # apply min and max speed limit speed = min(speed, self.vel_speed_max) speed = max(speed, self.vel_speed_min) # apply acceleration limit speed_chg_max = self.vel_accel * dt speed = min(speed, self.vel_speed_last + speed_chg_max) speed = max(speed, self.vel_speed_last - speed_chg_max) # record speed for next iteration self.vel_speed_last = speed ''' cos_pitch=math.cos(angle_pitch) velocity_x=gain*speedSet*math.cos(angle_yaw)*cos_pitch*spdfactor velocity_y=float(gain*speedSet*math.sin(angle_yaw)*cos_pitch*spdfactor) #velocity_x=speedSet*math.cos(angle_yaw)*cos_pitch*speed #velocity_y=speedSet*math.sin(angle_yaw)*cos_pitch*speed #velocity_z=speedSet*math.sin(angle_pitch) velocity_z=0 #print 'angle and velocity....' #print angle_yawdeg,velocity_x,velocity_y #start calculate the pid controller's term dtx=self.vx_fopi.get_dt(2.0) #errorx=math.sqrt(deltax*deltax+deltay*deltay)*math.cos(angle_yaw)*0.265/size#radius of the red circle is 25.5cm #errorx=math.sqrt(deltax*deltax+deltay*deltay)*math.cos(angle_yaw)*0.265/(1*size)#radius of the red circle is 25.5cm #errorx=math.sqrt(deltax*deltax+deltay*deltay)*math.cos(angle_yaw)*0.012/(1*size)#radius of the blue circle is 1.2cm #errorx=errorx-deltasatx*0.3 #anti-wind #errorx=errorx*gain error_total=math.sqrt(deltax*deltax+deltay*deltay)*0.265/(1*size)#radius of the blue circle is 1.2cm error_x=error_total*math.cos(angle_yaw) error_y=error_total*math.sin(angle_yaw) #print'error_total,gain,angletoN',error_total,gain,angle_yaw upx=error_total*math.cos(angle_yaw)*gain*coeffx upy=error_total*math.sin(angle_yaw)*gain*coeffy upx_fo=self.vx_fopi.get_fopi(error_x, dtx)*gain upy_fo=upx_fo/math.tan(angle_yaw) #print 'upx,upx_fo,upy,upy_fo to object',upx,upx_fo,upy,upy_fo #print 'err_total,error_x',error_total,error_x upx=upx_fo upy=upy_fo #print 'upx,upy to drone',upx,upy if upx>1.0: upx=1.0 upy=upx/math.tan(angle_yaw) elif upx<-1.0: upx=-1.0 upy=upx/math.tan(angle_yaw) if upy>1.0: upy=1.0 upx=upy*math.tan(angle_yaw) elif upy<-1.0: upy=-1.0 upx=upy*math.tan(angle_yaw) #errorx=velocity_x #upx=upx*gain ''' dty=self.vy_fopi.get_dt(2.0) #errory=velocity_y #errory=math.sqrt(deltax*deltax+deltay*deltay)*math.sin(angle_yaw)*0.265/size errory=math.sqrt(deltax*deltax+deltay*deltay)*math.sin(angle_yaw)*0.265/size #errory=math.sqrt(deltax*deltax+deltay*deltay)*math.sin(angle_yaw)*0.012/size #errory=errory-deltasaty*0.3 errory=errory*gain print'errory,gain',errory,gain upy=self.vy_fopi.get_fopi(errory, dty) #upy=upy*gain ''' ''' print'gain,num,sampling time',gain,cycle_num,dtx print 'control speed of x from fo-pi to p upx,velx,upy,vely',upx,velocity_x,upy,velocity_y ''' if deltay > 100: upx=gain elif deltay < -100: upx=-gain else: upx=deltay*0.01*gain if deltax > 100: upy=gain elif deltax < -100: upy=-gain else: upy=deltax*0.01*gain self.vel_control(upx,upy) cv2.putText(newframe,'upx(N): ' + str(upx) + ', upy(E): ' + str(upy),(10,80), font, 0.3,(255,255,255),1) time.sleep(0.1) #deltatime=time.time()-self.timelast gf = open(log_gname, mode='a') gf.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n"%(time.time(),yaw_origin,angle_yaw,xpos,ypos,error_total,upx,upy,gain,size,roll_origin,pitch_origin,RC6VAL)) gf.close() else:#not-found print "Guided mode,object not found. Gain = %f; Time = %f" % (gain,time.time()) self.vel_control(0,0) #time.sleep(0.5) RC6VAL=float(self.vehicle.channels['6']) #deltatime=time.time()-self.timelast gf = open(log_gname, mode='a') gf.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n"%(time.time(),0,0,-566,-566,0,0,0,0,size,roll_origin,pitch_origin,RC6VAL)) gf.close() else:#non-guided print "Vision control but non-guided. Yaw=", math.degrees(yaw_origin), ", Time=", time.time() RC6VAL=float(self.vehicle.channels['6']) #deltatime=time.time()-self.timelast gf = open(log_gname, mode='a') gf.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n"%(time.time(),0,0,-566,-566,0,0,0,0,size,roll_origin,pitch_origin,RC6VAL)) gf.close() if last_mode=="GUIDED": self.vel_control(0,0) last_mode=self.vehicle.mode.name time.sleep(0.5) else:#non-vision-controlled print "non-vision-controlled,landing......" #deltatime=time.time()-self.timelast gf = open(log_gname, mode='a') gf.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n"%(time.time(),yaw_origin,angle_yaw,xpos,ypos,error_total,upx,upy,gain,size,RC6VAL,RC6VAL,RC6VAL)) gf.close() while(self.vehicle.mode.name=="GUIDED"): self.vehicle.mode.name="LAND" self.vehicle.mode = VehicleMode("LAND") self.vehicle.flush() time.sleep(1) while(self.vehicle.armed is True): self.vehicle.armed=False self.vehicle.flush() time.sleep(1) video_writer.write(newframe) self.webframe=newframe #time.sleep(1) # handle interrupts except KeyboardInterrupt: print 'interrupted, exiting' # exit and close web server print "exiting..." web.close() #Shang uncommented here. video_writer.release()