"""test-send.py: Test script to send RC commands to a MultiWii Board."""
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2016 Altax.net"
__license__ = "GPL"
__version__ = "1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pymultiwii import MultiWii
import time
if __name__ == "__main__":
#board = MultiWii("/dev/tty.usbserial-AM016WP4")
board = MultiWii("/dev/tty.SLAB_USBtoUART")
try:
board.arm()
print ("Board is armed now!")
print ("In 3 seconds it will disarm...")
time.sleep(3)
board.disarm()
print ("Disarmed.")
time.sleep(3)
except Exception,error:
print ("Error on Main: "+str(error))
data = data.split(':')
if data == ['']:
continue
#print str(data)
orden_s = [int(data[0]), int(data[1]), int(data[2]), int(data[3])]
print orden_s
if orden_s[3] == -2:
print 'disarm command received'
break
if (orden_s != orden):
orden = orden_s
#imu=uav.sendCMDreceiveATT(8,MultiWii.SET_RAW_RC,orden)
uav.sendCMD(8, MultiWii.SET_RAW_RC, orden)
##message =str(uav.getData(MultiWii.ATTITUDE))
##mon_socket.sendto(message,address)
#if imu!=None:
#imu_socket.sendto(imu,dir)
##print orden
if not data: break
cmd_socket.close()
uav.disarm()
except KeyboardInterrupt:
cmd_socket.close()
uav.disarm()
except Exception as e:
print str(e)
cmd_socket.close()
uav.disarm()
idle = False
#Loop until the user clicks the close button.
done = False
time.sleep(1)
roll = 1500
pitch = 1500
rotate = 1500
throttle = 1000
roll_offset = 0
pitch_offset = 0
rotate_offset = 0
butterfly.disarm()
console_messege_timer_start = 0
auto_land = False
auto_pilot = False
control_signal_log = {}
captured_image_log = {}
predicted_probability_log = {}
top_label_log = {}
log_timer_start = time.time()
prestate = 'None'
prestate2 = 'None'
cap = cv2.VideoCapture(0)
board = MultiWii("COM4")
#cap = cv2.VideoCapture(0)
try:
print('Press button g to start control')
print('Press button q to quit ')
while True:
#camera show
#ret, frame = cap.read()
#cv2.imshow('frame',frame)
#cv2.imwrite('img{:>05}.png'.format(i), frame)
CNN = -1
prestate = 'None'
if (msvcrt.kbhit()):
key = msvcrt.getch()
if (key.decode() == 'd'):
board.disarm()
print("Disarmed.")
elif (key.decode() == 'q'):
print('Quit...')
break
elif (key.decode() == 'c'):
if (cam == 1):
cam = 0
print('camera number change to 0')
elif (cam == 0):
cam = 1
print('camera number change to 1')
elif (key.decode() == 'g'):
cap = cv2.VideoCapture(cam)
while True:
pitch = 1500
# final = 1000
# else:
# final = int(1200 + roll_out)
data = [1650, 1550, 1500, 1820]
print("go_left")
elif decision > go_right:
data = [1350, 1525, 1500, 1840]
print("go_right")
else:
data = [
int(1500 - pitch_out),
int(1505 + roll_out), 1500, 1800
] #roll pitch yaw throttle
print("fly_up")
try:
drone.sendCMD(8, MultiWii.SET_RAW_RC, data, '4h')
except Exception as error:
print(error)
pass
start = time.time()
except KeyboardInterrupt as error:
drone.disarm()
time.sleep(0.5)
while True:
pass
roll = ROLL + mando[3] pitch = PITCH + mando[2] yaw = YAW + mando[1] trothle = TROTHLE + mando[0] boton_up = mando[5] boton_down = mando[4] print 'Iniciando bucle principal' while Done == False: bat = ina.read() if boton_up == 12 and arm == 0: placa.arm() arm = 1 elif boton_up == 12 and arm ==1: placa.disarm() arm = 0 if arm ==1: data =[roll,pitch,yaw,trothle,1000,1000,1000,1000] placa.sendCMDreceiveATT(16,MultiWii.SET_RAW_RC,data) x = int(placa.attitude['angx']) y = int(placa.attitude['angy']) z = int(placa.attitude['heading']) array = [x,y,z,bat] msg = pickle.dumps(array) sock.sendto(msg, (UDP_HOST,UDP_PORT)) elif arm ==0: placa.getData(MultiWii.ATTITUDE) x = int(placa.attitude['angx']) y = int(placa.attitude['angy']) z = int(placa.attitude['heading'])