def find_board():
""" Lists serial port names
:raises EnvironmentError:
On unsupported or unknown platforms
:returns:
A list of the serial ports available on the system
"""
if sys.platform.startswith('win'):
ports = ['COM%s' % (i + 1) for i in range(256)]
elif sys.platform.startswith('linux') or sys.platform.startswith('cygwin'):
# this excludes your current terminal "/dev/tty"
ports = glob.glob('/dev/tty[A-Za-z]*')
elif sys.platform.startswith('darwin'):
ports = glob.glob('/dev/tty.*')
else:
raise EnvironmentError('Unsupported platform')
for port in ports:
board = None
try:
s = serial.Serial(port)
s.close()
board = MultiWii(port)
board.getData(MultiWii.RC)
return board
except (OSError, serial.SerialException):
if board:
board.disconnect()
pass
return None
self.senses += 1 plt.plot([self.sb, self.senses], [self.axbuf, ax], color='r', label='AX') plt.plot([self.sb, self.senses], [self.aybuf, ay], color='g', label='AY') plt.plot([self.sb, self.senses], [self.azbuf, az], color='b', label='AZ') plt.plot([self.sb, self.senses], [self.gxbuf, gx], color='y', label='GX') plt.plot([self.sb, self.senses], [self.gybuf, gy], color='black', label='GY') plt.plot([self.sb, self.senses], [self.gzbuf, gz], color='pink', label='GZ') self.fig.canvas.draw() self.sb, self.axbuf, self.aybuf, self.azbuf, self.gxbuf, self.gybuf, self.gzbuf = self.senses, ax, ay, az, gx, gy, gz if __name__ == "__main__": chart = Chart() serialPort = "/dev/ttyUSB0" #serialPort = "/dev/tty.SLAB_USBtoUART" board = MultiWii(serialPort) try: while True: board.getData(MultiWii.RAW_IMU) #print board.rawIMU t = float(board.rawIMU['timestamp']) ax = board.rawIMU['ax'] ay = board.rawIMU['ay'] az = board.rawIMU['az'] gx = board.rawIMU['gx'] gy = board.rawIMU['gy'] gz = board.rawIMU['gz'] chart.plot(ax, ay, az, gx, gy, gz) except Exception,error: print "Error on Main: "+str(error)
__license__ = "GPL"
__version__ = "1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pyMultiwii import MultiWii
from sys import stdout
if __name__ == "__main__":
board = MultiWii("/dev/ttyUSB0")
# board = MultiWii("/dev/tty.SLAB_USBtoUART")
try:
while True:
board.getData(MultiWii.ATTITUDE)
# print board.attitude #uncomment for regular printing
# Fancy printing (might not work on windows...)
message = "angx = {:+.2f} \t angy = {:+.2f} \t heading = {:+.2f} \t elapsed = {:+.4f} \t".format(
float(board.attitude["angx"]),
float(board.attitude["angy"]),
float(board.attitude["heading"]),
float(board.attitude["elapsed"]),
)
stdout.write("\r%s" % message)
stdout.flush()
# End of fancy printing
except Exception, error:
print "Error on Main: " + str(error)
from pyMultiwii import MultiWii
from sys import stdout
if __name__ == "__main__":
board = MultiWii("/dev/ttyUSB0")
try:
while True:
board.getData(MultiWii.MOTOR)
message = "m1 = {:+.2f} \t m2 = {:+.2f} \t m3 = {:+.2f} \t m4 = {:+.4f} \t".format(float(board.motor['m1']),float(board.motor['m2']),float(board.motor['m3']),float(board.motor['m4'],float(board.attitude['elapsed']))
stdout.write("\r%s" % message )
stdout.flush()
except Exception,error:
print "Error on Main: "+str(error)
channels[3] = 1500+500*msg.angular.z
channels[4] = 1000
channels[5] = 1000
channels[6] = 1000
channels[7] = 1000
print channels
pub_pose = rospy.Publisher('pose',Quaternion,queue_size=1)
sub_cmd = rospy.Subscriber('cmd',Twist,send_command)
while not rospy.is_shutdown():
if mode == 'att':
# get Euler angles
att = drone.getData(MultiWii.ATTITUDE)
mode = 'bat'
elif mode == 'bat':
vbat = drone.getData(MultiWii.ANALOG)
mode = 'cmd'
elif mode == 'cmd':
drone.sendCMD(16,MultiWii.SET_RAW_RC,channels)
mode = 'att'
else:
print 'mode '+mode+' unknown.'
if not att is None:
temp = tf.transformations.quaternion_from_euler(att['angx'],att['angy'],att['heading'])
# publish
q.x = temp[0]
q.y = temp[1]
q.z = temp[2]
class MultiWiiMain:
''' <summary>
<para>Data Array Format</para>
<para>0->Altitude</para>
<para>1->Accelerometer X</para>
<para>2->Accelerometer Y</para>
<para>3->Accelerometer Z</para>
<para>4->Gyroscope X</para>
<para>5->Gyroscope Y</para>
<para>6->Gyroscope Z</para>
<para>7->Magnetometer X</para>
<para>8->Magnetometer Y</para>
<para>9->Magnetometer Z</para>
<para>10->Motor 1</para>
<para>11->Motor 2</para>
<para>12->Motor 3</para>
<para>13->Motor 4</para>
<para>14->Roll</para>
<para>15->Pitch</para>
<para>16->Yaw</para>
<para>17->Throttle</para>
<para>18->AUX1</para>
<para>19->AUX2</para>
<para>20->AUX3</para>
<para>21->AUX4</para>
<para>22->Angle X</para>
<para>23->Angle Y</para>
<para>24->Heading</para>
</summary>'''
def __init__(self):
#INIT MULTIWII
self.WiiProtocol = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0
]
self.USBPort = "COM4"
self.Board = MultiWii(self.USBPort)
self.Board.arm()
print("ARMED")
def MultiWiiWrite(self):
#MULTIWII WRITE | SET MULTIWII VALUES
print("MULTIWII WRITE")
def MultiWiiRead(self):
#MULTIWII READ | READ MULTIWII DATA
print("M_R")
#MultiWii READ
self.Board.getData(MultiWii.ALTITUDE)
#print(self.Board.attitude['angx'])
#INIT ARRAY DATA
self.WiiProtocol[0] = 100
'''ALTITUDE'''
self.WiiProtocol[1] = 0
'''Accelerometer X'''
self.WiiProtocol[2] = 0
'''Accelerometer Y'''
self.WiiProtocol[3] = 0
'''Accelerometer Z'''
self.WiiProtocol[4] = 0
'''Gyroscope X'''
self.WiiProtocol[5] = 0
'''Gyroscope Y'''
self.WiiProtocol[6] = 0
'''Gyroscope Z'''
self.WiiProtocol[7] = 100
'''Magnetometer X'''
self.WiiProtocol[8] = 100
'''Magnetometer Y'''
self.WiiProtocol[9] = 5
'''Magnetometer Z'''
self.WiiProtocol[10] = 1500
'''Motor 1'''
self.WiiProtocol[11] = 1500
'''Motor 2'''
self.WiiProtocol[12] = 1500
'''Motor 3'''
self.WiiProtocol[13] = 1500
'''Motor 4'''
self.WiiProtocol[14] = 0
'''Roll'''
self.WiiProtocol[15] = 0
'''Pitch'''
self.WiiProtocol[16] = 0
'''Yaw'''
self.WiiProtocol[17] = 0
'''Throttle'''
self.WiiProtocol[18] = 0
'''AUX1'''
self.WiiProtocol[19] = 0
'''AUX2'''
self.WiiProtocol[20] = 0
'''AUX3'''
self.WiiProtocol[21] = 0
'''AUX4'''
self.WiiProtocol[22] = 0
'''ANGLE X'''
self.WiiProtocol[23] = 0
'''ANGLE Y'''
self.WiiProtocol[24] = 0.2
'''HEADING'''
messageData = ""
for i in range(0, len(self.WiiProtocol)):
if (i == len(self.WiiProtocol) - 1):
messageData += str(self.WiiProtocol[i])
else:
messageData += str(self.WiiProtocol[i]) + ","
return messageData
#!/usr/bin/env python
"""test-send.py: Test script to send RC commands to a MultiWii Board."""
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2014 Aldux.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/ttyUSB0")
try:
print board.getData(MultiWii.IDENT)
except Exception,error:
print "Error on Main: "+str(error)
__license__ = "GPL"
__version__ = "1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pyMultiwii import MultiWii
from sys import stdout
if __name__ == "__main__":
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/tty.SLAB_USBtoUART")
try:
while True:
board.getData(MultiWii.RAW_IMU)
#print board.attitude #uncomment for regular printing
# Fancy printing (might not work on windows...)
message = "ax = {:+.0f} \t ay = {:+.0f} \t az = {:+.0f} gx = {:+.0f} \t gy = {:+.0f} \t gz = {:+.0f} mx = {:+.0f} \t my = {:+.0f} \t mz = {:+.0f} \t elapsed = {:+.4f} \t".format(
float(board.rawIMU['ax']), float(board.rawIMU['ay']),
float(board.rawIMU['az']), float(board.rawIMU['gx']),
float(board.rawIMU['gy']), float(board.rawIMU['gz']),
float(board.rawIMU['mx']), float(board.rawIMU['my']),
float(board.rawIMU['mz']), float(board.attitude['elapsed']))
stdout.write("\r%s" % message)
stdout.flush()
# End of fancy printing
except Exception, error:
print "Error on Main: " + str(error)
# t_bat.join()
# t_can.join()
count = 0
# if t_bat.isAlive():
# print "Alive"
while True:
count = count + 1
print q.get()
# print str(head)
x = len(q.get())
print x
ser2.write("####" + str(x) + "##" + q.get() + "," +
str(count) + "$$")
board.getData(MultiWii.ATTITUDE)
head = board.attitude['heading']
# print str(head)
# print "Main"
time.sleep(1)
if (count % 10 == 0):
# lock.acquire()
# q.queue.clear()
# lock.release()
# print "Cleared"
print str(head)
ser2.write("####" + str(head) + "$$")
except KeyboardInterrupt:
# print files
for f in files:
type_val = drone.get_type(rc_type)
if type_val > -1:
val = drone.get_value(type_val)
return jsonify({'status': 'success', rc_type: val})
else:
return jsonify({'status': 'error', 'type': 'No RC value found'})
@app.route('/drone/rc/<rc_type>/<value>')
def set_rc(rc_type, value):
type_val = drone.get_type(rc_type)
if type_val > -1:
drone.set_value(type_val, value)
return jsonify({'status': 'success'})
else:
return jsonify({'status': 'error', 'type': 'No RC value found'})
@app.route('/drone/abort')
def abort():
drone.set_value(drone.THROTTLE, board.misc['failsafethrottle'])
return jsonify({'status': 'success'})
if __name__ == '__main__':
board = MultiWii("/dev/ttyUSB0")
# Get the constant values
board.getData(MultiWii.MISC)
drone = DroneControl(board)
app.run(host='0.0.0.0')
import time
IP = '0.0.0.0'
port = '8080'
c = zmq.Context()
s = c.socket(zmq.REP)
s.bind("tcp://" + IP + ":" + port)
drone = MultiWii("/dev/ttyUSB0")
drone_state = {}
goon = True
while goon:
reply = s.recv_json()
if reply.has_key("pad"):
att = drone.getData(MultiWii.ATTITUDE)
mot = drone.getData(MultiWii.MOTOR)
vbat = drone.getData(MultiWii.ANALOG)
if not att is None:
drone_state["angx"] = float(att['angx'])
drone_state["angy"] = float(att['angy'])
drone_state["heading"] = float(att['heading'])
if not mot is None:
drone_state["m1"] = float(mot['m1'])
drone_state["m2"] = float(mot['m2'])
drone_state["m3"] = float(mot['m3'])
drone_state["m4"] = float(mot['m4'])
if not vbat is None:
drone_state["vbat"] = vbat
drone.sendCMD(16, MultiWii.SET_RAW_RC, reply["pad"])
