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()
from web_server import Webserver
try:
w = Webserver()
except OSError:
import machine
machine.reset()
print('OSError, im do machine.restart()')
w.start()
