__, __, tb1_gps, __, __, memory_gps, cpu4_gps, __, motor_gps = \
update_cells(heading=drone_heading, velocity=velocity_gps, \
tb1=tb1_gps, memory=memory_gps, cx=cx_gps)
if (frame_num) % 5 == 0:
heading = motor_gps * 100.0 / scale
if heading > 2:
heading = heading * 8.0 + 1
heading = np.min([np.max([-15, heading]), 15])
print heading
#navigation_heading += heading
if np.abs(heading) > 1.0:
print "rotating"
condition_yaw(drone, heading, relative=True)
if (frame_num + 1) % 5 == 0:
send_ned_velocity(drone, 2 * np.cos(drone_heading),
2 * np.sin(drone_heading), 0, 1)
# write the frame for later recheck
if RECORDING == 'true':
out.write(next)
# logging
logging.info('sl:{} sr:{} heading:{} velocity:{} position:{}'.format(
sl, sr, drone.heading, drone.velocity,
drone.location.global_relative_frame))
angle_optical, distance_optical = cx_optical.decode_cpu4(cpu4_optical)
angle_gps, distance_gps = cx_gps.decode_cpu4(cpu4_gps)
logging.info('Angle_optical:{} Distance_optical:{} Angle_gps:{} Distance_gps:{} \
elapsed_time:{}' .format((angle_optical/np.pi)*180.0, distance_optical, \
(angle_gps/np.pi)*180.0, distance_gps, elapsed_time))
# Load commands
cmds = vehicle.commands
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)