def run(self):

        # initialise video capture
        camera = balloon_video.get_camera()

        # create trackbars for color change
        cv2.namedWindow('Colour Filters')
        cv2.createTrackbar('Hue min','Colour Filters',self.h_low,255,self.empty_callback)
        cv2.createTrackbar('Hue max','Colour Filters',self.h_high,255,self.empty_callback)
        cv2.createTrackbar('Sat min','Colour Filters',self.s_low,255,self.empty_callback)
        cv2.createTrackbar('Sat max','Colour Filters',self.s_high,255,self.empty_callback)
        cv2.createTrackbar('Bgt min','Colour Filters',self.v_low,255,self.empty_callback)
        cv2.createTrackbar('Bgt max','Colour Filters',self.v_high,255,self.empty_callback)
        cv2.createTrackbar('Save','Colour Filters',0,10,self.save_callback)

        while(True):
            # get a frame
            _, frame = camera.read()
        
            # Convert BGR to HSV
            hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        
            # get latest trackbar positions
            self.h_low = cv2.getTrackbarPos('Hue min','Colour Filters')
            self.h_high = cv2.getTrackbarPos('Hue max','Colour Filters')
            self.s_low = cv2.getTrackbarPos('Sat min','Colour Filters')
            self.s_high = cv2.getTrackbarPos('Sat max','Colour Filters')
            self.v_low = cv2.getTrackbarPos('Bgt min','Colour Filters')
            self.v_high = cv2.getTrackbarPos('Bgt max','Colour Filters')
        
            # 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
            res = cv2.bitwise_and(frame,frame, mask=dilated_img)
        
            cv2.imshow('Original',frame)
            cv2.imshow('Mask',mask)
            cv2.imshow('Filtered Result',res)
            
            #cv2.imshow('grey_res',grey_res)
            k = cv2.waitKey(5) & 0xFF
            if k == 27:
                break

        # close all windows
        cv2.destroyAllWindows()
示例#2
0
    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
            camera = balloon_video.get_camera()

            print "Ready to go!"

            while (True):
                # get a frame
                _, frame = camera.read()

                # 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 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
            camera = balloon_video.get_camera()

            print "Ready to go!"

            while True:
                # get a frame
                _, frame = camera.read()

                # 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))

        camera = balloon_video.get_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 = camera.read()
            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)

	    print found_in_image, xpos, ypos, size
            # 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
        camera.release()
    def main(self):
        web = Webserver(balloon_config.config.parser, (lambda: self.frame))

        camera = balloon_video.get_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 = camera.read()
            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
        camera.release()
     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']
        last_mode=None
        gain=1.0

        deltasatx=0
        deltasaty=0  
        

        try:
            
            #web=Webserver(balloon_config.config.parser,(lambda:self.frame))
            
            fourcc = cv2.cv.CV_FOURCC(*'XVID')
            video_writer = cv2.VideoWriter('output.avi',fourcc, 1.0, (640,480))
            
            print "initialising camera"
            # initialise video capture
            camera = balloon_video.get_camera()
            
            log_gname='Guidedv%s' % (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 RC6VAL\n")
            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:              
              #   video_writer.write(newframe)
              #print xpos
              
              RC6VAL=float(self.vehicle.channel_readback['6'])
              RC7VAL=self.vehicle.channel_readback['7']  
              if RC7VAL<=0 or RC7MAX is None:
                 gain=1.0
              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:
                 gain=0.6

              print 'gain is:',gain

              #RC6VAL=1
              #if RC6VAL==1:
              if RC6VAL==RC6MIN:#vision-controlled
                           
                 
                 yaw_origin=self.vehicle.attitude.yaw
                 
                 if self.vehicle.mode.name=="GUIDED":#visual-guided
                    last_mode="GUIDED" 

                    

                    if self.object_found: 
                       print('object found')

                       pixelsize=0.03
                       deltax=xpos-320
                       deltay=240-ypos
                       print 'yaw-origin: deltaxpos  deltaypos ', yaw_origin,deltax,deltay

                       #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
                       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
                       
                       
                        
                       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
                       '''
                       up_total=self.vx_fopi.get_fopi(error_total, dtx)
                       upx=up_total*math.cos(angle_yaw)*gain
                       upy=up_total*math.sin(angle_yaw)*gain
                       '''
                       #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

                       self.vel_control(upx,upy)
                       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"%(deltatime,yaw_origin,angle_yaw,xpos,ypos,error_total,upx,upy,gain,size,coeffx,coeffy,RC6VAL))
                       gf.close()

                    else:#not-found
                         print "guided mode,object not found"
                         self.vel_control(0,0)
                         time.sleep(0.5)
                         
                         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\n"%(deltatime,0,0,-566,-566,0,0,0,0,size,-500))
                         gf.close()
                 else:#non-guided
                        
                        print "vision control but non-guided"
                        
                        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......"
                   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)
                      
                    
              #time.sleep(1)  
        # handle interrupts
        except:
            print "interrupted, exiting"

        # exit and close web server
        print "exiting..."
     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()  
     def run(self):

        yawangle=0.0
        last_mode=None
        try:
            #print "starting web server"
            # initialise web server which will display final image
            #web = Webserver(balloon_config.config.parser, (lambda : self.frame_filtered))
            #web=Webserver(balloon_config.config.parser,(lambda:self.frame))

                 
            print "initialising camera"
            # initialise video capture
            camera = balloon_video.get_camera()
            

            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('1')
              #print('1.5')
              
              # get a frame
              _, frame = camera.read()
              self.balloon_found, xpos, ypos, size = balloon_finder.analyse_frame(frame)
              
              self.frame=frame

              #deltatime=time.time()-self.timelast
              #self.timelast=time.time()
              #print "time difference:",deltatime
              #print "velocity x:",self.vehicle.velocity[0]
              
                
              if self.vehicle.mode.name=="alt_hold":
                 last_mode="alt_hold"
                 print "Overriding a RC channel" 
                 self.vehicle.channel_override = {"2" : 1400}
                 self.vehicle.flush()
                 print "Current overrides are:", self.vehicle.channel_override     
                 print "Cancel override"
                 time.sleep(3)
                 #self.vehicle.channel_override={"2":0}
                 #self.vehicle.flush()   
                 #time.sleep(1)
                 self.vehicle.channel_override = {"2" : 1600}
                 self.vehicle.flush()
                 time.sleep(3)
                 '''
                 self.vehicle.channel_override = {"1":1600,"2" : 1400}
                 self.vehicle.flush()

                 time.sleep(5)
                 print "Current overrides1 are right-forward:", self.vehicle.channel_override
                 self.vehicle.channel_override = {"1":1400,"2" : 1600}
                 self.vehicle.flush()

                 time.sleep(5)
                 print "Current overrides2 are left-afterward:", self.vehicle.channel_override
                 '''

              else:
                 if last_mode=="alt_hold":
                    self.vehicle.channel_override={"1":0,"2":0}
                    self.vehicle.flush()
                    time.sleep(0.08)
                    last_mode=None 

 

              #if self.balloon_found and self.vehicle.mode.name=="GUIDED":  
              if self.vehicle.mode.name=="GUIDED":#only for testing speed command,response characteristics 
                 last_mode="GUIDED"
                 self.vel_control(0,0)
                 time.sleep(3)
  
                 print"guided mode"

                 log_filename='posstep%s' % (time.time())
                 #print 'log filename',log_filename
                 f = open(log_filename, mode='a+')
                 f.write("STEP RESPONSE LOG DATA %s\n"%(time.localtime()))
                 f.write("time\tset_vx\tvx\tset_vy\tvy\tdeltax\tdeltay\tdeltaz\t\n")
                 '''
                 vel_setx=0.3
                 vel_sety=0
                 

                 if self.vehicle.location is None:
                    print 'location is unknown in direction-x'
                 else:
                    #ref_home_location = Location(self.vehicle.location.lat,self.vehicle.location.lon,self.vehicle.location.alt)
                    home_lat=self.vehicle.location.lat
                    home_lon=self.vehicle.location.lon
                    home_alt=self.vehicle.location.alt
                 print'flying x-direction'
                 self.timelast=time.time()
                 for i in range(1, 301):
                     if self.vehicle.mode.name=="GUIDED": 
                      print'index:',i
                      deltatime=time.time()-self.timelast
                      cur_lat=self.vehicle.location.lat
                      cur_lon=self.vehicle.location.lon
                      cur_alt=self.vehicle.location.alt
                      dlat=cur_lat-home_lat
                      dlon=cur_lon-home_lon
                      dalt=cur_alt-home_alt
                      deltax = dlat * pos_latlon_to_m
                      deltay = dlon* pos_latlon_to_m
                      deltaz = dalt  
                      f.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n" % (deltatime,vel_setx,self.vehicle.velocity[0],vel_sety,self.vehicle.velocity[1],deltax,deltay,deltaz))
                      self.vel_control(vel_setx,vel_sety)
 
                      time.sleep(0.1)
                 '''
                 self.vel_control(0,0)
                 time.sleep(3)
                 
                 if self.vehicle.location is None:
                    print 'location is unknown in direction-y'
                 else:
                    #ref_home_location = Location(self.vehicle.location.lat,self.vehicle.location.lon,self.vehicle.location.alt)
                    home_lat=self.vehicle.location.lat
                    home_lon=self.vehicle.location.lon
                    home_alt=self.vehicle.location.alt
                 print'flying y-direction'
                 self.timelast=time.time()  
                 vel_setx=0
                 vel_sety=0.3
                 for i in range(1, 201):
                    if self.vehicle.mode.name=="GUIDED":
                      print'y-index:',i
                      deltatime=time.time()-self.timelast

                      cur_lat=self.vehicle.location.lat
                      cur_lon=self.vehicle.location.lon
                      cur_alt=self.vehicle.location.alt
                      dlat=cur_lat-home_lat
                      dlon=cur_lon-home_lon
                      dalt=cur_alt-home_alt
                      deltax = dlat * pos_latlon_to_m
                      deltay = dlon * pos_latlon_to_m
                      deltaz = dalt
                      f.write("%f\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n" % (deltatime,vel_setx,self.vehicle.velocity[0],vel_sety,self.vehicle.velocity[1],deltax,deltay,deltaz))
                      self.vel_control(vel_setx,vel_sety)

                      time.sleep(0.1)

                 
                 self.vel_control(0,0)
                 time.sleep(3)
                    
                 print'ready to land...........'
                 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)

                 f.close()

  
                      #altitude_origin=self.get_mav_param('%s_MIN'  % param, 0) 
                      #print "altitude:",altitude_origin
                      #print "Location: %s" % self.vehicle.location.alt

              else:
                 if last_mode=="GUIDED":
                    last_mode=None
                    self.vel_control(0,0)


              time.sleep(0.1)
                
        # handle interrupts
        except:
            print "interrupted, exiting"

        # exit and close web server
        print "exiting..."
示例#10
0
    def run(self):

        # initialise video capture
        camera = balloon_video.get_camera()

        # create trackbars for color change
        cv2.namedWindow('Colour Filters')
        cv2.createTrackbar('Hue min', 'Colour Filters', self.h_low, 255,
                           self.empty_callback)
        cv2.createTrackbar('Hue max', 'Colour Filters', self.h_high, 255,
                           self.empty_callback)
        cv2.createTrackbar('Sat min', 'Colour Filters', self.s_low, 255,
                           self.empty_callback)
        cv2.createTrackbar('Sat max', 'Colour Filters', self.s_high, 255,
                           self.empty_callback)
        cv2.createTrackbar('Bgt min', 'Colour Filters', self.v_low, 255,
                           self.empty_callback)
        cv2.createTrackbar('Bgt max', 'Colour Filters', self.v_high, 255,
                           self.empty_callback)
        cv2.createTrackbar('Save', 'Colour Filters', 0, 10, self.save_callback)

        while (True):
            # get a frame
            _, frame = camera.read()

            # Convert BGR to HSV
            hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

            # get latest trackbar positions
            self.h_low = cv2.getTrackbarPos('Hue min', 'Colour Filters')
            self.h_high = cv2.getTrackbarPos('Hue max', 'Colour Filters')
            self.s_low = cv2.getTrackbarPos('Sat min', 'Colour Filters')
            self.s_high = cv2.getTrackbarPos('Sat max', 'Colour Filters')
            self.v_low = cv2.getTrackbarPos('Bgt min', 'Colour Filters')
            self.v_high = cv2.getTrackbarPos('Bgt max', 'Colour Filters')

            # 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
            res = cv2.bitwise_and(frame, frame, mask=dilated_img)

            cv2.imshow('Original', frame)
            cv2.imshow('Mask', mask)
            cv2.imshow('Filtered Result', res)

            #cv2.imshow('grey_res',grey_res)
            k = cv2.waitKey(5) & 0xFF
            if k == 27:
                break

        # close all windows
        cv2.destroyAllWindows()
     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']
        last_mode=None
        gain=1.0

        deltasatx=0
        deltasaty=0  
        

        try:
            
            #web=Webserver(balloon_config.config.parser,(lambda:self.frame))
            
            fourcc = cv2.cv.CV_FOURCC(*'XVID')
            video_writer = cv2.VideoWriter('output.avi',fourcc, 1.0, (640,480))
            
            print "initialising camera"
            # initialise video capture
            camera = balloon_video.get_camera()
            
            log_gname='Guidedv%s' % (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 RC6VAL\n")
            gf.close

 

            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)              
              #video_writer.write(newframe)
              #print xpos
              
              RC6VAL=float(self.vehicle.channel_readback['6'])
              RC7VAL=self.vehicle.channel_readback['7']  
              if RC7VAL<=0 or RC7MAX is None:
                 gain=1.0
              else:
                 gain=7.0*(RC7VAL-RC7MIN)/(RC7MAX-RC7MIN)
              
              #RC6VAL=1
              #if RC6VAL==1: 
              if RC6VAL==RC6MIN:#visual-guided    
              
                 
                 yaw_origin=self.vehicle.attitude.yaw

                 print "yaw-origin",yaw_origin

                 if self.vehicle.mode.name=="ALTHOLD":
                        last_mode="ALTHOLD"
                        vel_setx=0.5 
                        vel_sety=0
                        print "flying forward..."
                        for i in range(1, 31):
                              deltatime=time.time()-self.timelast

                              self.vel_control(vel_setx,vel_sety)
                                
                              time.sleep(0.5) 

                        print 'flying backward....'

                        self.vel_control(0,0)
                        time.sleep(5)

                        vel_setx=-0.5 
                        vel_sety=0 
                        for i in range(1, 31):
                            deltatime=time.time()-self.timelast

                            self.vel_control(vel_setx,vel_sety)
                            
                            time.sleep(0.5)

                        self.vel_control(0,0)
                        time.sleep(5)
                        print "flying end" 
                    
                 else:#non-guided mode
                      print "non-ALTHOLD"
                      if last_mode=="ALTHOLD":
                         self.vel_control(vel_setx,vel_sety)
                         time.sleep(0.5)
                         last_mode=None  
                             
                 
              else:#non-controlled  mode,land the drone
                   print "non-CONTROLLED mode"
                   
                   while(self.vehicle.mode.name=="ALTHOLD"):

                             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)
                   
                   print "landed"
                    
              time.sleep(0.1)
              #time.sleep(1)  
        # handle interrupts
        except:
            print "interrupted, exiting"

        # exit and close web server
        print "exiting..."