def inside_landing_area(self): vehPos = PositionVector.get_from_location(veh_control.get_location()) landPos = PositionVector.get_from_location(veh_control.get_landing()) ''' vehPos = PositionVector.get_from_location(Location(0,0,10)) landPos = PositionVector.get_from_location(Location(0,0,0)) ''' if(PositionVector.get_distance_xy(vehPos,landPos) < self.landing_area_radius): #below area if(vehPos.z < self.landing_area_min_alt): return -1 #in area else: return 1 #outside area else: return 0
def inside_landing_area(self): vehPos = PositionVector.get_from_location(self.vehicle.location.global_relative_frame) landPos = PositionVector.get_from_location(PositionVector.get_home_location()) ''' vehPos = PositionVector.get_from_location(Location(0,0,10)) landPos = PositionVector.get_from_location(Location(0,0,0)) ''' if(PositionVector.get_distance_xy(vehPos,landPos) < self.landing_area_radius): #below area if(vehPos.z < self.landing_area_min_alt): print "VehPos.Z: %f " % (vehPos.z) return 1 #in area else: return 1 #outside area else: return 0
def inside_landing_area(self): vehPos = PositionVector.get_from_location(veh_control.get_location()) landPos = PositionVector.get_from_location(veh_control.get_landing()) ''' vehPos = PositionVector.get_from_location(Location(0,0,10)) landPos = PositionVector.get_from_location(Location(0,0,0)) ''' if (PositionVector.get_distance_xy(vehPos, landPos) < self.landing_area_radius): #below area if (vehPos.z < self.landing_area_min_alt): return -1 #in area else: return 1 #outside area else: return 0
logging.info("home position is %s", home_position) PositionVector.set_home_location( LocationGlobal(home_position.lat, home_position.lon, home_position.alt)) arm_and_takeoff(vehicle, 5) vehicle.simple_goto(pos1.get_location()) #set_yaw(vehicle,0) #set yaw while True: pos = PositionVector.get_from_location(vehicle.location.global_frame) logging.debug("vehicle_pos in N-S direction from home is %f" % pos.x) logging.debug("vehicle_pos in W-E direction from home is %f" % pos.y) print "distance from vehicle to pos1 is %s" % PositionVector.get_distance_xy( PositionVector.get_from_location(vehicle.location.global_frame), pos1) logging.debug( "distance from vehicle to pos1 is %s" % PositionVector.get_distance_xy( PositionVector.get_from_location(vehicle.location.global_frame), pos1)) time.sleep(0.5) if PositionVector.get_distance_xy( PositionVector.get_from_location(vehicle.location.global_frame), pos1) <= dist_tol: print vehicle.location.global_relative_frame print("reached........hurrrayyyyyyy") logging.debug("vehicle reached position1") break #set_yaw(vehicle,45) vehicle.simple_goto(pos2.get_location())
PositionVector.set_home_location(vehicle.home_location) #Read waypoints from mission file waypoints = mission_to_locations(read_mission("../thunderhill.mission")) #navigate waypoints in manual mode DIST_THRESH = 4 #distance to waypoint before moving onto next waypoint P_TURN = 1 #p term of navigation controller SPEED = 100 #throttle level wp_cnt = 1 for wp in waypoints: #convert global lat/lon local meters(position vectors) wp_posvec = PositionVector.get_from_location(wp) veh_posvec = PositionVector.get_from_location(vehicle.location.global_relative_frame) dist = PositionVector.get_distance_xy(veh_posvec,wp_posvec) while dist > DIST_THRESH: #calculate target_heading and heading error target_heading = math.degrees(PositionVector.get_bearing(veh_posvec,wp_posvec)) error = target_heading - vehicle.heading #remap error from 0 to 360 -> -180 to 180 if error > 180: error = error - 360 if error < -180: error = 360 + error print "going to wp {}: {} meters, {} error".format(wp_cnt,dist,error) #calculate RC overrides(P controller)
#Read waypoints from mission file waypoints = mission_to_locations(read_mission("thunderhill.mission")) #navigate waypoints in manual mode DIST_THRESH = 4 #distance to waypoint before moving onto next waypoint P_TURN = 1 #p term of navigation controller SPEED = 100 #throttle level wp_cnt = 1 for wp in waypoints: #convert global lat/lon local meters(position vectors) wp_posvec = PositionVector.get_from_location(wp) veh_posvec = PositionVector.get_from_location( vehicle.location.global_relative_frame) dist = PositionVector.get_distance_xy(veh_posvec, wp_posvec) while dist > DIST_THRESH: #calculate target_heading and heading error target_heading = math.degrees( PositionVector.get_bearing(veh_posvec, wp_posvec)) error = target_heading - vehicle.heading #remap error from 0 to 360 -> -180 to 180 if error > 180: error = error - 360 if error < -180: error = 360 + error print "going to wp {}: {} meters, {} error".format(wp_cnt, dist, error) #calculate RC overrides(P controller)