source: trunk/brainstorms/Puzzlebox/Brainstorms/Helicopter_Control.py @ 213

Last change on this file since 213 was 213, checked in by sc, 12 years ago

trunk/brainstorms/Puzzlebox/Brainstorms/Helicopter_Control.py:

  • PACKET_READ_SIZE added
  • High-speed echo mode added
  • Property svn:executable set to *
File size: 18.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)
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: 2010.11.18
13
14"""
15
16import sys, time
17import signal
18import serial
19
20try:
21        import PySide as PyQt4
22        from PySide import QtCore
23except:
24        print "Using PyQt4 module"
25        from PyQt4 import QtCore
26else:
27        print "Using PySide module"
28
29import Configuration as configuration
30
31#import Puzzlebox.Synapse.Protocol as protocol
32from Puzzlebox.Synapse import Protocol
33
34#####################################################################
35# Globals
36#####################################################################
37
38DEBUG = 3
39
40DEFAULT_COMMAND = 'dump_packets'
41
42SERIAL_DEVICE = '/dev/ttyUSB0'
43DEFAULT_SERIAL_BAUDRATE = 115200 # This is the closest "standard" baud rate the USB-to-Serial device will support
44#DEFAULT_SERIAL_BAUDRATE = 125000 # This is the speed reported by the forum post
45#DEFAULT_SERIAL_BAUDRATE = 133333 # This is the speed reported by the logic analyzer
46#DEFAULT_SERIAL_BAUDRATE = 230400 # This is the next highest "standard" baud rate the USB-to-Serial device will support
47DEFAULT_MODE = 'read'
48
49PROTOCOL_SYNC = '\x80'
50PROTOCOL_SYNC_HEAD1 = '\x00'
51PROTOCOL_SYNC_HEAD2 = '\x00'
52PROTOCOL_ADD_SYNC_TO_HEAD = False
53PROTOCOL_ADD_SYNC_TO_TAIL = False
54PACKET_LENGTH = 14
55PAYLOAD_MINIMUM_LENGTH = 8
56PACKET_READ_SIZE = 11
57ECHO_ON = False
58
59DEVICE_BUFFER_TIMER = 22  # Frame cycle 22ms
60
61COMMAND_PACKET = {
62        'neutral': '\x80\x53\x68\x81\x86\xd1\xa4\x29', \
63        'no_thrust': '\x80\xac\xdf\xa2\x9a\xb0\x8b\x8d\x54', \
64        'minimum_thrust': '\x80\x59\x68\x81\x86\xd1\xa4\x29', \
65        'maximum_thrust': '\x80\x83\xad\xe7\xa2\xad\xf0\x8b\x8d\x54\xff',
66        'test_packet': '\x00\x00\x03\x54\x06\x15\x09\xca\x0e\x2f\x13\x54\x14\xaa', \
67        'maximum_forward': '\x83\xad\xde\x11\xcd\xb0\x8b\x8d\x54', \
68}
69
70DEFAULT_COMMAND_PACKET = COMMAND_PACKET['neutral']
71
72#####################################################################
73# Classes
74#####################################################################
75
76class puzzlebox_brainstorms_helicopter_control(QtCore.QThread):
77       
78        def __init__(self, \
79                     device_address=SERIAL_DEVICE, \
80                     command=DEFAULT_COMMAND, \
81                     DEBUG=DEBUG, \
82                     parent=None):
83               
84                QtCore.QThread.__init__(self, parent)
85               
86                self.log = None
87                self.DEBUG = DEBUG
88                self.parent = parent
89               
90                self.device_address = device_address
91                self.command = command
92                self.mode = DEFAULT_MODE
93               
94                self.serial_device = None
95                self.protocol = None
96               
97               
98                #self.configureRemote()
99       
100       
101        ##################################################################
102       
103        def configureRemote(self):
104               
105                self.serial_device = \
106                        SerialDevice( \
107                                self.log, \
108                                device_address=self.device_address, \
109                                mode=self.mode, \
110                                DEBUG=self.DEBUG, \
111                                parent=self)
112               
113                self.serial_device.start()
114               
115               
116                self.protocol = \
117                        ProtocolHandler( \
118                                self.log, \
119                                self.serial_device, \
120                                mode=self.mode, \
121                                DEBUG=self.DEBUG, \
122                                parent=self)
123               
124                self.protocol.start()
125       
126       
127        ##################################################################
128       
129        def processPacket(self, packet):
130               
131                if self.DEBUG:
132                        print "data_payload:",
133                        print packet['data_payload']
134                        print packet['data_payload'].encode("hex")
135                       
136                        #if packet['data_payload'].encode("hex") == '80acdf22cdb08b8d54':
137                                #print True
138                                #import cPickle as pickle
139                                #file = open('packet.data', 'w')
140                                #pickle.dump(packet['data_payload'], file)
141                                #file.close()
142                                #sys.exit(app.exec_())
143               
144                #if (packet != {}):
145                        #self.packet_queue.append(packet)
146                        ###self.sendPacketQueue()
147               
148                if (self.parent != None):
149                        self.parent.processPacket(self.protocol.data_packet)
150       
151       
152        ##################################################################
153       
154        def dump_packets(self):
155               
156                pass
157       
158       
159        ##################################################################
160       
161        def neutral(self):
162               
163                if self.DEBUG:
164                        print "--> Command: neutral"
165               
166                self.protocol.command_packet = COMMAND_PACKET['neutral']       
167       
168       
169        ##################################################################
170       
171        def test_packet(self):
172               
173                if self.DEBUG:
174                        print "--> Command: test_packet"
175               
176                self.protocol.command_packet = COMMAND_PACKET['test_packet']
177       
178       
179        ##################################################################
180       
181        def hover(self):
182               
183                if self.DEBUG:
184                        print "--> Command: hover"
185               
186                self.protocol.command_packet = COMMAND_PACKET['neutral']
187                QtCore.QThread.msleep(1 * 1000) # 1 second
188               
189                self.protocol.command_packet = COMMAND_PACKET['minimum_thrust']
190                QtCore.QThread.msleep(1 * 1000) # 1 second
191               
192                self.protocol.command_packet = COMMAND_PACKET['neutral']
193       
194       
195        ##################################################################
196       
197        def fly_forward(self, duration):
198               
199                if self.DEBUG:
200                        print "--> Command: fly_forward"
201               
202                self.protocol.command_packet = COMMAND_PACKET['no_thrust']
203                QtCore.QThread.msleep(1 * 1000) # 1 second
204               
205                self.protocol.command_packet = COMMAND_PACKET['maximum_forward']
206                QtCore.QThread.msleep(duration * 1000) # 1 second
207               
208                self.protocol.command_packet = COMMAND_PACKET['no_thrust']
209       
210       
211        ##################################################################
212       
213        def processCommand(self):
214               
215                if (self.command == 'dump_packets'):
216                        self.mode = 'read'
217                else:
218                        self.mode = 'write'
219               
220               
221                self.configureRemote()
222               
223               
224                if (self.command == 'dump_packets'):
225                        self.mode = 'read'
226                        self.dump_packets()
227               
228                elif (self.command == 'neutral'):
229                        self.mode = 'write'
230                        self.neutral()         
231                elif (self.command == 'test_packet'):
232                        self.mode = 'write'
233                        self.test_packet()
234               
235                elif (self.command == 'hover'):
236                        self.mode = 'write'
237                        self.hover()
238               
239                elif (self.command == 'fly_forward'):
240                        self.mode = 'write'
241                        self.fly_forward(duration=3)
242       
243       
244        ##################################################################
245       
246        def stop(self):
247               
248                #self.connection.close()
249                pass
250       
251       
252        ##################################################################
253       
254        def run(self):
255               
256                if self.DEBUG:
257                        print "<---- [%s] Main thread running" % "Helicopter Remote"
258               
259               
260                self.processCommand()
261               
262                self.exec_()
263       
264       
265        ##################################################################
266       
267        def exitThread(self, callThreadQuit=True):
268               
269                try:
270                        self.emulationTimer.stop()
271                except:
272                        pass
273               
274                if self.serial_device != None:
275                        self.serial_device.exitThread()
276               
277                if self.protocol != None:
278                        self.protocol.exitThread()
279               
280                self.socket.close()
281               
282                if callThreadQuit:
283                        QtCore.QThread.quit(self)
284               
285                if self.parent == None:
286                        sys.exit()
287
288
289#####################################################################
290#####################################################################
291
292class ProtocolHandler(QtCore.QThread):
293       
294        def __init__(self, log, \
295                               serial_device, \
296                               mode=DEFAULT_MODE, \
297                               DEBUG=DEBUG, \
298                               parent=None):
299               
300                QtCore.QThread.__init__(self,parent)
301               
302                self.log = log
303                self.DEBUG = DEBUG
304                self.parent = parent
305               
306                self.device = None
307                self.mode = mode
308               
309                self.device = serial_device
310               
311                self.packet_count = 0
312                self.bad_packets = 0
313               
314                self.keep_running = True
315               
316                self.command_packet = DEFAULT_COMMAND_PACKET
317       
318       
319        ##################################################################
320       
321        def processDataPayload(self, data_payload, payload_timestamp):
322               
323                packet_update = {}
324                packet_update['data_payload'] = data_payload
325                packet_update['payload_timestamp'] = payload_timestamp
326               
327               
328                if (self.parent != None):
329                        self.parent.processPacket(packet_update)
330       
331       
332        ##################################################################
333       
334        def parseStream(self):
335               
336                # Loop forever, parsing one packet per loop
337                packet_count = 0
338               
339                while self.keep_running:
340                       
341                        # Synchronize on [SYNC] bytes
342                        byte = self.device.read()
343                        #print byte.encode("hex")
344                       
345                        #if (byte != PROTOCOL_SYNC):
346                        if (byte != PROTOCOL_SYNC_HEAD1):
347                                continue
348                       
349                        byte = self.device.read()
350                        if (byte != PROTOCOL_SYNC_HEAD2):
351                                continue
352                       
353                       
354                        payload_timestamp = time.time()
355                       
356                        data_payload = self.device.getBuffer()
357                       
358                       
359                        if len(data_payload) < PAYLOAD_MINIMUM_LENGTH:
360                                continue
361                       
362                       
363                        self.processDataPayload(data_payload, payload_timestamp)
364                       
365                       
366                        #if self.DEBUG > 1:
367                                #packet_count += 1
368                                #if packet_count >= DEBUG_PACKET_COUNT:
369                                        #print "max debugging count reached, disconnecting"
370                                        #self.keep_running = False
371                                        #self.device.stop()
372                                        #QtCore.QThread.quit(self)
373                                        ##sys.exit()
374       
375       
376        ##################################################################
377       
378        def writeStream(self):
379               
380                # Loop forever, writing one packet per loop
381                packet_count = 0
382               
383                #import cPickle as pickle
384                #file = open('packet.data', 'r')
385                #packet = pickle.loads(file.read())
386                #file.close()
387               
388                while self.keep_running:
389                       
390                        # Preppend or Append [SYNC] bytes
391                        #if PROTOCOL_ADD_SYNC_TO_HEAD:
392                                #buffer = PROTOCOL_SYNC_HEAD1
393                                #buffer += PROTOCOL_SYNC_HEAD2
394                                #buffer += self.command_packet
395                       
396                        #if PROTOCOL_ADD_SYNC_TO_TAIL:
397                                #buffer = self.command_packet
398                                #buffer += PROTOCOL_SYNC_HEAD1
399                                #buffer += PROTOCOL_SYNC_HEAD2
400                               
401                        buffer = self.command_packet
402                        self.device.buffer = buffer
403                        #self.device.buffer = packet
404                        #print packet.encode("hex")
405                       
406                        # Sleep for 20 ms
407                        # Based on 50 Hz refresh rate of Blade MLP4DSM RC device
408                        # (1/50) * 1000 = 20
409                        QtCore.QThread.msleep(DEVICE_BUFFER_TIMER)
410       
411       
412        ##################################################################
413       
414        def run(self):
415               
416                self.packet_count = 0
417                self.bad_packets = 0
418                self.session_start_timestamp = time.time()
419               
420                if self.mode == 'read':
421                        self.parseStream()
422               
423                elif self.mode == 'write':
424                        self.writeStream()
425       
426       
427        ##################################################################
428       
429        def exitThread(self, callThreadQuit=True):
430               
431                try:
432                        self.device.stop()
433                except:
434                        pass
435               
436                #self.wait()
437                if callThreadQuit:
438                        QtCore.QThread.quit(self)
439
440
441#####################################################################
442#####################################################################
443
444class SerialDevice(QtCore.QThread):
445       
446        def __init__(self, log, \
447                               device_address=SERIAL_DEVICE, \
448                               mode=DEFAULT_MODE, \
449                               DEBUG=DEBUG, \
450                               parent=None):
451               
452                QtCore.QThread.__init__(self, parent)
453               
454                self.log = log
455                self.DEBUG = DEBUG
456               
457                self.device_address = device_address
458                self.mode = mode
459                self.device = None
460                self.buffer = ''
461               
462                if (self.device_address.count(':') == 5):
463                        # Device address is a Bluetooth MAC address
464                        self.device = self.initializeBluetoothDevice()
465                else:
466                        # Device address is a serial port address
467                        self.device = self.initializeSerialDevice()
468               
469                #self.buffer_check_timer = QtCore.QTimer()
470                #QtCore.QObject.connect(self.buffer_check_timer, \
471                                       #QtCore.SIGNAL("timeout()"), \
472                                       #self.checkBuffer)
473                #self.buffer_check_timer.start(DEVICE_BUFFER_TIMER)
474               
475                self.keep_running = True
476       
477       
478        ##################################################################
479       
480        #def initializeBluetoothDevice(self):
481               
482                #socket = bluetooth.BluetoothSocket( bluetooth.RFCOMM )
483               
484                #try:
485                        #socket.connect((self.device_address, THINKGEAR_DEVICE_BLUETOOTH_CHANNEL))
486               
487                #except Exception, e:
488                        #if self.DEBUG:
489                                #print "ERROR:",
490                                #print e
491                                #sys.exit()
492               
493               
494                #return socket
495       
496       
497        ###################################################################
498       
499        def initializeSerialDevice(self):
500               
501                baudrate = DEFAULT_SERIAL_BAUDRATE
502                bytesize = 8
503                parity = 'NONE'
504                stopbits = 1
505                software_flow_control = 'f'
506                rts_cts_flow_control = 'f'
507                #timeout = 15
508                timeout = 5
509               
510                # convert bytesize
511                if (bytesize == 5):
512                        init_byte_size = serial.FIVEBITS
513                elif (bytesize == 6):
514                        init_byte_size = serial.SIXBITS
515                elif (bytesize == 7):
516                        init_byte_size = serial.SEVENBITS
517                elif (bytesize == 8):
518                        init_byte_size = serial.EIGHTBITS
519                else:
520                        #self.log.perror("Invalid value for %s modem byte size! Using default (8)" % modem_type)
521                        init_byte_size = serial.EIGHTBITS
522               
523                # convert parity
524                if (parity == 'NONE'):
525                        init_parity = serial.PARITY_NONE
526                elif (parity == 'EVEN'):
527                        init_parity = serial.PARITY_EVEN
528                elif (parity == 'ODD'):
529                        init_parity = serial.PARITY_ODD
530                else:
531                        #self.log.perror("Invalid value for %s modem parity! Using default (NONE)" % modem_type)
532                        init_parity = serial.PARITY_NONE
533               
534                # convert stopbits
535                if (stopbits == 1):
536                        init_stopbits = serial.STOPBITS_ONE
537                elif (stopbits == 2):
538                        init_stopbits = serial.STOPBITS_TWO
539                else:
540                        #self.log.perror("Invalid value for %s modem stopbits! Using default (8)" % modem_type)
541                        init_byte_size = serial.STOPBITS_ONE
542               
543                # convert software flow control
544                if (software_flow_control == 't'):
545                        init_software_flow_control = 1
546                else:
547                        init_software_flow_control = 0
548               
549                # convert rts cts flow control
550                if (rts_cts_flow_control == 't'):
551                        init_rts_cts_flow_control = 1
552                else:
553                        init_rts_cts_flow_control = 0
554               
555               
556                try:
557                       
558                        device = serialWrapper(port = self.device_address, \
559                                                    baudrate = baudrate, \
560                                                    bytesize = init_byte_size, \
561                                                    parity = init_parity, \
562                                                    stopbits = init_stopbits, \
563                                                    xonxoff = init_software_flow_control, \
564                                                    rtscts = init_rts_cts_flow_control, \
565                                                    timeout = timeout)
566               
567                except Exception, e:
568                        if self.DEBUG:
569                                print "ERROR:",
570                                print e,
571                                print self.device_address
572                                sys.exit()
573               
574               
575                #device.flushInput()
576                ##device.flushOutput()
577               
578               
579                return(device)
580       
581       
582        ###################################################################
583       
584        #def checkBuffer(self):
585               
586                #if self.DEBUG > 1:
587                        #print "INFO: Buffer size check:",
588                        #print len(self.buffer),
589                        #print "(maximum before reset is %i)" % DEVICE_BUFFER_MAX_SIZE
590               
591                #if (DEVICE_BUFFER_MAX_SIZE <= len(self.buffer)):
592                       
593                        #if self.DEBUG:
594                                #print "ERROR: Buffer size has grown too large, resetting"
595                       
596                        #self.reset()
597       
598       
599        ###################################################################
600       
601        def getBuffer(self):
602               
603                data_payload = self.buffer
604               
605                self.resetBuffer()
606               
607               
608                return(data_payload)
609       
610       
611        ###################################################################
612       
613        def resetBuffer(self):
614               
615                self.buffer = ''
616       
617       
618        ###################################################################
619       
620        def read(self, length=1):
621               
622                # Sleep for 20 ms if buffer is empty
623                # Based on 50 Hz refresh rate of Blade MLP4DSM RC device
624                # (1/50) * 1000 = 20
625                while len(self.buffer) < length:
626                        QtCore.QThread.msleep(DEVICE_BUFFER_TIMER)
627                       
628                bytes = self.buffer[:length]
629               
630                self.buffer = self.buffer[length:]
631               
632                return(bytes)
633       
634       
635        ###################################################################
636       
637        def stop(self):
638               
639                #self.buffer_check_timer.stop()
640                self.keep_running = False
641       
642       
643        ###################################################################
644       
645        def exitThread(self, callThreadQuit=True):
646               
647                self.stop()
648                self.close()
649               
650                if callThreadQuit:
651                        QtCore.QThread.quit(self)
652       
653       
654        ###################################################################
655       
656        def close(self):
657               
658                self.device.close()
659       
660       
661        ###################################################################
662       
663        def readBuffer(self):
664       
665                self.buffer = ''
666               
667                while self.keep_running:
668                       
669                       
670                        # High-Speed Echo Mode
671                        if (self.DEBUG > 3) and ECHO_ON:
672                                byte = self.device.recv(PACKET_READ_SIZE)
673                                self.device.write(byte)
674                                continue
675                       
676                       
677                        try:
678                                #byte = self.device.read()
679                                byte = self.device.recv(PACKET_READ_SIZE)
680                               
681                                #if ECHO_ON:
682                                self.device.write(byte)
683                               
684                                if (len(byte) != 0):
685                                        if self.DEBUG > 2:
686                                                print "Device read:",
687                                                print byte,
688                                                if ECHO_ON:
689                                                        print byte.encode("hex"),
690                                                        print "wrote:",
691                                                print byte.encode("hex")
692                                               
693                                        self.buffer += byte
694                       
695                        except:
696                                if self.DEBUG:
697                                        print "ERROR: failed to read from serial device"
698                                break
699               
700               
701                self.exitThread()
702       
703       
704        ###################################################################
705       
706        def writeBuffer(self):
707       
708                self.buffer = ''
709               
710                while self.keep_running:
711                       
712                        if (len(self.buffer) != 0):
713                                buffer = self.buffer
714                                self.buffer = ''
715                               
716                                try:
717                                        self.device.write(buffer)
718                                       
719                                        if self.DEBUG > 1:
720                                                print "Device wrote:",
721                                                print buffer,
722                                                print buffer.encode("hex")
723                               
724                                except:
725                                        if self.DEBUG:
726                                                print "ERROR: failed to write to serial device"
727                                        break
728                       
729                       
730                        # Sleep for 20 ms if buffer is empty
731                        # Based on 50 Hz refresh rate of Blade MLP4DSM RC device
732                        # (1/50) * 1000 = 20
733                        QtCore.QThread.msleep(DEVICE_BUFFER_TIMER)
734               
735                self.exitThread()
736       
737       
738        ###################################################################
739       
740        def run(self):
741               
742                if self.mode == 'read':
743                        self.readBuffer()
744               
745                elif self.mode == 'write':
746                        self.writeBuffer()
747
748
749#####################################################################
750#####################################################################
751
752class serialWrapper(serial.Serial):
753       
754        #__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)
755       
756        def recv(self, size=1):
757               
758                return(self.read(size))
759
760
761#####################################################################
762# Functions
763#####################################################################
764
765#####################################################################
766# Main
767#####################################################################
768
769if __name__ == '__main__':
770       
771        # Perform correct KeyboardInterrupt handling
772        signal.signal(signal.SIGINT, signal.SIG_DFL)
773       
774        # Collect default settings and command line parameters
775        device = SERIAL_DEVICE
776        command = DEFAULT_COMMAND
777       
778        for each in sys.argv:
779               
780                if each.startswith("--device="):
781                        device = each[ len("--device="): ]
782                elif each.startswith("--command="):
783                        command = each[ len("--command="): ]
784       
785       
786        app = QtCore.QCoreApplication(sys.argv)
787       
788        rc = puzzlebox_brainstorms_helicopter_control(device_address=device, command=command, DEBUG=DEBUG)
789       
790        rc.start()
791       
792        sys.exit(app.exec_())
793
Note: See TracBrowser for help on using the repository browser.