def climb(self): if(veh_control.get_location().alt < self.climb_altitude): sc_logger.text(sc_logger.GENERAL, 'climbing') veh_control.set_velocity(0,0,self.climb_rate) self.climbing = True else: sc_logger.text(sc_logger.GENERAL, 'Reached top of search zone') veh_control.set_velocity(0,0,0) self.climbing = False
def climb(self):
if (veh_control.get_location().alt < self.climb_altitude):
sc_logger.text(sc_logger.GENERAL, 'climbing')
veh_control.set_velocity(0, 0, self.climb_rate)
self.climbing = True
else:
sc_logger.text(sc_logger.GENERAL, 'Reached top of search zone')
veh_control.set_velocity(0, 0, 0)
self.climbing = False
def release_control(self):
sc_logger.text(sc_logger.GENERAL, 'Releasing control')
#put vehicle in stable state
veh_control.set_velocity(0, 0, 0)
#autopilot_land()
#dont let us take control ever again
self.pl_enabled = False
'''
def release_control(self): sc_logger.text(sc_logger.GENERAL, 'Releasing control') #put vehicle in stable state veh_control.set_velocity(0,0,0) #autopilot_land() #dont let us take control ever again self.pl_enabled = False '''
def move_to_target(self,target_info,attitude,location):
x,y = target_info[1]
#shift origin to center of image
x,y = shift_to_origin((x,y),self.camera_width,self.camera_height)
#this is necessary because the simulator is 100% accurate
if(self.simulator):
hfov = 48.7
vfov = 49.7
else:
hfov = self.camera_hfov
vfov = self.camera_vfov
#stabilize image with vehicle attitude
x -= (self.camera_width / hfov) * math.degrees(attitude.roll)
y += (self.camera_height / vfov) * math.degrees(attitude.pitch)
#convert to distance
X, Y = self.pixel_point_to_position_xy((x,y),location.alt)
#convert to world coordinates
target_heading = math.atan2(Y,X) % (2*math.pi)
target_heading = (attitude.yaw - target_heading)
target_distance = math.sqrt(X**2 + Y**2)
sc_logger.text(sc_logger.GENERAL, "Distance to target: {0}".format(round(target_distance,2)))
#calculate speed toward target
speed = target_distance * self.dist_to_vel
#apply max speed limit
speed = min(speed,self.vel_speed_max)
#calculate cartisian speed
vx = speed * math.sin(target_heading) * -1.0
vy = speed * math.cos(target_heading)
#only descend when on top of target
if(target_distance > self.descent_radius):
vz = 0
else:
vz = self.descent_rate
#send velocity commands toward target heading
veh_control.set_velocity(vx,vy,vz)
def move_to_target(self, target_info, attitude, location):
x, y = target_info[1]
#shift origin to center of image
x, y = shift_to_origin((x, y), self.camera_width, self.camera_height)
#this is necessary because the simulator is 100% accurate
if (self.simulator):
hfov = 48.7
vfov = 49.7
else:
hfov = self.camera_hfov
vfov = self.camera_vfov
#stabilize image with vehicle attitude
x -= (self.camera_width / hfov) * math.degrees(attitude.roll)
y += (self.camera_height / vfov) * math.degrees(attitude.pitch)
#convert to distance
X, Y = self.pixel_point_to_position_xy((x, y), location.alt)
#convert to world coordinates
target_heading = math.atan2(Y, X) % (2 * math.pi)
target_heading = (attitude.yaw - target_heading)
target_distance = math.sqrt(X**2 + Y**2)
sc_logger.text(
sc_logger.GENERAL,
"Distance to target: {0}".format(round(target_distance, 2)))
#calculate speed toward target
speed = target_distance * self.dist_to_vel
#apply max speed limit
speed = min(speed, self.vel_speed_max)
#calculate cartisian speed
vx = speed * math.sin(target_heading) * -1.0
vy = speed * math.cos(target_heading)
#only descend when on top of target
if (target_distance > self.descent_radius):
vz = 0
else:
vz = self.descent_rate
#send velocity commands toward target heading
veh_control.set_velocity(vx, vy, vz)
def straight_raise(self): veh_control.set_velocity(0,0,1) time.sleep(10)
def straight_descent(self): #veh_control.set_velocity(0,0,self.descent_rate) veh_control.set_velocity(0,0,self.descent_rate)
def main(self):
veh_control.connect(local_connect())
self.set_target_location(veh_control.get_location())
while(veh_control.is_connected()):
location = veh_control.get_location()
attitude = veh_control.get_attitude()
self.refresh_simulator(location,attitude)
frame = self.get_frame()
cv2.imshow('frame',frame)
key = cv2.waitKey(1)
print key
if key ==1113938:
veh_control.set_velocity(2,0,0) #forward
elif key == 1113940:
veh_control.set_velocity(-2,0,0) #backward
elif key == 1113937:
veh_control.set_velocity(0,-2,0) #left
elif key ==1113939:
veh_control.set_velocity(0,2,0) #right
elif(key == 1048690):
yaw = math.degrees(attitude.yaw) #yaw left
veh_control.set_yaw(yaw - 5)
elif(key == 1048692):
yaw = math.degrees(attitude.yaw) #yaw right
veh_control.set_yaw(yaw + 5)
elif(key == 1048677):
veh_control.set_velocity(0,0,-2) #down
elif(key == 1048689):
veh_control.set_velocity(0,0,2) #up
else:
veh_control.set_velocity(0,0,0) #still
def straight_descent(self):
veh_control.set_velocity(0, 0, self.descent_rate)
def autopilot_land(self):
#descend velocity
veh_control.set_velocity(0, 0, self.descent_rate)
def control(self, target_info, attitude, location):
#we have control from autopilot
if self.in_control:
valid_target = False
now = time.time()
#detected a target
if target_info[1] is not None:
self.target_detected = True
valid_target = True
initial_descent = False
self.last_valid_target = now
#attempt to use precision landing
if (self.inside_landing_area() == 1):
#we have detected a target in landing area
if (self.target_detected):
self.climbing = False
self.initial_descent = False
#we currently see target
if (valid_target):
sc_logger.text(sc_logger.GENERAL,
'Target detected. Moving to target')
#move to target
self.move_to_target(target_info, attitude, location)
#lost target
else:
#we have lost the target for more than settle_time
if (now - self.last_valid_target > self.settle_time):
self.target_detected = False
#temporarily lost target,
#top section of cylinder
if (veh_control.get_location().alt >
self.abort_height):
sc_logger.text(sc_logger.GENERAL,
'Lost Target: Straight Descent')
#continue descent
self.straight_descent()
else:
sc_logger.text(sc_logger.GENERAL,
'Lost Target: Holding')
#neutralize velocity
veh_control.set_velocity(0, 0, 0)
#there is no known target in landing area
else:
#currently searching
if (self.climbing):
self.climb()
#not searching, decide next move
else:
#top section of cylinder
if (veh_control.get_location().alt >
self.abort_height):
#initial descent entering cylinder
if (self.initial_descent):
sc_logger.text(sc_logger.GENERAL,
'No Target: Initial Descent')
#give autopilot control
self.autopilot_land()
#all other attempts prior to intial target detection
else:
sc_logger.text(sc_logger.GENERAL,
'No target: Straight descent')
#straight descent
self.straight_descent()
#lower section of cylinder
else:
#we can attempt another land
if (self.attempts < self.search_attempts):
self.attempts += 1
sc_logger.text(
sc_logger.GENERAL,
'Climbing to attempt {0}'.format(
self.attempts))
#start climbing
self.climb()
#give up and
else:
sc_logger.text(sc_logger.GENERAL,
'Out of attempts: Giving up')
#give autopilot control
self.autopilot_land()
#final descent
elif (self.inside_landing_area() == -1):
sc_logger.text(sc_logger.GENERAL, 'In final descent')
#straight descent
self.straight_descent()
self.target_detected = False
#outside cylinder
else:
sc_logger.text(sc_logger.GENERAL, 'Outside landing zone')
#give autopilot control
self.autopilot_land()
self.target_detected = False
self.initial_descent = True
#the program is running but the autopilot is in an invalid mode
else:
sc_logger.text(sc_logger.GENERAL, 'Not in control of vehicle')
def autopilot_land(self): #descend velocity veh_control.set_velocity(0,0,self.descent_rate)
def control(self,target_info,attitude,location):
#we have control from autopilot
if self.in_control:
valid_target = False
#now=0;
#print (time.time()-now);
now = time.time()
print now;
#detected a target
if target_info[1] is not None:
self.target_detected = True
valid_target = True
initial_descent = False
self.last_valid_target = now
#attempt to use precision landing
if(self.inside_landing_area() == 1):
#we have detected a target in landing area
if(self.target_detected):
self.climbing = False
self.initial_descent = False
#we currently see target
if(valid_target):
sc_logger.text(sc_logger.GENERAL, 'Target detected. Moving to target')
##add myself
if(veh_control.get_location().alt < 1):
self.autopilot_land()
sc_logger.text(sc_logger.GENERAL, 'Landing!!!!!!!!')
#move to target
else:
self.move_to_target(target_info,attitude,location)
#lost target
else:
#we have lost the target for more than settle_time
if(now - self.last_valid_target > self.settle_time):
self.target_detected = False
#temporarily lost target,
#top section of cylinder
if(veh_control.get_location().alt > self.abort_height):
sc_logger.text(sc_logger.GENERAL, 'Lost Target: Straight Descent')
#continue descent
self.straight_descent()
else:
sc_logger.text(sc_logger.GENERAL, 'Lost Target: Holding')
#neutralize velocity
veh_control.set_velocity(0,0,0)
#there is no known target in landing area
else:
#currently searching
if(self.climbing):
self.climb()
#not searching, decide next move
else:
#top section of cylinder
if(veh_control.get_location().alt > self.abort_height):
#initial descent entering cylinder
if(self.initial_descent):
sc_logger.text(sc_logger.GENERAL, 'No Target: Initial Descent')
#give autopilot control
self.autopilot_land()
#all other attempts prior to intial target detection
else:
sc_logger.text(sc_logger.GENERAL, 'No target: Straight descent')
#straight descent
self.straight_descent()
#lower section of cylinder
else:
#we can attempt another land
if(self.attempts < self.search_attempts):
self.attempts += 1
sc_logger.text(sc_logger.GENERAL, 'Climbing to attempt {0}'.format(self.attempts))
#start climbing
self.climb()
#give up and
else:
sc_logger.text(sc_logger.GENERAL, 'Out of attempts: Giving up')
#give autopilot control
self.autopilot_land()
#final descent
elif(self.inside_landing_area() == -1):
sc_logger.text(sc_logger.GENERAL, 'In final descent')
#straight descent
#change LAND mode
#self.straight_descent()
self.vehicle.mode = VehicleMode("LAND")
self.vehicle.flush()
self.target_detected = False
#outside cylinder
else:
sc_logger.text(sc_logger.GENERAL, 'Outside landing zone')
#give autopilot control
self.autopilot_land()
self.target_detected = False
self.initial_descent = True
#the program is running but the autopilot is in an invalid mode
else:
sc_logger.text(sc_logger.GENERAL, 'Not in control of vehicle')
def main(self):
veh_control.connect(local_connect())
self.set_target_location(veh_control.get_location())
while (veh_control.is_connected()):
location = veh_control.get_location()
attitude = veh_control.get_attitude()
self.refresh_simulator(location, attitude)
frame = self.get_frame()
cv2.imshow('frame', frame)
key = cv2.waitKey(1)
print key
if key == 1113938:
veh_control.set_velocity(2, 0, 0) #forward
elif key == 1113940:
veh_control.set_velocity(-2, 0, 0) #backward
elif key == 1113937:
veh_control.set_velocity(0, -2, 0) #left
elif key == 1113939:
veh_control.set_velocity(0, 2, 0) #right
elif (key == 1048690):
yaw = math.degrees(attitude.yaw) #yaw left
veh_control.set_yaw(yaw - 5)
elif (key == 1048692):
yaw = math.degrees(attitude.yaw) #yaw right
veh_control.set_yaw(yaw + 5)
elif (key == 1048677):
veh_control.set_velocity(0, 0, -2) #down
elif (key == 1048689):
veh_control.set_velocity(0, 0, 2) #up
else:
veh_control.set_velocity(0, 0, 0) #still
# if starting from mavproxy
if __name__ == "__builtin__":
# start precision landing
start = takeland()
start.connect()
# run strategy
start.arm_and_takeoff()
print veh_control.get_location().alt
start.changemode("GUIDED")
while veh_control.get_location().alt<100:
print veh_control.get_location().alt
#start.set_velocity(0,1,0);
#veh_control.set_velocity(0,1,0)
#veh_control.set_velocity(1,0,0)
#veh_control.set_velocity(-1,0,0)
veh_control.set_velocity(-1,-1,0)
time.sleep(0.5)
while veh_control.get_location().alt>100:
print veh_control.get_location().alt
#start.set_velocity(0,1,0);
#veh_control.set_velocity(0,1,0)
#veh_control.set_velocity(1,0,0)
#veh_control.set_velocity(0,0,-1)
#veh_control.set_velocity(0,0,1)
time.sleep(0.5)
print veh_control.get_location().alt
start.changemode("GUIDED")
#start.straight_raise()
#start.straight_descent()
#start.changemode("GUIDED")
