source: trunk/Puzzlebox/Brainstorms/Helicopter_Control.py

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