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)
