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 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/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)
#!/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)