def draw_fake_balloon(self, frame, veh_pos, balloon_pos, vehicle_roll,
vehicle_pitch, vehicle_yaw):
# calculate bearing to balloon
bearing_to_balloon = PositionVector.get_bearing(veh_pos, balloon_pos)
yaw_to_balloon = balloon_utils.wrap_PI(bearing_to_balloon -
vehicle_yaw)
# calculate earth frame pitch angle from vehicle to balloon
pitch_to_balloon = vehicle_pitch + PositionVector.get_elevation(
veh_pos, balloon_pos)
#print "Fake Balloon Bearing:%f Pitch:%f Dist:%f" % (math.degrees(bearing_to_balloon), math.degrees(pitch_to_balloon), dist_to_balloon_xy)
# calculate pixel position of balloon
balloon_x = balloon_video.angle_to_pixels_x(
yaw_to_balloon) + balloon_video.img_center_x
balloon_y = balloon_video.angle_to_pixels_y(
pitch_to_balloon) + balloon_video.img_center_y
# calculate size of balloon in pixels from distance and size
dist_to_balloon_xyz = PositionVector.get_distance_xyz(
veh_pos, balloon_pos)
balloon_radius = balloon_utils.get_pixels_from_distance(
dist_to_balloon_xyz, balloon_finder.balloon_radius_expected)
# store balloon radius
self.last_balloon_radius = balloon_radius
# draw balloon
cv2.circle(frame, (balloon_x, balloon_y), balloon_radius,
self.fake_balloon_colour_bgr_scalar, -1)
def draw_fake_balloon(self, frame, veh_pos, balloon_pos, vehicle_roll, vehicle_pitch, vehicle_yaw):
# calculate bearing to balloon
bearing_to_balloon = PositionVector.get_bearing(veh_pos, balloon_pos)
yaw_to_balloon = balloon_utils.wrap_PI(bearing_to_balloon-vehicle_yaw)
# calculate earth frame pitch angle from vehicle to balloon
pitch_to_balloon = vehicle_pitch + PositionVector.get_elevation(veh_pos, balloon_pos)
#print "Fake Balloon Bearing:%f Pitch:%f Dist:%f" % (math.degrees(bearing_to_balloon), math.degrees(pitch_to_balloon), dist_to_balloon_xy)
# calculate pixel position of balloon
balloon_x = balloon_video.angle_to_pixels_x(yaw_to_balloon) + balloon_video.img_center_x
balloon_y = balloon_video.angle_to_pixels_y(pitch_to_balloon) + balloon_video.img_center_y
# calculate size of balloon in pixels from distance and size
dist_to_balloon_xyz = PositionVector.get_distance_xyz(veh_pos, balloon_pos)
balloon_radius = balloon_utils.get_pixels_from_distance(dist_to_balloon_xyz, balloon_finder.balloon_radius_expected)
# store balloon radius
self.last_balloon_radius = balloon_radius
# draw balloon
cv2.circle(frame,(balloon_x,balloon_y), balloon_radius, self.fake_balloon_colour_bgr_scalar, -1)
def get_attitude(self, desired_time_in_sec):
# return current attitude immediately if dictionary is empty
if len(self.att_dict) == 0:
return self.vehicle.attitude
# get times from dict
keys = sorted(self.att_dict.keys())
# debug
#print "AttDict looking for %f" % desired_time_in_sec
# initialise best before and after keys
key_before = keys[0]
time_diff_before = (key_before - desired_time_in_sec)
key_after = keys[len(keys)-1]
time_diff_after = (key_after - desired_time_in_sec)
# handle case where we hit the time exactly or the time is beyond the end
if (time_diff_before >= 0):
#debug
#print "Time %f was before first entry's time %f" % (desired_time_in_sec, key_before)
return self.att_dict[key_before]
if (time_diff_after <= 0):
#debug
#print "Time %f was after last entry's time %f" % (desired_time_in_sec, key_after)
return self.att_dict[key_after]
# iteration through attitude dictionary
for t in keys:
# calc this dictionary entry's time diff from the desired time
time_diff = t - desired_time_in_sec
# if before the desired time but after the current best 'before' time use it
if time_diff <= 0 and time_diff > time_diff_before:
time_diff_before = time_diff
key_before = t
# if after the desired time but before the current best 'before' time use it
if time_diff >= 0 and time_diff < time_diff_after:
time_diff_after = time_diff
key_after = t
# calc time between before and after attitudes
tot_time_diff = -time_diff_before + time_diff_after
# debug
if (tot_time_diff <= 0):
print "Div By Zero!"
print "des time:%f" % desired_time_in_sec
print "num keys:%d" % len(keys)
print "key bef:%f aft:%f" % (key_before, key_after)
print "keys: %s" % keys
# get attitude before and after
att_before = self.att_dict[key_before]
att_after = self.att_dict[key_after]
# interpolate roll, pitch and yaw values
interp_val = (-time_diff_before / tot_time_diff)
roll = wrap_PI(att_before.roll + (wrap_PI(att_after.roll - att_before.roll) * interp_val))
pitch = att_before.pitch + (att_after.pitch - att_before.pitch) * interp_val
yaw = wrap_PI(att_before.yaw + (wrap_PI(att_after.yaw - att_before.yaw) * interp_val))
ret_att = Attitude(pitch, yaw, roll)
return ret_att
def move_to_balloon(self):
# exit immediately if we are not controlling the vehicle
if not self.controlling_vehicle:
return
# get current time
now = time.time()
# exit immediately if it's been too soon since the last update
if (now - self.guided_last_update) < self.vel_update_rate:
return
# if we have a new balloon position recalculate velocity vector
if (self.balloon_found):
# calculate change in yaw since we began the search
yaw_error = wrap_PI(self.balloon_heading -
self.search_balloon_heading)
# calculate pitch vs ideal pitch angles. This will cause to attempt to get to 5deg above balloon
pitch_error = wrap_PI(self.balloon_pitch_top -
self.vel_pitch_target)
# 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
# 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
# calculate yaw correction and final yaw movement
yaw_correction = self.vel_xy_pid.get_pid(yaw_error, dt)
yaw_final = wrap_PI(self.search_balloon_heading + yaw_correction)
# calculate pitch correction and final pitch movement
pitch_correction = self.vel_z_pid.get_pid(pitch_error, dt)
pitch_final = wrap_PI(self.search_balloon_pitch_top -
pitch_correction)
# calculate velocity vector we wish to move in
self.guided_target_vel = balloon_finder.get_ef_velocity_vector(
pitch_final, yaw_final, speed)
# send velocity vector to flight controller
self.send_nav_velocity(self.guided_target_vel[0],
self.guided_target_vel[1],
self.guided_target_vel[2])
self.guided_last_update = now
# if have not seen the balloon
else:
# if more than a few seconds has passed without seeing the balloon give up
if now - self.last_spotted_time > self.lost_sight_timeout:
if self.debug:
print "Lost Balloon, Giving up"
# To-Do: start searching again or maybe slowdown?
self.complete()
def search_for_balloon(self):
# exit immediately if we are not controlling the vehicle or do not know our position
if not self.controlling_vehicle or self.vehicle_pos is None:
return
# exit immediately if we are not searching
if self.search_state <= 0:
return
# search states: 0 = not searching, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw
# search_state = 1: spinning and looking
if (self.search_state == 1):
# check if we have seen a balloon
if self.balloon_found:
# if this is the first balloon we've found or the closest, store it's position as the closest
if (self.search_balloon_pos is None) or (
self.balloon_distance < self.search_balloon_distance):
# check distance is within acceptable limits
if (self.mission_alt_min == 0
or self.balloon_pos.z >= self.mission_alt_min
) and (self.mission_alt_max == 0
or self.balloon_pos.z <= self.mission_alt_max
) and (self.mission_distance_max == 0
or self.balloon_distance <=
self.mission_distance_max):
# record this balloon as the closest
self.search_balloon_pos = self.balloon_pos
self.search_balloon_heading = self.balloon_heading
self.search_balloon_pitch_top = self.balloon_pitch_top # we target top of balloon
self.search_balloon_distance = self.balloon_distance
print "Found Balloon at heading:%f Alt:%f Dist:%f" % (
math.degrees(self.balloon_heading),
self.balloon_pos.z, self.balloon_distance)
else:
print "Balloon Ignored at heading:%f Alt:%f Dist:%f" % (
math.degrees(self.balloon_heading),
self.balloon_pos.z, self.balloon_distance)
# update yaw so we keep spinning
if math.fabs(
wrap_PI(self.vehicle.attitude.yaw -
self.search_target_heading)) < math.radians(
self.search_yaw_speed * 2.0):
# increase yaw target
self.search_target_heading = self.search_target_heading - math.radians(
self.search_yaw_speed)
self.search_total_angle = self.search_total_angle + math.radians(
self.search_yaw_speed)
# send yaw heading
self.condition_yaw(math.degrees(self.search_target_heading))
# end search if we've gone all the way around
if self.search_total_angle >= math.radians(360):
# if we never saw a balloon then just complete (return control to user or mission)
if self.search_balloon_pos is None:
print "Search did not find balloon, giving up"
self.search_state = 0
self.complete()
else:
# update target heading towards closest balloon and send to flight controller
self.search_target_heading = self.search_balloon_heading
self.condition_yaw(
math.degrees(self.search_target_heading))
self.search_state = 2 # advance towards rotating to best balloon stage
print "best balloon at %f" % math.degrees(
self.search_balloon_heading)
# search_state = 2: rotating to best balloon and double checking
elif (self.search_state == 2):
# check yaw is close to target
if math.fabs(
wrap_PI(self.vehicle.attitude.yaw -
self.search_target_heading)) < math.radians(5):
# reset targeting start time
if self.targeting_start_time == 0:
self.targeting_start_time = time.time()
# if targeting time has elapsed, double check the visible balloon is within acceptable limits
if (time.time() - self.targeting_start_time >
self.targeting_delay_time):
if self.balloon_found and (
not self.balloon_pos is None
) and (self.mission_alt_min == 0
or self.balloon_pos.z >= self.mission_alt_min) and (
self.mission_alt_max == 0
or self.balloon_pos.z <= self.mission_alt_max
) and (self.mission_distance_max == 0
or self.balloon_distance <=
self.mission_distance_max):
# balloon is within limits so reset balloon target
self.search_balloon_pos = self.balloon_pos
self.search_balloon_heading = self.balloon_heading
self.search_balloon_pitch_top = self.balloon_pitch_top
self.search_balloon_distance = self.balloon_distance
# move to final heading
self.search_target_heading = self.search_balloon_heading
self.condition_yaw(
math.degrees(self.search_target_heading))
# move to finalise yaw state
self.search_state = 3
print "Balloon was near expected heading, finalising yaw to %f" % (
math.degrees(self.search_balloon_heading))
# we somehow haven't found the balloon when we've turned back to find it so giveup
elif (time.time() - self.targeting_start_time) > (
self.targeting_delay_time + 1.0):
print "Balloon was not at expected heading: %f, giving up" % (
math.degrees(self.search_target_heading))
self.search_state = 0
self.complete()
# search_state = 3: finalising yaw
elif (self.search_state == 3):
# check yaw is close to target
if math.fabs(
wrap_PI(self.vehicle.attitude.yaw -
self.search_target_heading)) < math.radians(5):
# complete search, move should take-over
# Note: we don't check again for the balloon, but surely it's still there
self.search_state = 0
print "Finalised yaw to %f, beginning run" % (math.degrees(
self.search_balloon_heading))
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 move_to_balloon(self):
# exit immediately if we are not controlling the vehicle
if not self.controlling_vehicle:
return
# get current time
now = time.time()
# exit immediately if it's been too soon since the last update
if (now - self.guided_last_update) < self.vel_update_rate:
return;
# if we have a new balloon position recalculate velocity vector
if (self.balloon_found):
# calculate change in yaw since we began the search
yaw_error = wrap_PI(self.balloon_heading - self.search_balloon_heading)
# calculate pitch vs ideal pitch angles. This will cause to attempt to get to 5deg above balloon
pitch_error = wrap_PI(self.balloon_pitch_top - self.vel_pitch_target)
# 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
# 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
# calculate yaw correction and final yaw movement
yaw_correction = self.vel_xy_pid.get_pid(yaw_error, dt)
yaw_final = wrap_PI(self.search_balloon_heading + yaw_correction)
# calculate pitch correction and final pitch movement
pitch_correction = self.vel_z_pid.get_pid(pitch_error, dt)
pitch_final = wrap_PI(self.search_balloon_pitch_top + pitch_correction)
# calculate velocity vector we wish to move in
self.guided_target_vel = balloon_finder.get_ef_velocity_vector(pitch_final, yaw_final, speed)
# send velocity vector to flight controller
self.send_nav_velocity(self.guided_target_vel[0], self.guided_target_vel[1], self.guided_target_vel[2])
self.guided_last_update = now
# if have not seen the balloon
else:
# if more than a few seconds has passed without seeing the balloon give up
if now - self.last_spotted_time > self.lost_sight_timeout:
if self.debug:
print "Lost Balloon, Giving up"
# To-Do: start searching again or maybe slowdown?
self.complete()
def search_for_balloon(self):
# exit immediately if we are not controlling the vehicle or do not know our position
if not self.controlling_vehicle or self.vehicle_pos is None:
return
# exit immediately if we are not searching
if self.search_state <= 0:
return
# search states: 0 = not searching, 1 = spinning and looking, 2 = rotating to best balloon and double checking, 3 = finalise yaw
# search_state = 1: spinning and looking
if (self.search_state == 1):
# check if we have seen a balloon
if self.balloon_found:
# if this is the first balloon we've found or the closest, store it's position as the closest
if (self.search_balloon_pos is None) or (self.balloon_distance < self.search_balloon_distance):
# check distance is within acceptable limits
if (self.mission_alt_min == 0 or self.balloon_pos.z >= self.mission_alt_min) and (self.mission_alt_max == 0 or self.balloon_pos.z <= self.mission_alt_max) and (self.mission_distance_max == 0 or self.balloon_distance <= self.mission_distance_max):
# record this balloon as the closest
self.search_balloon_pos = self.balloon_pos
self.search_balloon_heading = self.balloon_heading
self.search_balloon_pitch_top = self.balloon_pitch_top # we target top of balloon
self.search_balloon_distance = self.balloon_distance
print "Found Balloon at heading:%f Alt:%f Dist:%f" % (math.degrees(self.balloon_heading), self.balloon_pos.z, self.balloon_distance)
else:
print "Balloon Ignored at heading:%f Alt:%f Dist:%f" % (math.degrees(self.balloon_heading), self.balloon_pos.z, self.balloon_distance)
# update yaw so we keep spinning
if math.fabs(wrap_PI(self.vehicle.attitude.yaw - self.search_target_heading)) < math.radians(self.search_yaw_speed * 2.0):
# increase yaw target
self.search_target_heading = self.search_target_heading - math.radians(self.search_yaw_speed)
self.search_total_angle = self.search_total_angle + math.radians(self.search_yaw_speed)
# send yaw heading
self.condition_yaw(math.degrees(self.search_target_heading))
# end search if we've gone all the way around
if self.search_total_angle >= math.radians(360):
# if we never saw a balloon then just complete (return control to user or mission)
if self.search_balloon_pos is None:
print "Search did not find balloon, giving up"
self.search_state = 0
self.complete()
else:
# update target heading towards closest balloon and send to flight controller
self.search_target_heading = self.search_balloon_heading
self.condition_yaw(math.degrees(self.search_target_heading))
self.search_state = 2 # advance towards rotating to best balloon stage
print "best balloon at %f" % math.degrees(self.search_balloon_heading)
# search_state = 2: rotating to best balloon and double checking
elif (self.search_state == 2):
# check yaw is close to target
if math.fabs(wrap_PI(self.vehicle.attitude.yaw - self.search_target_heading)) < math.radians(5):
# reset targeting start time
if self.targeting_start_time == 0:
self.targeting_start_time = time.time()
# if targeting time has elapsed, double check the visible balloon is within acceptable limits
if (time.time() - self.targeting_start_time > self.targeting_delay_time):
if self.balloon_found and (not self.balloon_pos is None) and (self.mission_alt_min == 0 or self.balloon_pos.z >= self.mission_alt_min) and (self.mission_alt_max == 0 or self.balloon_pos.z <= self.mission_alt_max) and (self.mission_distance_max == 0 or self.balloon_distance <= self.mission_distance_max):
# balloon is within limits so reset balloon target
self.search_balloon_pos = self.balloon_pos
self.search_balloon_heading = self.balloon_heading
self.search_balloon_pitch_top = self.balloon_pitch_top
self.search_balloon_distance = self.balloon_distance
# move to final heading
self.search_target_heading = self.search_balloon_heading
self.condition_yaw(math.degrees(self.search_target_heading))
# move to finalise yaw state
self.search_state = 3
print "Balloon was near expected heading, finalising yaw to %f" % (math.degrees(self.search_balloon_heading))
# we somehow haven't found the balloon when we've turned back to find it so giveup
elif (time.time() - self.targeting_start_time) > (self.targeting_delay_time + 1.0):
print "Balloon was not at expected heading: %f, giving up" % (math.degrees(self.search_target_heading))
self.search_state = 0
self.complete()
# search_state = 3: finalising yaw
elif (self.search_state == 3):
# check yaw is close to target
if math.fabs(wrap_PI(self.vehicle.attitude.yaw - self.search_target_heading)) < math.radians(5):
# complete search, move should take-over
# Note: we don't check again for the balloon, but surely it's still there
self.search_state = 0
print "Finalised yaw to %f, beginning run" % (math.degrees(self.search_balloon_heading))
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()