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 __init__(self):
self.rcData = RCData()
self.cam = CameraData()
self.cmdManager = CommandManager()
self.actionManager = ActionManager()
self.heightController = heightControl(self.rcData)
#self.recog = Recognition(self.cam)
self.symbolList = []
self.currentCommand = None
self.board = MultiWii('/dev/ttyUSB0')
#self.loadActions()
#self.loadCommands()
#self.loadSymbols()
self.Load.loadAction()
self.Load.loadCommands()
self.Load.loadSymbols()
time.sleep(1)
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
def __init__(self):
self.rcData = RCData()
self.cam = CameraData()
self.cmdManager = CommandManager()
self.actionManager = ActionManager()
self.heightController = heightControl(self.rcData)
#self.recog = Recognition(self.cam)
self.symbolList = []
self.currentCommand = None
self.board = MultiWii('/dev/ttyUSB0')
#self.loadActions()
#self.loadCommands()
#self.loadSymbols()
self.Load.loadAction()
self.Load.loadCommands()
self.Load.loadSymbols()
time.sleep(1)
def __init__(self, attitude_reader_port, anchor_configs):
self.locations = []
for anchor_config in anchor_configs:
self.anchors.append(
SerialDistanceReader(anchor_config["serial_port"],
anchor_config["bias"]))
self.locations.append([
float(anchor_config["x"]),
float(anchor_config["y"]),
float(anchor_config["z"])
])
print("Initializing anchor " + anchor_config["serial_port"] +
" with locations: " + str(self.locations[-1]))
thread = Thread(target=bg_update_distances,
args=(self.anchors, self.thread_lock))
thread.daemon = True
thread.start()
time.sleep(5)
self.trilateration = Trilateration(self.locations, 100)
self.attitude_reader = MultiWii(attitude_reader_port)
print("Initialized multiwii on " + attitude_reader_port)
import zmq
from pyMultiwii import MultiWii
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:
#!/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)
from pyMultiwii import MultiWii
board = MultiWii("/dev/ttyUSB0")
board.sendCMD(0, MultiWii.IDENT, [])
class Controller(object):
def __init__(self):
self.rcData = RCData()
self.cam = CameraData()
self.cmdManager = CommandManager()
self.actionManager = ActionManager()
self.heightController = heightControl(self.rcData)
#self.recog = Recognition(self.cam)
self.symbolList = []
self.currentCommand = None
self.board = MultiWii('/dev/ttyUSB0')
#self.loadActions()
#self.loadCommands()
#self.loadSymbols()
self.Load.loadAction()
self.Load.loadCommands()
self.Load.loadSymbols()
time.sleep(1)
def start(self): #starten van de threading en processen
self.commandThread = threading.Thread(target=self.commandHandler)
self.symbolThread = threading.Thread(target=self.compareSymbols)
self.symbolThread.start()
self.commandThread.start()
self.distance.start()
self.cam.start()
while True:
self.board.sendCMD(8, MultiWii.SEND_RAW_RC, self.rcData.toArray())
time.sleep(0.1)
def compareSymbols(self): #vergelijken van images
while self.recog.processedImage is None:
pass
oldTimestamp = None
while True:
if oldTimestamp != self.recog.timestamp:
oldTimestamp = self.recog.timestamp
diffs = [
self.recog.compareImage(self.recog.processedImage,
symbol.image)
for symbol in self.symbolList
]
filteredDiffs = [diff for diff in diffs if diff is not None]
index = diffs.index(min(filteredDiffs))
detectedSymbol = self.symbolList[index]
self.currentCommand = detectedSymbol.command
def commandHandler(self): #afhandeling van commando's.
while self.currentCommand is None:
pass
previousCommand = None
commandThread = None
while True:
if self.currentCommand != previousCommand:
previousCommand = self.currentCommand
if commandThread is not None:
self.cmdManager.stopCommand()
while commandThread.isAlive():
pass
commandThread = threading.Thread(
target=self.cmdManager.execute,
args=(self.currentCommand, ))
commandThread.start()
print 'Battery :'+BATTERY
print 'can :'+CAN
print 'xbee :'+XBEE
print 'Flight :'+FLIGHT +'\n'
# ----------Open Xbee Serial Port----------------------
ser2 = serial.Serial(XBEE, 115200, timeout=2, xonxoff=True, rtscts=True, dsrdtr=False ) #Tried with and without the last 3 parameters, and also at 1Mbps, same happens.
ser2.flushInput()
ser2.flushOutput()
# ------------Open Flight Board Serial Port-------------
path_ = '/dev/ttyUSB1'
# board = MultiWii(path_)
try:
board = MultiWii(FLIGHT)
t_flight = Thread(target=Accelerometer, args=(board,))
t_flight_G = Thread(target=GPS, args=(board,))
t_flight.start()
t_flight_G.start()
except Exception,e:
print 'ERROR : '+str(e)
print "-----------------------------------------------"
if t_flight.isAlive():
print "Flight started state : Alive"
print "-----------------------------------------------"
# ------------Start Main Threads-------------------------
try:
#!/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
from sys import stdout
import time
if __name__ == "__main__":
#board = MultiWii("/dev/tty.usbserial-AM016WP4")
board = MultiWii("/dev/ttyUSB0")
try:
board.holdStick([1500]*2+[1000, 2000], 0.5)
print "Board is armed now!"
print "In 4 seconds it will disarm..."
board.holdStick([1500]*2+[2000, 2000]+[1500]*4, 3.0) board.holdStick([1500]*2+[1000, 1000], 0.5)
print "Disarmed."
except Exception,error:
print "Error on Main: "+str(error)
class Controller(object):
def __init__(self):
self.rcData = RCData()
self.cam = CameraData()
self.cmdManager = CommandManager()
self.actionManager = ActionManager()
self.heightController = heightControl(self.rcData)
#self.recog = Recognition(self.cam)
self.symbolList = []
self.currentCommand = None
self.board = MultiWii('/dev/ttyUSB0')
#self.loadActions()
#self.loadCommands()
#self.loadSymbols()
self.Load.loadAction()
self.Load.loadCommands()
self.Load.loadSymbols()
time.sleep(1)
def start(self): #starten van de threading en processen
self.commandThread = threading.Thread(target = self.commandHandler)
self.symbolThread = threading.Thread(target = self.compareSymbols)
self.symbolThread.start()
self.commandThread.start()
self.distance.start()
self.cam.start()
while True:
self.board.sendCMD(8, MultiWii.SEND_RAW_RC, self.rcData.toArray())
time.sleep(0.1)
def compareSymbols(self): #vergelijken van images
while self.recog.processedImage is None:
pass
oldTimestamp = None
while True:
if oldTimestamp != self.recog.timestamp:
oldTimestamp = self.recog.timestamp
diffs = [self.recog.compareImage(self.recog.processedImage, symbol.image) for symbol in self.symbolList]
filteredDiffs = [diff for diff in diffs if diff is not None]
index = diffs.index(min(filteredDiffs))
detectedSymbol = self.symbolList[index]
self.currentCommand = detectedSymbol.command
def commandHandler(self): #afhandeling van commando's.
while self.currentCommand is None: pass
previousCommand = None
commandThread = None
while True:
if self.currentCommand != previousCommand:
previousCommand = self.currentCommand
if commandThread is not None:
self.cmdManager.stopCommand()
while commandThread.isAlive():
pass
commandThread = threading.Thread(target = self.cmdManager.execute, args = (self.currentCommand,))
commandThread.start()
#!/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/ttyUSB0")
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)
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/tty.usbserial-A801WZA1"
#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)
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')
__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 "Armed"
time.sleep(2)
board.throttle(1250)
print "Throttle 1250"
time.sleep(2)
board.disarm()
print "Disarmed"
time.sleep(2)
except Exception,error:
print "Error on Main: "+str(error)
from pyMultiwii import MultiWii
import time
import sys
"""
In config.h of the multiwii source code, there is a parameter called
ESC_CALIB_CANNOT_FLY. Enabling that will pass the raw throttle values
through to the ESCs. This is useful for calibrating the ESCs all at once.
This script will set the PWM range on the ESCs and allow you to test motor
speeds after calibrating.
"""
THROTTLE_MIN = 1000
THROTTLE_MAX = 2000
board = MultiWii("/dev/ttyACM0")
print 'Setting throttle to', THROTTLE_MAX
start = time.time()
board.sendCMD(8, MultiWii.SET_RAW_RC, [1500, 1500, 1500, THROTTLE_MAX])
raw_input('Press enter to advance.')
print time.time() - start, 'seconds elapsed.'
print 'Setting throttle to', THROTTLE_MIN
start = time.time()
board.sendCMD(8, MultiWii.SET_RAW_RC, [1500, 1500, 1500, THROTTLE_MIN])
raw_input('Press enter to advance.')
print time.time() - start, 'seconds elapsed.'
while True:
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2016 Altax.net"
__license__ = "GPL"
__version__ = "1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pyMultiwii import MultiWii
import pyqtgraph as pg
from pyqtgraph.Qt import QtCore, QtGui
board = MultiWii("/dev/tty.usbserial-A801WZA1")
win = pg.GraphicsWindow()
win.setWindowTitle('MultiWii IMU plotting')
p1 = win.addPlot()
win.nextRow()
p2 = win.addPlot()
data1 = [0] * 300
data2 = [0] * 300
data3 = [0] * 300
data4 = [0] * 300
data5 = [0] * 300
data6 = [0] * 300
curve1 = p1.plot(data1, name="ax", pen=(255,0,0))
curve2 = p1.plot(data2, name="ay", pen=(0,255,0))
curve3 = p1.plot(data3, name="az", pen=(0,0,255))
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2016 Altax.net"
__license__ = "GPL"
__version__ = "1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pyMultiwii import MultiWii
import pyqtgraph as pg
from pyqtgraph.Qt import QtCore, QtGui
#board = MultiWii("/dev/tty.usbserial-A801WZA1")
board = MultiWii("/dev/tty.SLAB_USBtoUART")
win = pg.GraphicsWindow()
win.setWindowTitle('MultiWii IMU plotting')
p1 = win.addPlot()
win.nextRow()
p2 = win.addPlot()
data1 = [0] * 300
data2 = [0] * 300
data3 = [0] * 300
data4 = [0] * 300
data5 = [0] * 300
data6 = [0] * 300
curve1 = p1.plot(data1, name="ax", pen=(255, 0, 0))
curve2 = p1.plot(data2, name="ay", pen=(0, 255, 0))
curve3 = p1.plot(data3, name="az", pen=(0, 0, 255))
class Main(object): if __name__ == '__main__': flight_obj = FLIGHTBOARD() can_obj = can() bat_obj = bat() XBEE = "/dev/ttyUSB0" BATTERY = "/dev/ttyUSB0" CAN = "/dev/ttyUSB0" FLIGHT = "/dev/ttyUSB0" path_can = "/dev/ttyUSB0" try: ########################## Configure USB ports ################################# list1 = serial.tools.list_ports.comports() # battery_cable = "USB VID:PID=0403:6001 SNR=A50285BI" # Xbee1 = "USB VID:PID=0403:6001 SNR=AH03I7PP" Flight = "USB VID:PID=0403:6001 SNR=AM016X60" Xbee = "USB VID:PID=0403:6001 SNR=AH03I79P" Device = "Linux 3.13.0-24-generic ehci_hcd EHCI Host Controller 0000:00:1a.0" Device_ = 'Linux 3.13.0-24-generic xhci_hcd xHCI Host Controller 0000:02:00.0' VID = 'USB VID:PID=1a86:7523' #print(list(serial.tools.list_ports.comports()))s for a in range(0,len(list1) ): if((Device == list1[a][1]) & (VID == list1[a][2]) ): BATTERY = list1[a][0] if( (Xbee == list1[a][2]) & (Device == list1[a][1]) ): XBEE = list1[a][0] if(Flight == list1[a][2]): FLIGHT = list1[a][0] if(Device_ == list1[a][1]): CAN = list1[a][0] print "\n----------Com Port Configuration--------------" print 'Battery :'+BATTERY print 'can :'+CAN print 'xbee :'+XBEE print 'Flight :'+FLIGHT +'\n' # ----------Open Xbee Serial Port---------------------- ser2 = serial.Serial(XBEE, 115200, timeout=2, xonxoff=True, rtscts=True, dsrdtr=False ) #Tried with and without the last 3 parameters, and also at 1Mbps, same happens. ser2.flushInput() ser2.flushOutput() # ------------Open Flight Board Serial Port------------- try: flight_obj.board = MultiWii(FLIGHT) t_flight = Thread(target=flight_obj.Accelerometer, args=()) t_flight_G = Thread(target=flight_obj.GPS, args=()) t_flight.start() t_flight_G.start() can.path_can = CAN t_can = Thread(target=can_obj.read_can, args=()) t_can.start() bat.path_can = BATTERY # t_bat = Thread(target=bat_obj.read_battery, args=()) # t_bat.start() except Exception,e: print 'ERROR : '+str(e) print 'Pass' # ----------------Run and print-------------------- count=0 past_load='' while True: count = count + 1 load = data_H.q.get() x = len(load) # print "length : "+str(x)+" Size : "+str(sys.getsizeof(q.get()))+" bytes Pht No : "+str(count)+"\n"+ q.get() # print data_H.q.get() send = "####,"+str(x)+","+str(count)+","+load+"!!" print send if load != past_load: ser2.write(send) past_load = load # print str(data_H.q.get()) except Exception,e: print 'ERROR : '+str(e)
from pyMultiwii import MultiWii
import time
if __name__ == "__main__":
board = MultiWii("/dev/ttyUSB0")
try:
data = data - [100, 100, 100, 100]
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)
"""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
if __name__ == "__main__":
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/tty.usbserial-A801WZA1")
try:
while True:
#example of 8 RC channels to be send
data = [1500,1550,1600,1560,1000,1040,1000,1000]
# Old function
#board.sendCMD(16,MultiWii.SET_RAW_RC,data)
#New function that will receive attitude after setting the rc commands
board.sendCMDreceiveATT(16,MultiWii.SET_RAW_RC,data)
print board.attitude
except Exception,error:
print "Error on Main: "+str(error)
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2014 Aldux.net"
__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()
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
import socket
import sys
from pyMultiwii import MultiWii
# Some Gloabals
host = 'localhost'
if __name__ == '__main__':
# Initiate the MWC
board = MultiWii("/dev/ttyUSB0")
# Create a TCP/IP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
# Bind and listen for connections
server_address = (host, 10001)
print >>sys.stderr, 'starting up on %s port %s' % server_address
sock.bind(server_address)
sock.listen(1)
print >>sys.stderr, 'waiting for a connection'
connection, client_address = sock.accept()
print >>sys.stderr, 'client connected:', client_address
data = 'init'
try:
while data!="":
data = connection.recv(19)
print >>sys.stderr, 'received "%s"' % data
pwmValues = data.split()
board.sendCMD(8,MultiWii.SET_RAW_RC,pwmValues)
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Quaternion,Twist
import tf
from pyMultiwii import MultiWii
rospy.init_node('drone')
rate = rospy.Rate(10)
q = Quaternion()
cmd = Twist()
channels = [1000]*8
drone = MultiWii("/dev/ttyUSB0")
mode = 'att'
att = None
vbat = None
def send_command(msg):
global channels
channels[0] = 1500+500*msg.linear.y
channels[1] = 1500+500*msg.linear.x
channels[2] = 1000+1000*msg.linear.z
channels[3] = 1500+500*msg.angular.z
channels[4] = 1000
channels[5] = 1000
channels[6] = 1000
channels[7] = 1000
print channels
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2016 Altax.net"
__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)
class App():
def __init__(self):
self.stdin_path = '/dev/null'
self.stdout_path = '/dev/tty'
self.stderr_path = '/dev/tty'
self.pidfile_path = '/tmp/foo.pid'
self.pidfile_timeout = 5
def run(self):
main()
if __name__ == '__main__':
flight_obj = FLIGHTBOARD()
can_obj = can()
bat_obj = bat()
XBEE = "/dev/ttyUSB0"
BATTERY = "/dev/ttyUSB0"
CAN = "/dev/ttyUSB0"
FLIGHT = "/dev/ttyUSB0"
path_can = "/dev/ttyUSB0"
try:
########################## Configure USB ports #################################
list1 = serial.tools.list_ports.comports()
# battery_cable = "USB VID:PID=0403:6001 SNR=A50285BI"
# Xbee1 = "USB VID:PID=0403:6001 SNR=AH03I7PP"
Flight = "USB VID:PID=0403:6001 SNR=AM016X60"
Xbee = "USB VID:PID=0403:6001 SNR=AH03I79P"
Device = "Linux 3.13.0-24-generic ehci_hcd EHCI Host Controller 0000:00:1a.0"
Device_ = 'Linux 3.13.0-24-generic xhci_hcd xHCI Host Controller 0000:02:00.0'
VID = 'USB VID:PID=1a86:7523'
#print(list(serial.tools.list_ports.comports()))s
for a in range(0,len(list1) ):
if((Device == list1[a][1]) & (VID == list1[a][2]) ):
BATTERY = list1[a][0]
if( (Xbee == list1[a][2]) & (Device == list1[a][1]) ):
XBEE = list1[a][0]
if(Flight == list1[a][2]):
FLIGHT = list1[a][0]
if(Device_ == list1[a][1]):
CAN = list1[a][0]
print "\n----------Com Port Configuration--------------"
print 'Battery :'+BATTERY
print 'can :'+CAN
print 'xbee :'+XBEE
print 'Flight :'+FLIGHT +'\n'
# ----------Open Xbee Serial Port----------------------
ser2 = serial.Serial(XBEE, 115200, timeout=2, xonxoff=True, rtscts=True, dsrdtr=False ) #Tried with and without the last 3 parameters, and also at 1Mbps, same happens.
ser2.flushInput()
ser2.flushOutput()
# ------------Open Flight Board Serial Port-------------
try:
flight_obj.board = MultiWii(FLIGHT)
t_flight = Thread(target=flight_obj.Accelerometer, args=())
t_flight_G = Thread(target=flight_obj.GPS, args=())
t_flight.start()
t_flight_G.start()
except Exception,e:
print 'ERROR t_flight : '+str(e)
try:
can.path_can = CAN
t_can = Thread(target=can_obj.read_can, args=())
t_can.start()
except Exception,e:
print 'ERROR t_can : '+str(e)
# ser_batt.flushInput()
# ser_batt.flushOutput()
# ser_can = serial.Serial('/dev/ttyUSB0', 115200, timeout=2, xonxoff=False, rtscts=False, dsrdtr=False) #Tried with and without the last 3 parameters, and also at 1Mbps, same happens.
# ser_can.flushInput()
# ser_can.flushOutput()
ser2 = serial.Serial(
XBEE, 115200, timeout=2, xonxoff=True, rtscts=True, dsrdtr=False
) #Tried with and without the last 3 parameters, and also at 1Mbps, same happens.
ser2.flushInput()
ser2.flushOutput()
path_ = '/dev/ttyUSB1'
# board = MultiWii(path_)
board = MultiWii(FLIGHT)
t_flight = Thread(target=Accelerometer, args=(board, ))
t_flight.start()
print "-----------------------------------------------"
if t_flight.isAlive():
print "Flight started state : Alive"
print "-----------------------------------------------"
#xbee = XBee.XBee("/dev/ttyUSB0")
################################################
IF = 1 # Debuging purposes
if IF:
print("VEGA logging data ...")
print "-----------------------------------------------"
print "Xbee : " + XBEE
print "Flight : " + FLIGHT
def plot(self, ax, ay, az, gx, gy, gz): 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)
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2017 Altax.net"
__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
"""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
if __name__ == "__main__":
#board = MultiWii("/dev/ttyUSB0")
board = MultiWii("/dev/tty.usbserial-A801WZA1")
try:
while True:
#example of 8 RC channels to be send
data = [1500, 1550, 1600, 1560, 1000, 1040, 1000, 1000]
# Old function
#board.sendCMD(16,MultiWii.SET_RAW_RC,data)
#New function that will receive attitude after setting the rc commands
board.sendCMDreceiveATT(16, MultiWii.SET_RAW_RC, data)
print board.attitude
except Exception, error:
print "Error on Main: " + str(error)
def run(self):
global RUNNING
values = {
'roll': INITIAL_ROLL,
'pitch': INITIAL_PITCH,
'yaw': INITIAL_YAW,
'throttle': INITIAL_THROTTLE
}
board = MultiWii(SERIAL, PRINT=False)
last_command = time.time()
armed = False
try:
while RUNNING:
command = None
try:
command = QUEUE.get_nowait()
QUEUE.task_done() # we don't retry commands
except Empty:
if (time.time() - last_command) > 2:
#fail safe - if no commands stop the drone
board.disarm()
armed = False
continue
if armed:
data = [values['roll'], values['pitch'], values['yaw'], values['throttle']]
board.sendCMD(8,MultiWii.SET_RAW_RC,data)
time.sleep(0.05)
continue
last_command = time.time()
if not command or not 'action' in command:
continue
print "got command: %s" % command
if command['action'] == 'arm':
board.arm()
armed = True
elif command['action'] == 'disarm':
board.disarm()
armed = False
elif command['action'] == 'update':
try:
values.update(command['data'])
except:
logging.exception('error update values')
else:
logging.debug('invalid command %s' % command)
except:
logging.exception("Error")
board.disarm()
#!/usr/bin/env python3
"""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)
__author__ = "Aldo Vargas"
__copyright__ = "Copyright 2014 Aldux.net"
__license__ = "GPL"
__version__ = "1"
__maintainer__ = "Aldo Vargas"
__email__ = "*****@*****.**"
__status__ = "Development"
from pyMultiwii import MultiWii
import random
from sys import stdout
if __name__ == "__main__":
board = MultiWii("/dev/ttyUSB1")
#board = MultiWii("/dev/tty.usbserial-A801WZA1")
try:
board.arm()
while True:
#example of 8 RC channels to be send
#data = [random.randrange(100, 1600) for i in range(8)]
data = [1100] * 4
# Old function
#board.sendCMD(16,MultiWii.SET_RAW_RC,data)
#New function that will receive attitude after setting the rc commands
board.sendCMDreceiveATT(16,MultiWii.SET_RAW_RC,data)
board.getData(MultiWii.ATTITUDE)
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']))
