def test():
api = local_connect()
cop_ctrl = CopterControl(api)
print "ARMED = " + str(cop_ctrl.is_armed())
cop_ctrl.arm()
print "MODE = " + str(cop_ctrl.get_mode_name()) # should be stabilize if copter has just been turned on
cop_ctrl.set_mode("GUIDED")
time.sleep(3) # wait for changes to take effect
print "Taking off"
cop_ctrl.takeoff(15)
time.sleep(10)
origin = (40.345763, -74.649955)
print "GOTO"
cop_ctrl.goto(origin, 15)
time.sleep(4)
print "Setting Yaw"
cop_ctrl.set_yaw(250)
time.sleep(10)
print 'GOTO 2'
start = (40.345652, -74.650070)
cop_ctrl.goto(start, 15)
time.sleep(3)
"""
origin = (40.345763, -74.649955)
thirty = (40.345967, -74.650021)
forty = (40.345712, -74.649880)
print "TAKING OFF!"
cop_ctrl.takeoff(15)
time.sleep(15)
print "SENDING VELOCITY COMMAND"
cop_ctrl.set_velocity(1, 0, 0)
time.sleep(10)
cop_ctrl.set_velocity(0, 0, 0)
"""
"""
def test():
api = local_connect()
cop_ctrl = CopterControl(api)
print "ARMED = " + str(cop_ctrl.is_armed())
cop_ctrl.arm()
print "MODE = " + str(cop_ctrl.get_mode_name(
)) # should be stabilize if copter has just been turned on
cop_ctrl.set_mode("GUIDED")
time.sleep(3) # wait for changes to take effect
print "Taking off"
cop_ctrl.takeoff(15)
time.sleep(10)
origin = (40.345763, -74.649955)
print "GOTO"
cop_ctrl.goto(origin, 15)
time.sleep(4)
print "Setting Yaw"
cop_ctrl.set_yaw(250)
time.sleep(10)
print 'GOTO 2'
start = (40.345652, -74.650070)
cop_ctrl.goto(start, 15)
time.sleep(3)
"""
origin = (40.345763, -74.649955)
thirty = (40.345967, -74.650021)
forty = (40.345712, -74.649880)
print "TAKING OFF!"
cop_ctrl.takeoff(15)
time.sleep(15)
print "SENDING VELOCITY COMMAND"
cop_ctrl.set_velocity(1, 0, 0)
time.sleep(10)
cop_ctrl.set_velocity(0, 0, 0)
"""
"""
end = (40.345112, -74.647657) # practice field 25
'''
'''
start = (40.344641, -74.649337) # track mid
end = (40.344448, -74.649908) # track end
'''
path = diag_line(start, end)
# Initialize the copter connection
api = local_connect()
cc = CopterControl(api)
# arm copter
cc.arm()
# change to guided mode
cc.set_mode("GUIDED")
# wait for changes to take effect
time.sleep(3)
# takeoff!
print "Taking off ..."
cc.takeoff(15)
time.sleep(10)
# going to start location
print "going to start loc"
cc.goto((start[0], start[1]), start[2])
