python_project/flight_user.py at master · muditjps/python_project · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
import serial,time,struct


class MultiWii:
      ATTITUDE = 108
      def __init__(self,serPort):

          self.attitude = {'angx':0,'angy':0,'heading':0}
          self.ser = serial.Serial()
          self.ser.port = serPort
          self.ser.baudrate = 115200
          self.ser.bytesize = serial.EIGHTBITS
          self.ser.parity = serial.PARITY_NONE
          self.ser.stopbits = serial.STOPBITS_ONE
          self.ser.timeout = 0
          self.ser.xonxoff = False
          self.ser.rtscts = False
          self.ser.dsrdtr = False
          self.ser.writeTimeout = 2
          self.PRINT = 1
		  ## wakeup timer to 2 sec ##
          wakeup = 2
          ## wrapping in try catch block ##
          try:
            self.ser.open()
            if self.PRINT:
                print "Waking up board on "+self.ser.port+"..."
            for i in range(1,wakeup):
                if self.PRINT:
                    print wakeup-i
                    time.sleep(1)
                else:
                    time.sleep(1)
          except Exception, error:
            print "\n\nError opening "+self.ser.port+" port.\n"+str(error)+"\n\n"


      """Function for sending a command to the board"""

      def sendCMD(self, data_length, code, data):
        checksum = 0
        total_data = ['$', 'M', '<', data_length, code] + data
        for i in struct.pack('<2B%dH' % len(data), *total_data[3:len(total_data)]):
            checksum = checksum ^ ord(i)
        total_data.append(checksum)
        try:
            b = None
            b = self.ser.write(struct.pack('<3c2B%dHB' % len(data), *total_data))
        except Exception, error:
            #print "\n\nError in sendCMD."
            #print "("+str(error)+")\n\n"
            pass


      """Function to receive a data packet from the board"""

      def getData(self, cmd):
        try:
            start = time.time()
            self.sendCMD(0,cmd,[])
            while True:
                header = self.ser.read()
                if header == '$':
                    header = header+self.ser.read(2)
                    break
            datalength = struct.unpack('<b', self.ser.read())[0]
            code = struct.unpack('<b', self.ser.read())
            data = self.ser.read(datalength)
            temp = struct.unpack('<'+'h'*(datalength/2),data)
            self.ser.flushInput()
            self.ser.flushOutput()
            elapsed = time.time() - start
            if cmd == MultiWii.ATTITUDE:
                self.attitude['angx']=float(temp[0]/10.0)
                self.attitude['angy']=float(temp[1]/10.0)
                self.attitude['heading']=float(temp[2])
                return self.attitude

        except Exception, error:
            #print error
            pass


serialPort = "/dev/ttyUSB0"
board = MultiWii(serialPort)
while True:
		print board.getData(MultiWii.ATTITUDE)