source: trunk/Puzzlebox/Brainstorms/Helicopter_Control.py

Last change on this file was 343, checked in by sc, 8 years ago
  • Update for configuration-based COMMAND_ACTIVATE definition
  • Property svn:executable set to *
File size: 22.1 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__ = """2012.06.14
12"""
13
14import sys, time
15import signal
16import serial
17
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"
42        from PyQt4 import QtCore
43
44#import Puzzlebox.Synapse.Protocol as protocol
45#from Puzzlebox.Synapse import Protocol
46
47#####################################################################
48# Globals
49#####################################################################
50
51DEBUG = 2
52
53DEFAULT_COMMAND = 'dump_packets'
54
55COMMAND_ACTIVATE = configuration.COMMAND_ACTIVATE
56
57SERIAL_DEVICE = '/dev/ttyUSB0'
58#SERIAL_DEVICE = '/dev/ttyACM0'
59#DEFAULT_SERIAL_BAUDRATE = 115200 # This is the closest "standard" baud rate the USB-to-Serial device will support
60#DEFAULT_SERIAL_BAUDRATE = 125000 # This is the speed reported by the forum post
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
63#DEFAULT_SERIAL_BAUDRATE = 230400 # This is the next highest "standard" baud rate the USB-to-Serial device will support
64DEFAULT_MODE = 'read'
65
66PROTOCOL_SYNC_TIME = 5
67PROTOCOL_SYNC_HEAD1 = '\x00'
68PROTOCOL_SYNC_HEAD2 = '\x00'
69PROTOCOL_ADD_SYNC_TO_HEAD = False
70PROTOCOL_ADD_SYNC_TO_TAIL = False
71PACKET_LENGTH = 14
72PAYLOAD_MINIMUM_LENGTH = 12
73PACKET_READ_SIZE = 14
74ECHO_ON = False
75
76#DEVICE_BUFFER_TIMER = 22  # Frame cycle 22ms
77DEVICE_BUFFER_TIMER = 21  # Frame cycle 22ms
78
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
141COMMAND_PACKET = {
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
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
149        'maximum_thrust':       '\x00\x00\x03\x54\x05\xc5\x09\xde\x0e\x0b\x13\x54\x14\xaa',  # maximum possible throttle and trim
150        'fifty_percent_thrust': '\x00\x00\x01\x7d\x05\xc5\x09\xde\x0e\x0b\x13\x54\x14\xaa', # calculated 50% throttle
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',
155}
156
157DEFAULT_COMMAND_PACKET = COMMAND_PACKET['neutral']
158
159#####################################################################
160# Classes
161#####################################################################
162
163class puzzlebox_brainstorms_helicopter_control(QtCore.QThread):
164       
165        def __init__(self, \
166                     device_address=SERIAL_DEVICE, \
167                     command=DEFAULT_COMMAND, \
168                     DEBUG=DEBUG, \
169                     parent=None):
170               
171                QtCore.QThread.__init__(self, parent)
172               
173                self.log = None
174                self.DEBUG = DEBUG
175                self.parent = parent
176               
177                self.device_address = device_address
178                self.command = command
179                self.mode = DEFAULT_MODE
180               
181                self.serial_device = None
182                self.protocol = None
183               
184               
185                #self.configureRemote()
186       
187       
188        ##################################################################
189       
190        def configureRemote(self):
191               
192                self.serial_device = \
193                        SerialDevice( \
194                                self.log, \
195                                device_address=self.device_address, \
196                                mode=self.mode, \
197                                DEBUG=self.DEBUG, \
198                                parent=self)
199               
200                self.serial_device.start()
201               
202               
203                self.protocol = \
204                        ProtocolHandler( \
205                                self.log, \
206                                self.serial_device, \
207                                mode=self.mode, \
208                                DEBUG=self.DEBUG, \
209                                parent=self)
210               
211                self.protocol.start()
212       
213       
214        ##################################################################
215       
216        def processPacket(self, packet):
217               
218                if self.DEBUG:
219                        print "data_payload:",
220                        #print packet['data_payload']
221                        print packet['data_payload'].encode("hex")
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_())
230               
231                #if (packet != {}):
232                        #self.packet_queue.append(packet)
233                        ###self.sendPacketQueue()
234               
235                if (self.parent != None):
236                        self.parent.processPacket(self.protocol.data_packet)
237       
238       
239        ##################################################################
240       
241        def dump_packets(self):
242               
243                pass
244       
245       
246        ##################################################################
247       
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       
256        def neutral(self):
257               
258                if self.DEBUG:
259                        print "--> RC Helicopter Command: neutral"
260               
261                #self.protocol.command_packet = COMMAND_PACKET['neutral']
262                self.protocol.command_packet = COMMAND_PACKET['no_thrust']
263       
264       
265        ##################################################################
266       
267        def test_packet(self):
268               
269                if self.DEBUG:
270                        print "--> RC Helicopter Command: test_packet"
271               
272                self.protocol.command_packet = COMMAND_PACKET['test_packet']
273       
274       
275        ##################################################################
276       
277        def test_mode(self):
278               
279                if self.DEBUG:
280                        print "--> RC Helicopter Command: test_mode"
281               
282                #self.sync_to_helicopter()
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       
292        def hover(self, duration=2):
293               
294                if self.DEBUG:
295                        print "--> RC Helicopter Command: hover"
296               
297                #self.sync_to_helicopter()
298               
299                #self.protocol.command_packet = COMMAND_PACKET['fifty_percent_thrust']
300                ##self.protocol.command_packet = COMMAND_PACKET['maximum_thrust']
301               
302                self.protocol.command_packet = COMMAND_PACKET[COMMAND_ACTIVATE]
303               
304                if duration != None:
305                        QtCore.QThread.msleep(duration * 1000)
306                       
307                        self.protocol.command_packet = COMMAND_PACKET['no_thrust']
308       
309       
310        ##################################################################
311       
312        def fly_forward(self, duration=2):
313               
314                if self.DEBUG:
315                        print "--> RC Helicopter Command: fly_forward"
316               
317                #self.sync_to_helicopter()
318               
319                self.protocol.command_packet = COMMAND_PACKET['fly_forward']
320               
321                if duration != None:
322                        QtCore.QThread.msleep(duration * 1000)
323                       
324                        self.protocol.command_packet = COMMAND_PACKET['no_thrust']
325       
326       
327        ##################################################################
328       
329        def processCommand(self):
330               
331                if (self.command == 'dump_packets') or (self.command == 'read'):
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'
342                        self.dump_packets()
343               
344                elif (self.command == 'neutral'):
345                        self.mode = 'write'
346                        self.neutral()
347               
348                elif (self.command == 'test_packet'):
349                        self.mode = 'write'
350                        self.test_packet()
351               
352                elif (self.command == 'test_mode'):
353                        self.mode = 'write'
354                        self.sync_to_helicopter()
355                        self.test_mode()
356               
357                elif (self.command == 'hover'):
358                        self.mode = 'write'
359                        self.sync_to_helicopter()
360                        self.hover()
361               
362                elif (self.command == 'fly_forward'):
363                        self.mode = 'write'
364                        self.sync_to_helicopter()
365                        self.fly_forward(duration=2)
366       
367       
368        ##################################################################
369       
370        def stop(self):
371               
372                #self.connection.close()
373                pass
374       
375       
376        ##################################################################
377       
378        def run(self):
379               
380                if self.DEBUG:
381                        print "<---- [%s] Main thread running" % "Helicopter Remote"
382               
383               
384                self.processCommand()
385               
386                self.exec_()
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, \
420                               mode=DEFAULT_MODE, \
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
431                self.mode = mode
432               
433                self.device = serial_device
434               
435                self.packet_count = 0
436                self.bad_packets = 0
437               
438                self.keep_running = True
439               
440                self.command_packet = DEFAULT_COMMAND_PACKET
441       
442       
443        ##################################################################
444       
445        def processDataPayload(self, data_payload, payload_timestamp):
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)
454       
455       
456        ##################################################################
457       
458        def parseStream(self):
459               
460                # Loop forever, parsing one packet per loop
461                packet_count = 0
462               
463                while self.keep_running:
464                       
465                        # Synchronize on [SYNC] bytes
466                        byte = self.device.read()
467                        #print byte.encode("hex")
468                       
469                        #if (byte != PROTOCOL_SYNC):
470                        if (byte != PROTOCOL_SYNC_HEAD1):
471                                continue
472                       
473                        byte = self.device.read()
474                        if (byte != PROTOCOL_SYNC_HEAD2):
475                                continue
476                       
477                       
478                        payload_timestamp = time.time()
479                       
480                        data_payload = self.device.getBuffer()
481                        data_payload = "%s%s%s" % (PROTOCOL_SYNC_HEAD1, PROTOCOL_SYNC_HEAD2, data_payload)
482                       
483                       
484                        if len(data_payload) < PAYLOAD_MINIMUM_LENGTH:
485                        #if len(data_payload) != PACKET_LENGTH:
486                                continue
487                       
488                       
489                        self.processDataPayload(data_payload, payload_timestamp)
490                       
491                       
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()
500       
501       
502        ##################################################################
503       
504        def writeStream(self):
505               
506                # Loop forever, writing one packet per loop
507                packet_count = 0
508               
509                #import cPickle as pickle
510                #file = open('packet.data', 'r')
511                #packet = pickle.loads(file.read())
512                #file.close()
513               
514                while self.keep_running:
515                       
516                        # Preppend or Append [SYNC] bytes
517                        #if PROTOCOL_ADD_SYNC_TO_HEAD:
518                                #buffer = PROTOCOL_SYNC_HEAD1
519                                #buffer += PROTOCOL_SYNC_HEAD2
520                                #buffer += self.command_packet
521                       
522                        #if PROTOCOL_ADD_SYNC_TO_TAIL:
523                                #buffer = self.command_packet
524                                #buffer += PROTOCOL_SYNC_HEAD1
525                                #buffer += PROTOCOL_SYNC_HEAD2
526                               
527                        buffer = self.command_packet
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)
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               
546                if self.mode == 'read':
547                        self.parseStream()
548               
549                elif self.mode == 'write':
550                        self.writeStream()
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
570class SerialDevice(QtCore.QThread):
571       
572        def __init__(self, log, \
573                               device_address=SERIAL_DEVICE, \
574                               mode=DEFAULT_MODE, \
575                               DEBUG=DEBUG, \
576                               parent=None):
577               
578                QtCore.QThread.__init__(self, parent)
579               
580                self.log = log
581                self.DEBUG = DEBUG
582               
583                self.device_address = device_address
584                self.mode = mode
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)
599                #self.buffer_check_timer.start(DEVICE_BUFFER_TIMER)
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       
727        def getBuffer(self):
728               
729                data_payload = self.buffer
730               
731                self.resetBuffer()
732               
733               
734                return(data_payload)
735       
736       
737        ###################################################################
738       
739        def resetBuffer(self):
740               
741                self.buffer = ''
742       
743       
744        ###################################################################
745       
746        def read(self, length=1):
747               
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
751                while len(self.buffer) < length:
752                        QtCore.QThread.msleep(DEVICE_BUFFER_TIMER)
753                       
754                bytes = self.buffer[:length]
755               
756                self.buffer = self.buffer[length:]
757               
758                return(bytes)
759       
760       
761        ###################################################################
762       
763        def stop(self):
764               
765                #self.buffer_check_timer.stop()
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       
789        def readBuffer(self):
790       
791                self.buffer = ''
792               
793                while self.keep_running:
794                       
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                       
803                        try:
804                                #byte = self.device.read()
805                                byte = self.device.recv(PACKET_READ_SIZE)
806                               
807                                #if ECHO_ON:
808                                self.device.write(byte)
809                               
810                                if (len(byte) != 0):
811                                        if self.DEBUG > 2:
812                                                print "Device read:",
813                                                print byte,
814                                                if ECHO_ON:
815                                                        print byte.encode("hex"),
816                                                        print "wrote:",
817                                                print byte.encode("hex")
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               
827                self.exitThread()
828       
829       
830        ###################################################################
831       
832        def writeBuffer(self):
833       
834                self.buffer = ''
835                #beacon_timer = 0
836               
837                while self.keep_running:
838                       
839                        if (len(self.buffer) != 0):
840                                buffer = self.buffer
841                                self.buffer = ''
842                               
843                               
844                                #if beacon_timer >= 750:
845                                        #buffer += '\xaa' + buffer
846                                        #beacon_timer = 0
847                               
848                               
849                                try:
850                                        self.device.write(buffer)
851                                       
852                                        if self.DEBUG > 1:
853                                                print "Device wrote:",
854                                                #print buffer,
855                                                print buffer.encode("hex")
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)
867                        #beacon_timer += DEVICE_BUFFER_TIMER
868               
869               
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()
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       
906        # Perform correct KeyboardInterrupt handling
907        signal.signal(signal.SIGINT, signal.SIG_DFL)
908       
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       
925        rc.start()
926       
927        sys.exit(app.exec_())
928
Note: See TracBrowser for help on using the repository browser.