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()
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()
# 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)
#print board.getData(MultiWii.MOTOR)
finally:
connection.close()
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]
q.w = temp[3]
pub_pose.publish(q)
rate.sleep()
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()
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:
val = 0
try:
raw = raw_input('Enter a throttle value: ')
val = int(raw)
from pyMultiwii import MultiWii
board = MultiWii("/dev/ttyUSB0")
board.sendCMD(0, MultiWii.IDENT, [])
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"])
s.send_json(drone_state)
