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"])