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