source: trunk/Puzzlebox/Brainstorms/Helicopter_Control.py @ 326

Last change on this file since 326 was 326, checked in by sc, 9 years ago

local:

  • doc section added with Linux examples

configuration.ini:

  • helicopter demonstration thresholds added

NXT_Control:

  • voltage parsing bug with 0.7.3 fixed

Helicopter_Control:

  • demonstration settings for Discovery Channel
  • Property svn:executable set to *
File size: 22.0 KB
Line 
1#!/usr/bin/env python
2# -*- coding: utf-8 -*-
3#
4# Puzzlebox - Brainstorms - Helicopter Control
5#
6# Copyright Puzzlebox Productions, LLC (2010-2012)
7#
8# This code is released under the GNU Pulic License (GPL) version 2
9# For more information please refer to http://www.gnu.org/copyleft/gpl.html
10
11__changelog__ = """
12Last Update: 2012.03.21
13"""
14
15import sys, time
16import signal
17import serial
18
19##try:
20##      import PySide as PyQt4
21##      from PySide import QtCore
22##except:
23##      print "Using PyQt4 module"
24##      from PyQt4 import QtCore
25##else:
26##      print "Using PySide module"
27
28import Configuration as configuration
29
30if configuration.ENABLE_PYSIDE:
31        try:
32                import PySide as PyQt4
33                from PySide import QtCore
34        except Exception, e:
35                print "ERROR: Exception importing PySide:",
36                print e
37                configuration.ENABLE_PYSIDE = False
38        else:
39                print "INFO: [Brainstorms:Helicopter_Control] Using PySide module"
40
41if not configuration.ENABLE_PYSIDE:
42        print "INFO: [Brainstorms:Helicopter_Control] Using PyQt4 module"
43        from PyQt4 import QtCore
44
45#import Puzzlebox.Synapse.Protocol as protocol
46#from Puzzlebox.Synapse import Protocol
47
48#####################################################################
49# Globals
50#####################################################################
51
52DEBUG = 2
53
54DEFAULT_COMMAND = 'dump_packets'
55
56SERIAL_DEVICE = '/dev/ttyUSB0'
57#SERIAL_DEVICE = '/dev/ttyACM0'
58#DEFAULT_SERIAL_BAUDRATE = 115200 # This is the closest "standard" baud rate the USB-to-Serial device will support
59#DEFAULT_SERIAL_BAUDRATE = 125000 # This is the speed reported by the forum post
60#DEFAULT_SERIAL_BAUDRATE = 128000 # This is the next closest somewhat commonly-found baud rate (though not supported by device)
61DEFAULT_SERIAL_BAUDRATE = 133333 # This is the speed reported by the logic analyzer
62#DEFAULT_SERIAL_BAUDRATE = 230400 # This is the next highest "standard" baud rate the USB-to-Serial device will support
63DEFAULT_MODE = 'read'
64
65PROTOCOL_SYNC_TIME = 5
66PROTOCOL_SYNC_HEAD1 = '\x00'
67PROTOCOL_SYNC_HEAD2 = '\x00'
68PROTOCOL_ADD_SYNC_TO_HEAD = False
69PROTOCOL_ADD_SYNC_TO_TAIL = False
70PACKET_LENGTH = 14
71PAYLOAD_MINIMUM_LENGTH = 12
72PACKET_READ_SIZE = 14
73ECHO_ON = False
74
75#DEVICE_BUFFER_TIMER = 22  # Frame cycle 22ms
76DEVICE_BUFFER_TIMER = 21  # Frame cycle 22ms
77
78# Protocol Specification
79#
80#       000000a605ff0a060dff135414aa
81#       AaAaBbbbCcccDdddEeee______Ff
82#       
83#       Aa - Frame sync bytes
84#       Bbbb - Throttle bytes (4 decimal values per trim step)
85#       Cccc - Aileron bytes (3 or 4 decimal values per trim step)
86#       Dddd - Elevator bytes (3 or 4 decimal values per trim step)
87#       Eeee - Rudder bytes (2 decimal values per trim step)
88#       Ff - End of Frame byte
89#       ____ - Unused/Unknown
90#
91#
92#       Example throttle bytes
93#
94#0354 : +20 highest throttle stick setting and trim [852]
95#
96#00fa:  +20 [250]
97#00f6:  +19
98#00f2:  +18
99#00ee:  +17
100#00ea:  +16
101#00e6:  +15
102#00e2:  +14
103#00de:  +13
104#00da:  +12
105#00d6:  +11
106#00d2:  +10
107#00ce:  +9
108#00ca:  +8
109#00c6:  +7
110#00c2:  +6
111#00be:  +5
112#00ba:  +4
113#00b6:  +3
114#00b2:  +2 [178]
115#00ae:  +1 [174]
116#00aa:  lowest throttle stick setting [170]
117#00a6:  -1 [166]
118#00a2:  -2
119#009e:  -3
120#009a:  -4
121#0096:  -5
122#0092:  -6
123#008e:  -7
124#008a:  -8
125#0086:  -9
126#0082:  -10
127#007e:  -11
128#007a:  -12
129#0076:  -13
130#0072:  -14
131#006e:  -15
132#006a:  -16
133#0066:  -17
134#0062:  -18
135#005e:  -19
136#005a:  -20 [90]
137
138
139
140COMMAND_PACKET = {
141        'default_neutral':      '\x00\x00\x00\xaa\x05\xff\x09\xff\x0d\xff\x13\x54\x14\xaa',  # default neutral setting to use for all commands
142        'default_full_thrust':  '\x00\x00\x03\x54\x05\xff\x09\xff\x0d\xff\x13\x54\x14\xaa',  # new controll set to highest throttle (no changes to trim)
143        #'neutral':             '\x00\x00\x00\xfa\x05\xc5\x09\xde\x0e\x0b\x13\x54\x14\xaa',  # 0.4.5 neutral setting to use for all commands
144        'neutral':              '\x00\x00\x00\xae\x05\xff\x09\xff\x0d\xff\x13\x54\x14\xaa',  # default neutral setting to use for all commands
145        'no_thrust':            '\x00\x00\x00\x5a\x05\xc5\x09\xde\x0e\x0b\x13\x54\x14\xaa',  # lowest trim setting for throttle
146        'minimum_thrust':       '\x00\x00\x00\xca\x05\xc5\x09\xde\x0e\x0b\x13\x54\x14\xaa',  # lowest trim level at which throttle kicks in
147        'minimum_thrust_minus_one': '\x00\x00\x00\xc6\x05\xc5\x09\xde\x0e\x0b\x13\x54\x14\xaa',  # lowest trim level at which throttle kicks in
148        'maximum_thrust':       '\x00\x00\x03\x54\x05\xc5\x09\xde\x0e\x0b\x13\x54\x14\xaa',  # maximum possible throttle and trim
149        'fifty_percent_thrust': '\x00\x00\x01\x7d\x05\xc5\x09\xde\x0e\x0b\x13\x54\x14\xaa', # calculated 50% throttle
150        'test_packet':          '\x00\x00\x03\x54\x06\x15\x09\xca\x0e\x2f\x13\x54\x14\xaa', # test packet from saleae logic screenshot
151        'maximum_forward':      '\x00\x00\x00\x5a\x05\xc5\x0b\x54\x0e\x0b\x13\x54\x14\xaa', # maximum possible elevator and trim
152            #'fly_forward':      '\x00\x00\x01\x7d\x05\xc5\x0a\xde\x0e\x0b\x13\x54\x14\xaa', # 0.4.5 fly_forward settings
153            'fly_forward':      '\x00\x00\x01\x7d\x05\xc5\x0a\xde\x0e\x0b\x13\x54\x14\xaa',
154}
155
156DEFAULT_COMMAND_PACKET = COMMAND_PACKET['neutral']
157
158#####################################################################
159# Classes
160#####################################################################
161
162class puzzlebox_brainstorms_helicopter_control(QtCore.QThread):
163       
164        def __init__(self, \
165                     device_address=SERIAL_DEVICE, \
166                     command=DEFAULT_COMMAND, \
167                     DEBUG=DEBUG, \
168                     parent=None):
169               
170                QtCore.QThread.__init__(self, parent)
171               
172                self.log = None
173                self.DEBUG = DEBUG
174                self.parent = parent
175               
176                self.device_address = device_address
177                self.command = command
178                self.mode = DEFAULT_MODE
179               
180                self.serial_device = None
181                self.protocol = None
182               
183               
184                #self.configureRemote()
185       
186       
187        ##################################################################
188       
189        def configureRemote(self):
190               
191                self.serial_device = \
192                        SerialDevice( \
193                                self.log, \
194                                device_address=self.device_address, \
195                                mode=self.mode, \
196                                DEBUG=self.DEBUG, \
197                                parent=self)
198               
199                self.serial_device.start()
200               
201               
202                self.protocol = \
203                        ProtocolHandler( \
204                                self.log, \
205                                self.serial_device, \
206                                mode=self.mode, \
207                                DEBUG=self.DEBUG, \
208                                parent=self)
209               
210                self.protocol.start()
211       
212       
213        ##################################################################
214       
215        def processPacket(self, packet):
216               
217                if self.DEBUG:
218                        print "data_payload:",
219                        #print packet['data_payload']
220                        print packet['data_payload'].encode("hex")
221                       
222                        #if packet['data_payload'].encode("hex") == '80acdf22cdb08b8d54':
223                                #print True
224                                #import cPickle as pickle
225                                #file = open('packet.data', 'w')
226                                #pickle.dump(packet['data_payload'], file)
227                                #file.close()
228                                #sys.exit(app.exec_())
229               
230                #if (packet != {}):
231                        #self.packet_queue.append(packet)
232                        ###self.sendPacketQueue()
233               
234                if (self.parent != None):
235                        self.parent.processPacket(self.protocol.data_packet)
236       
237       
238        ##################################################################
239       
240        def dump_packets(self):
241               
242                pass
243       
244       
245        ##################################################################
246       
247        def sync_to_helicopter(self):
248               
249                self.protocol.command_packet = COMMAND_PACKET['no_thrust']
250                QtCore.QThread.msleep(PROTOCOL_SYNC_TIME * 1000) # 4 seconds minimum to sync
251       
252       
253        ##################################################################
254       
255        def neutral(self):
256               
257                if self.DEBUG:
258                        print "--> RC Helicopter Command: neutral"
259               
260                #self.protocol.command_packet = COMMAND_PACKET['neutral']
261                self.protocol.command_packet = COMMAND_PACKET['no_thrust']
262       
263       
264        ##################################################################
265       
266        def test_packet(self):
267               
268                if self.DEBUG:
269                        print "--> RC Helicopter Command: test_packet"
270               
271                self.protocol.command_packet = COMMAND_PACKET['test_packet']
272       
273       
274        ##################################################################
275       
276        def test_mode(self):
277               
278                if self.DEBUG:
279                        print "--> RC Helicopter Command: test_mode"
280               
281                #self.sync_to_helicopter()
282               
283                self.protocol.command_packet = COMMAND_PACKET['minimum_thrust']
284                QtCore.QThread.msleep(2 * 1000) # 1 second
285               
286                self.protocol.command_packet = COMMAND_PACKET['no_thrust']
287       
288       
289        ##################################################################
290       
291        def hover(self, duration=2):
292               
293                if self.DEBUG:
294                        print "--> RC Helicopter Command: hover"
295               
296                #self.sync_to_helicopter()
297               
298                self.protocol.command_packet = COMMAND_PACKET['fifty_percent_thrust']
299                #self.protocol.command_packet = COMMAND_PACKET['maximum_thrust']
300               
301                if duration != None:
302                        QtCore.QThread.msleep(duration * 1000)
303                       
304                        self.protocol.command_packet = COMMAND_PACKET['no_thrust']
305       
306       
307        ##################################################################
308       
309        def fly_forward(self, duration=2):
310               
311                if self.DEBUG:
312                        print "--> RC Helicopter Command: fly_forward"
313               
314                #self.sync_to_helicopter()
315               
316                self.protocol.command_packet = COMMAND_PACKET['fly_forward']
317               
318                if duration != None:
319                        QtCore.QThread.msleep(duration * 1000)
320                       
321                        self.protocol.command_packet = COMMAND_PACKET['no_thrust']
322       
323       
324        ##################################################################
325       
326        def processCommand(self):
327               
328                if (self.command == 'dump_packets') or (self.command == 'read'):
329                        self.mode = 'read'
330                else:
331                        self.mode = 'write'
332               
333               
334                self.configureRemote()
335               
336               
337                if (self.command == 'dump_packets'):
338                        self.mode = 'read'
339                        self.dump_packets()
340               
341                elif (self.command == 'neutral'):
342                        self.mode = 'write'
343                        self.neutral()
344               
345                elif (self.command == 'test_packet'):
346                        self.mode = 'write'
347                        self.test_packet()
348               
349                elif (self.command == 'test_mode'):
350                        self.mode = 'write'
351                        self.sync_to_helicopter()
352                        self.test_mode()
353               
354                elif (self.command == 'hover'):
355                        self.mode = 'write'
356                        self.sync_to_helicopter()
357                        self.hover()
358               
359                elif (self.command == 'fly_forward'):
360                        self.mode = 'write'
361                        self.sync_to_helicopter()
362                        self.fly_forward(duration=2)
363       
364       
365        ##################################################################
366       
367        def stop(self):
368               
369                #self.connection.close()
370                pass
371       
372       
373        ##################################################################
374       
375        def run(self):
376               
377                if self.DEBUG:
378                        print "<---- [%s] Main thread running" % "Helicopter Remote"
379               
380               
381                self.processCommand()
382               
383                self.exec_()
384       
385       
386        ##################################################################
387       
388        def exitThread(self, callThreadQuit=True):
389               
390                try:
391                        self.emulationTimer.stop()
392                except:
393                        pass
394               
395                if self.serial_device != None:
396                        self.serial_device.exitThread()
397               
398                if self.protocol != None:
399                        self.protocol.exitThread()
400               
401                self.socket.close()
402               
403                if callThreadQuit:
404                        QtCore.QThread.quit(self)
405               
406                if self.parent == None:
407                        sys.exit()
408
409
410#####################################################################
411#####################################################################
412
413class ProtocolHandler(QtCore.QThread):
414       
415        def __init__(self, log, \
416                               serial_device, \
417                               mode=DEFAULT_MODE, \
418                               DEBUG=DEBUG, \
419                               parent=None):
420               
421                QtCore.QThread.__init__(self,parent)
422               
423                self.log = log
424                self.DEBUG = DEBUG
425                self.parent = parent
426               
427                self.device = None
428                self.mode = mode
429               
430                self.device = serial_device
431               
432                self.packet_count = 0
433                self.bad_packets = 0
434               
435                self.keep_running = True
436               
437                self.command_packet = DEFAULT_COMMAND_PACKET
438       
439       
440        ##################################################################
441       
442        def processDataPayload(self, data_payload, payload_timestamp):
443               
444                packet_update = {}
445                packet_update['data_payload'] = data_payload
446                packet_update['payload_timestamp'] = payload_timestamp
447               
448               
449                if (self.parent != None):
450                        self.parent.processPacket(packet_update)
451       
452       
453        ##################################################################
454       
455        def parseStream(self):
456               
457                # Loop forever, parsing one packet per loop
458                packet_count = 0
459               
460                while self.keep_running:
461                       
462                        # Synchronize on [SYNC] bytes
463                        byte = self.device.read()
464                        #print byte.encode("hex")
465                       
466                        #if (byte != PROTOCOL_SYNC):
467                        if (byte != PROTOCOL_SYNC_HEAD1):
468                                continue
469                       
470                        byte = self.device.read()
471                        if (byte != PROTOCOL_SYNC_HEAD2):
472                                continue
473                       
474                       
475                        payload_timestamp = time.time()
476                       
477                        data_payload = self.device.getBuffer()
478                        data_payload = "%s%s%s" % (PROTOCOL_SYNC_HEAD1, PROTOCOL_SYNC_HEAD2, data_payload)
479                       
480                       
481                        if len(data_payload) < PAYLOAD_MINIMUM_LENGTH:
482                        #if len(data_payload) != PACKET_LENGTH:
483                                continue
484                       
485                       
486                        self.processDataPayload(data_payload, payload_timestamp)
487                       
488                       
489                        #if self.DEBUG > 1:
490                                #packet_count += 1
491                                #if packet_count >= DEBUG_PACKET_COUNT:
492                                        #print "max debugging count reached, disconnecting"
493                                        #self.keep_running = False
494                                        #self.device.stop()
495                                        #QtCore.QThread.quit(self)
496                                        ##sys.exit()
497       
498       
499        ##################################################################
500       
501        def writeStream(self):
502               
503                # Loop forever, writing one packet per loop
504                packet_count = 0
505               
506                #import cPickle as pickle
507                #file = open('packet.data', 'r')
508                #packet = pickle.loads(file.read())
509                #file.close()
510               
511                while self.keep_running:
512                       
513                        # Preppend or Append [SYNC] bytes
514                        #if PROTOCOL_ADD_SYNC_TO_HEAD:
515                                #buffer = PROTOCOL_SYNC_HEAD1
516                                #buffer += PROTOCOL_SYNC_HEAD2
517                                #buffer += self.command_packet
518                       
519                        #if PROTOCOL_ADD_SYNC_TO_TAIL:
520                                #buffer = self.command_packet
521                                #buffer += PROTOCOL_SYNC_HEAD1
522                                #buffer += PROTOCOL_SYNC_HEAD2
523                               
524                        buffer = self.command_packet
525                        self.device.buffer = buffer
526                        #self.device.buffer = packet
527                        #print packet.encode("hex")
528                       
529                        # Sleep for 20 ms
530                        # Based on 50 Hz refresh rate of Blade MLP4DSM RC device
531                        # (1/50) * 1000 = 20
532                        QtCore.QThread.msleep(DEVICE_BUFFER_TIMER)
533       
534       
535        ##################################################################
536       
537        def run(self):
538               
539                self.packet_count = 0
540                self.bad_packets = 0
541                self.session_start_timestamp = time.time()
542               
543                if self.mode == 'read':
544                        self.parseStream()
545               
546                elif self.mode == 'write':
547                        self.writeStream()
548       
549       
550        ##################################################################
551       
552        def exitThread(self, callThreadQuit=True):
553               
554                try:
555                        self.device.stop()
556                except:
557                        pass
558               
559                #self.wait()
560                if callThreadQuit:
561                        QtCore.QThread.quit(self)
562
563
564#####################################################################
565#####################################################################
566
567class SerialDevice(QtCore.QThread):
568       
569        def __init__(self, log, \
570                               device_address=SERIAL_DEVICE, \
571                               mode=DEFAULT_MODE, \
572                               DEBUG=DEBUG, \
573                               parent=None):
574               
575                QtCore.QThread.__init__(self, parent)
576               
577                self.log = log
578                self.DEBUG = DEBUG
579               
580                self.device_address = device_address
581                self.mode = mode
582                self.device = None
583                self.buffer = ''
584               
585                if (self.device_address.count(':') == 5):
586                        # Device address is a Bluetooth MAC address
587                        self.device = self.initializeBluetoothDevice()
588                else:
589                        # Device address is a serial port address
590                        self.device = self.initializeSerialDevice()
591               
592                #self.buffer_check_timer = QtCore.QTimer()
593                #QtCore.QObject.connect(self.buffer_check_timer, \
594                                       #QtCore.SIGNAL("timeout()"), \
595                                       #self.checkBuffer)
596                #self.buffer_check_timer.start(DEVICE_BUFFER_TIMER)
597               
598                self.keep_running = True
599       
600       
601        ##################################################################
602       
603        #def initializeBluetoothDevice(self):
604               
605                #socket = bluetooth.BluetoothSocket( bluetooth.RFCOMM )
606               
607                #try:
608                        #socket.connect((self.device_address, THINKGEAR_DEVICE_BLUETOOTH_CHANNEL))
609               
610                #except Exception, e:
611                        #if self.DEBUG:
612                                #print "ERROR:",
613                                #print e
614                                #sys.exit()
615               
616               
617                #return socket
618       
619       
620        ###################################################################
621       
622        def initializeSerialDevice(self):
623               
624                baudrate = DEFAULT_SERIAL_BAUDRATE
625                bytesize = 8
626                parity = 'NONE'
627                stopbits = 1
628                software_flow_control = 'f'
629                rts_cts_flow_control = 'f'
630                #timeout = 15
631                timeout = 5
632               
633                # convert bytesize
634                if (bytesize == 5):
635                        init_byte_size = serial.FIVEBITS
636                elif (bytesize == 6):
637                        init_byte_size = serial.SIXBITS
638                elif (bytesize == 7):
639                        init_byte_size = serial.SEVENBITS
640                elif (bytesize == 8):
641                        init_byte_size = serial.EIGHTBITS
642                else:
643                        #self.log.perror("Invalid value for %s modem byte size! Using default (8)" % modem_type)
644                        init_byte_size = serial.EIGHTBITS
645               
646                # convert parity
647                if (parity == 'NONE'):
648                        init_parity = serial.PARITY_NONE
649                elif (parity == 'EVEN'):
650                        init_parity = serial.PARITY_EVEN
651                elif (parity == 'ODD'):
652                        init_parity = serial.PARITY_ODD
653                else:
654                        #self.log.perror("Invalid value for %s modem parity! Using default (NONE)" % modem_type)
655                        init_parity = serial.PARITY_NONE
656               
657                # convert stopbits
658                if (stopbits == 1):
659                        init_stopbits = serial.STOPBITS_ONE
660                elif (stopbits == 2):
661                        init_stopbits = serial.STOPBITS_TWO
662                else:
663                        #self.log.perror("Invalid value for %s modem stopbits! Using default (8)" % modem_type)
664                        init_byte_size = serial.STOPBITS_ONE
665               
666                # convert software flow control
667                if (software_flow_control == 't'):
668                        init_software_flow_control = 1
669                else:
670                        init_software_flow_control = 0
671               
672                # convert rts cts flow control
673                if (rts_cts_flow_control == 't'):
674                        init_rts_cts_flow_control = 1
675                else:
676                        init_rts_cts_flow_control = 0
677               
678               
679                try:
680                       
681                        device = serialWrapper(port = self.device_address, \
682                                                    baudrate = baudrate, \
683                                                    bytesize = init_byte_size, \
684                                                    parity = init_parity, \
685                                                    stopbits = init_stopbits, \
686                                                    xonxoff = init_software_flow_control, \
687                                                    rtscts = init_rts_cts_flow_control, \
688                                                    timeout = timeout)
689               
690                except Exception, e:
691                        if self.DEBUG:
692                                print "ERROR:",
693                                print e,
694                                print self.device_address
695                                sys.exit()
696               
697               
698                #device.flushInput()
699                ##device.flushOutput()
700               
701               
702                return(device)
703       
704       
705        ###################################################################
706       
707        #def checkBuffer(self):
708               
709                #if self.DEBUG > 1:
710                        #print "INFO: Buffer size check:",
711                        #print len(self.buffer),
712                        #print "(maximum before reset is %i)" % DEVICE_BUFFER_MAX_SIZE
713               
714                #if (DEVICE_BUFFER_MAX_SIZE <= len(self.buffer)):
715                       
716                        #if self.DEBUG:
717                                #print "ERROR: Buffer size has grown too large, resetting"
718                       
719                        #self.reset()
720       
721       
722        ###################################################################
723       
724        def getBuffer(self):
725               
726                data_payload = self.buffer
727               
728                self.resetBuffer()
729               
730               
731                return(data_payload)
732       
733       
734        ###################################################################
735       
736        def resetBuffer(self):
737               
738                self.buffer = ''
739       
740       
741        ###################################################################
742       
743        def read(self, length=1):
744               
745                # Sleep for 20 ms if buffer is empty
746                # Based on 50 Hz refresh rate of Blade MLP4DSM RC device
747                # (1/50) * 1000 = 20
748                while len(self.buffer) < length:
749                        QtCore.QThread.msleep(DEVICE_BUFFER_TIMER)
750                       
751                bytes = self.buffer[:length]
752               
753                self.buffer = self.buffer[length:]
754               
755                return(bytes)
756       
757       
758        ###################################################################
759       
760        def stop(self):
761               
762                #self.buffer_check_timer.stop()
763                self.keep_running = False
764       
765       
766        ###################################################################
767       
768        def exitThread(self, callThreadQuit=True):
769               
770                self.stop()
771                self.close()
772               
773                if callThreadQuit:
774                        QtCore.QThread.quit(self)
775       
776       
777        ###################################################################
778       
779        def close(self):
780               
781                self.device.close()
782       
783       
784        ###################################################################
785       
786        def readBuffer(self):
787       
788                self.buffer = ''
789               
790                while self.keep_running:
791                       
792                       
793                        # High-Speed Echo Mode
794                        if (self.DEBUG > 3) and ECHO_ON:
795                                byte = self.device.recv(PACKET_READ_SIZE)
796                                self.device.write(byte)
797                                continue
798                       
799                       
800                        try:
801                                #byte = self.device.read()
802                                byte = self.device.recv(PACKET_READ_SIZE)
803                               
804                                #if ECHO_ON:
805                                self.device.write(byte)
806                               
807                                if (len(byte) != 0):
808                                        if self.DEBUG > 2:
809                                                print "Device read:",
810                                                print byte,
811                                                if ECHO_ON:
812                                                        print byte.encode("hex"),
813                                                        print "wrote:",
814                                                print byte.encode("hex")
815                                               
816                                        self.buffer += byte
817                       
818                        except:
819                                if self.DEBUG:
820                                        print "ERROR: failed to read from serial device"
821                                break
822               
823               
824                self.exitThread()
825       
826       
827        ###################################################################
828       
829        def writeBuffer(self):
830       
831                self.buffer = ''
832                #beacon_timer = 0
833               
834                while self.keep_running:
835                       
836                        if (len(self.buffer) != 0):
837                                buffer = self.buffer
838                                self.buffer = ''
839                               
840                               
841                                #if beacon_timer >= 750:
842                                        #buffer += '\xaa' + buffer
843                                        #beacon_timer = 0
844                               
845                               
846                                try:
847                                        self.device.write(buffer)
848                                       
849                                        if self.DEBUG > 1:
850                                                print "Device wrote:",
851                                                #print buffer,
852                                                print buffer.encode("hex")
853                               
854                                except:
855                                        if self.DEBUG:
856                                                print "ERROR: failed to write to serial device"
857                                        break
858                       
859                       
860                        # Sleep for 20 ms if buffer is empty
861                        # Based on 50 Hz refresh rate of Blade MLP4DSM RC device
862                        # (1/50) * 1000 = 20
863                        QtCore.QThread.msleep(DEVICE_BUFFER_TIMER)
864                        #beacon_timer += DEVICE_BUFFER_TIMER
865               
866               
867                self.exitThread()
868       
869       
870        ###################################################################
871       
872        def run(self):
873               
874                if self.mode == 'read':
875                        self.readBuffer()
876               
877                elif self.mode == 'write':
878                        self.writeBuffer()
879
880
881#####################################################################
882#####################################################################
883
884class serialWrapper(serial.Serial):
885       
886        #__init__(port=None, baudrate=9600, bytesize=EIGHTBITS, parity=PARITY_NONE, stopbits=STOPBITS_ONE, timeout=None, xonxoff=False, rtscts=False, writeTimeout=None, dsrdtr=False, interCharTimeout=None)
887       
888        def recv(self, size=1):
889               
890                return(self.read(size))
891
892
893#####################################################################
894# Functions
895#####################################################################
896
897#####################################################################
898# Main
899#####################################################################
900
901if __name__ == '__main__':
902       
903        # Perform correct KeyboardInterrupt handling
904        signal.signal(signal.SIGINT, signal.SIG_DFL)
905       
906        # Collect default settings and command line parameters
907        device = SERIAL_DEVICE
908        command = DEFAULT_COMMAND
909       
910        for each in sys.argv:
911               
912                if each.startswith("--device="):
913                        device = each[ len("--device="): ]
914                elif each.startswith("--command="):
915                        command = each[ len("--command="): ]
916       
917       
918        app = QtCore.QCoreApplication(sys.argv)
919       
920        rc = puzzlebox_brainstorms_helicopter_control(device_address=device, command=command, DEBUG=DEBUG)
921       
922        rc.start()
923       
924        sys.exit(app.exec_())
925
Note: See TracBrowser for help on using the repository browser.