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

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

Brainstorms/Helicopter_Control.py:

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