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)