cmds.download()
cmds.wait_ready()
home = vehicle.home_location
adds_10wayP_mission(vehicle, home, vehicle.heading, 2.5)
# Compute the angle between the cureent position and first waypoint
for cmd in cmds:
if cmd.command == MAV_CMD_NAV_WAYPOINT:
break
first_waypoint = LocationGlobalRelative(cmd.x, cmd.y, cmd.z)
arm_and_takeoff(vehicle, 4)
send_ned_velocity(vehicle, 0, 0, 0, 1)
orientation_to_go = get_angles_degree(home, first_waypoint)
condition_yaw(vehicle, orientation_to_go, 0)
print get_angles_degree(home, first_waypoint)
time.sleep(5)
vehicle.mode = VehicleMode("AUTO")
# monitor mission execution
nextwaypoint = vehicle.commands.next
while nextwaypoint < len(vehicle.commands):
if vehicle.commands.next > nextwaypoint:
display_seq = vehicle.commands.next
print "Moving to waypoint %s" % display_seq
nextwaypoint = vehicle.commands.next
time.sleep(1)
print 'Return to launch'
vehicle.mode = VehicleMode("RTL")
# ------------------stop when the same period of time reached-------------------------------
#-------------------------------------------------------------------------------------------
# rotate to return direction first
while drone.mode.name == "GUIDED":
drone_heading = drone.heading / 180.0 * np.pi
velocity = np.array([0, 0]) / 4.0 # 0 as the drone is turning
__, __, tb1_optical, __, __, memory_optical, cpu4_optical, __, motor_optical = \
update_cells(heading=drone_heading, velocity=velocity, tb1=tb1_optical, \
memory=memory_optical, cx=cx_optical)
heading = motor_optical * 100.0 / scale
heading = np.min([np.max([-10, heading]), 10])
print heading
if np.abs(heading) > 1.0:
print "rotating"
condition_yaw(drone, heading, relative=True)
else:
break
time.sleep(0.5)
start_time = time.time()
while drone.mode.name == "GUIDED":
# Image processing, compute optical flow
frame_num += 1
frame_gray = picam.get_frame()
next = optflow.undistort(frame_gray)
flow = cv2.calcOpticalFlowFarneback(prvs, next, None, 0.5, 3, 15, 3, 5,
1.1, 0)
# speed
elapsed_time = time.time() - start_time
start_time = time.time()
cmds.add(cmd)
# Upload mission
print " Upload new commands to vehicle"
cmds.upload()
time.sleep(2)
cmds = download_mission(vehicle.commands)
cmd = cmds[cmds.next]
# Save the vehicle commands to a list
missionlist = []
first_waypoint = LocationGlobalRelative(cmd.x, cmd.y, cmd.z)
arm_and_takeoff(vehicle, 4)
send_ned_velocity(vehicle, 0, 0, 0, 1)
condition_yaw(vehicle, get_angles_degree(home, first_waypoint), 1)
print get_angles_degree(home, first_waypoint)
time.sleep(5)
vehicle.mode = VehicleMode("AUTO")
# monitor mission execution
nextwaypoint = vehicle.commands.next
while nextwaypoint < len(vehicle.commands):
if vehicle.commands.next > nextwaypoint:
display_seq = vehicle.commands.next
print "Moving to waypoint %s" % display_seq
nextwaypoint = vehicle.commands.next
time.sleep(1)
print 'Return to launch'
vehicle.mode = VehicleMode("RTL")