print "EXIT FAILURE"
state += 1
PORT = 6060
BUF_SIZE = 4096
TIMEOUT = 60 # timeout in seconds
state = 0
cs = None
states = {
0: connect,
1: arm,
2: takeoff,
3: goto_init,
4: formation,
5: exit_success,
6: exit_failure
}
# DroneAPI APIConnection
api = local_connect()
cc = CopterControl(api)
# propogate FSM
while state < len(states):
states[state]()
cs.close()
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)
"""
"""
from __future__ import print_function
import time, sys
from copter_control import CopterControl
# DroneAPI APIConnection
api = local_connect()
cc = CopterControl(api)
f = open('attitude.log', 'w')
start = time.time()
dur = 120.0
prev_yaw = -1.0
while True:
now = time.time()
diff = now-start
if diff >= dur:
break
attitude = cc.get_attitude()
yaw = attitude.yaw
if prev_yaw != yaw:
print(str(diff) + ": " + str(yaw))
print(str(diff) + ": " + str(yaw), file=f)
prev_yaw = yaw
f.close()
