Changeset 208 for trunk


Ignore:
Timestamp:
11/15/10 18:32:37 (11 years ago)
Author:
sc
Message:

Brainstorms/Helicopter_Control.py:

  • returned to single sync byte
  • PAYLOAD_MINIMUM_LENGTH processing added
  • processDataPayload and processPacket handling passes data back through to parent objects
  • comment cleanup
File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/brainstorms/Puzzlebox/Brainstorms/Helicopter_Control.py

    r207 r208  
    4343DEFAULT_SERIAL_BAUDRATE = 115200 
    4444 
    45 PROTOCOL_SYNC_HEAD = '\x80' 
    46 PROTOCOL_SYNC_TAIL = '\xff' 
    47 #PACKET_LENGTH = 10 
     45PROTOCOL_SYNC = '\x80' 
     46PACKET_LENGTH = 10 
     47PAYLOAD_MINIMUM_LENGTH = 8 
    4848 
    4949DEVICE_BUFFER_CHECK_TIMER = 20 # Check buffer size once every 20ms 
     
    5454##################################################################### 
    5555 
    56 #class puzzlebox_brainstorms_helicopter_control: 
    5756class puzzlebox_brainstorms_helicopter_control(QtCore.QThread): 
    5857         
     
    6766                self.log = None 
    6867                self.DEBUG = DEBUG 
     68                self.parent = parent 
    6969                 
    7070                self.device_address = device_address 
     
    126126                                parent=self) 
    127127                 
    128                 print "here" 
    129                  
    130128                self.serial_device.start() 
    131129                 
    132                 print "there" 
    133130                 
    134131                self.protocol = \ 
     
    146143        def processPacket(self, packet): 
    147144                 
    148                 if self.DEBUG > 2: 
    149                         print packet 
    150                  
    151                 if (packet != {}): 
    152                         self.packet_queue.append(packet) 
    153                         self.sendPacketQueue() 
    154                  
    155                 #if (self.parent != None): 
    156                         #self.parent.processPacketThinkGear(self.protocol.data_packet) 
     145                if self.DEBUG: 
     146                        print "data_payload:", 
     147                        print packet['data_payload'].encode("hex") 
     148                 
     149                #if (packet != {}): 
     150                        #self.packet_queue.append(packet) 
     151                        #self.sendPacketQueue() 
     152                 
     153                if (self.parent != None): 
     154                        self.parent.processPacket(self.protocol.data_packet) 
     155         
    157156         
    158157        ################################################################## 
     
    167166        def run(self, command): 
    168167                 
    169                 #if (command == 'drive_forward'): 
    170                         #self.drive_forward(self.connection, power=power, duration=3) 
    171                  
    172                 #elif (command == 'drive_reverse'): 
    173                         #self.drive_reverse(self.connection, power=power) 
    174                  
    175                 #elif (command == 'turn_left'): 
    176                         #self.turn_left(self.connection, power=power) 
    177                  
    178                 #elif (command == 'turn_right'): 
    179                         #self.turn_right(self.connection, power=power) 
    180                  
    181                 #elif (command == 'turn_left_in_reverse'): 
    182                         #self.turn_left_in_reverse(self.connection, power=power) 
    183                  
    184                 #elif (command == 'turn_right_in_reverse'): 
    185                         #self.turn_right_in_reverse(self.connection, power=power) 
    186                  
    187                 #elif (command == 'stop_motors'): 
    188                         #self.stop_motors(self.connection) 
    189                  
    190                 #elif (command == 'test_drive'): 
    191                         #self.test_drive(self.connection) 
    192                  
    193                  
    194                 self.dump_packets() 
     168                #if (command == 'hover'): 
     169                        #self.hover(self.connection, power=power) 
     170                 
     171                #elif (command == 'fly_forward'): 
     172                        #self.fly_forward(self.connection, power=power, duration=3) 
     173                 
     174                 
     175                #self.dump_packets() 
     176                 
     177                pass 
    195178         
    196179         
     
    267250        def processDataPayload(self, data_payload, payload_timestamp): 
    268251         
    269                 if self.DEBUG: 
    270                         print "data_paylod:", 
     252                #if self.DEBUG: 
     253                        #print "data_payload:", 
    271254                        #print data_payload 
    272                         print data_payload.encode("hex") 
     255                        #print data_payload.encode("hex") 
     256                 
     257                 
     258                packet_update = {} 
     259                packet_update['data_payload'] = data_payload 
     260                packet_update['payload_timestamp'] = payload_timestamp 
     261                 
     262                 
     263                if (self.parent != None): 
     264                        self.parent.processPacket(packet_update) 
    273265         
    274266         
     
    276268         
    277269        def parseStream(self): 
    278                  
    279                 '''Each Packet begins with its Header, followed by its Data Payload,  
    280                 and ends with the Payload's Check-sum Byte, as follows: 
    281                 [SYNC] [SYNC] [PLENGTH]      [PAYLOAD...]         [CHKSUM] 
    282                 _______________________      _____________     ____________ 
    283                 ^^^^^^^^(Header)^^^^^^^      ^^(Payload)^^     ^(Checksum)^''' 
    284270                 
    285271                # Loop forever, parsing one Packet per loop... 
     
    289275                         
    290276                        # Synchronize on [SYNC] bytes 
    291                         # Read from stream until two consecutive [SYNC] bytes are found 
    292277                        byte = self.device.read() 
    293                         if (byte != PROTOCOL_SYNC_TAIL): 
    294                                 continue 
    295                          
    296                         byte = self.device.read() 
    297                         if (byte != PROTOCOL_SYNC_HEAD): 
     278                        if (byte != PROTOCOL_SYNC): 
    298279                                continue 
    299280                                 
    300                         #if len(self.device.buffer) < PACKET_LENGTH: 
    301                                 #continue 
    302281                         
    303282                        payload_timestamp = time.time() 
    304                          
    305                         # Parse [PLENGTH] byte 
    306                          
    307                         # SPEC: [PLENGTH] byte indicates the length, in bytes, of the  
    308                         # Packet's Data Payload [PAYLOAD...] section, and may be any value  
    309                         # from 0 up to 169. Any higher value indicates an error  
    310                         # (PLENGTH TOO LARGE). Be sure to note that [PLENGTH] is the length  
    311                         # of the Packet's Data Payload, NOT of the entire Packet.  
    312                         # The Packet's complete length will always be [PLENGTH] + 4. 
    313                          
    314                         #byte = self.device.read() 
    315                         #packet_length = byte.encode("hex") 
    316                         #packet_length = int(packet_length, 16) 
    317                          
    318                         #if (packet_length > 170): 
    319                                 #self.bad_packets += 1 
    320                                 #if self.DEBUG: 
    321                                         #print "ERROR: packet length bad" 
    322                                         #continue 
    323                          
    324                          
    325                         # Collect [PAYLOAD...] bytes 
    326                         #data_payload = self.device.read(packet_length) 
    327283                         
    328284                        data_payload = self.device.readBuffer() 
     
    332288                         
    333289                         
    334                         # Calculate [PAYLOAD...] checksum 
    335                          
    336                         # SPEC: The [CHKSUM] Byte must be used to verify the integrity of the 
    337                         # Packet's Data Payload. The Payload's Checksum is defined as: 
    338                         #  1. summing all the bytes of the Packet's Data Payload 
    339                         #  2. taking the lowest 8 bits of the sum 
    340                         #  3. performing the bit inverse (one's compliment inverse) 
    341                         #     on those lowest 8 bits 
    342                          
    343                         #payload_checksum = 0 
    344                         #for byte in data_payload: 
    345                                 #value = byte.encode("hex") 
    346                                 #value = int(value, 16) 
    347                                 #payload_checksum += value 
    348                          
    349                          
    350                         # Take the lowest 8 bits of the calculated payload_checksum 
    351                         # and invert them. Serious C code mojo follows. 
    352                         #payload_checksum &= 0xff 
    353                         #payload_checksum = ~payload_checksum & 0xff 
    354                          
    355                          
    356                         # Parse [CKSUM] byte 
    357                         #packet_checksum = self.device.read() 
    358                         #packet_checksum = packet_checksum.encode("hex") 
    359                         #packet_checksum = int(packet_checksum, 16) 
    360                          
    361                          
    362                         ## Verify [CKSUM] byte against calculated [PAYLOAD...] checksum 
    363                         #if packet_checksum != payload_checksum: 
    364                                 #self.bad_packets += 1 
    365                                 #if self.DEBUG > 1: 
    366                                         #print "ERROR: packet checksum does not match" 
    367                                         #print "       packet_checksum:", 
    368                                         #print packet_checksum 
    369                                         #print "       payload_checksum:", 
    370                                         #print payload_checksum 
    371                                          
    372                                         ##self.device.checkBuffer() 
    373                                  
    374                                 #continue 
    375                          
    376                          
    377                         #else: 
    378                                 ## Since [CKSUM] is OK, parse the Data Payload 
    379                                 #if self.DEBUG > 3: 
    380                                         #print "packet checksum correct" 
    381                                  
    382                                  
     290                        #print len(data_payload) 
     291                        if len(data_payload) < PAYLOAD_MINIMUM_LENGTH: 
     292                                continue 
     293                         
     294                         
    383295                        self.processDataPayload(data_payload, payload_timestamp) 
    384                                  
    385                                  
    386                                 #if self.DEBUG > 1: 
    387                                         #packet_count += 1 
    388                                         #if packet_count >= DEBUG_PACKET_COUNT: 
    389                                                 #print "max debugging count reached, disconnecting" 
    390                                                 #self.keep_running = False 
    391                                                 #self.device.stop() 
    392                                                 #QtCore.QThread.quit(self) 
    393                                                 ##sys.exit() 
    394          
    395          
    396         ################################################################## 
    397          
    398         def updateDataPacket(self, packet_update): 
    399                  
    400                 if 'eSense' in packet_update.keys(): 
    401                         process_packet = {'eSense': {}} 
    402                         for key in packet_update['eSense'].keys(): 
    403                                 self.data_packet['eSense'][key] = packet_update['eSense'][key] 
    404                                 process_packet['eSense'][key] = packet_update['eSense'][key] 
    405                  
    406                 else: 
    407                         self.data_packet.update(packet_update) 
    408                         process_packet = packet_update 
    409                  
    410                  
    411                 process_packet['timestamp'] = packet_update['timestamp'] 
    412                  
    413                  
    414                 if self.DEBUG > 3: 
    415                         print self.data_packet 
    416                  
    417                  
    418                 if (self.parent != None): 
    419                         self.parent.processPacketThinkGear(process_packet) 
     296                         
     297                         
     298                        #if self.DEBUG > 1: 
     299                                #packet_count += 1 
     300                                #if packet_count >= DEBUG_PACKET_COUNT: 
     301                                        #print "max debugging count reached, disconnecting" 
     302                                        #self.keep_running = False 
     303                                        #self.device.stop() 
     304                                        #QtCore.QThread.quit(self) 
     305                                        ##sys.exit() 
     306         
     307         
     308        ################################################################## 
     309         
     310        #def updateDataPacket(self, packet_update): 
     311                 
     312                #if 'eSense' in packet_update.keys(): 
     313                        #process_packet = {'eSense': {}} 
     314                        #for key in packet_update['eSense'].keys(): 
     315                                #self.data_packet['eSense'][key] = packet_update['eSense'][key] 
     316                                #process_packet['eSense'][key] = packet_update['eSense'][key] 
     317                 
     318                #else: 
     319                        #self.data_packet.update(packet_update) 
     320                        #process_packet = packet_update 
     321                 
     322                 
     323                #process_packet['timestamp'] = packet_update['timestamp'] 
     324                 
     325                 
     326                #if self.DEBUG > 3: 
     327                        #print self.data_packet 
     328                 
     329                 
     330                #if (self.parent != None): 
     331                        #self.parent.processPacket(process_packet) 
    420332         
    421333         
     
    448360##################################################################### 
    449361 
    450 #class SerialDevice(Protocol.SerialDevice): 
    451 #class SerialDevice: 
    452362class SerialDevice(QtCore.QThread): 
    453363         
     
    634544        def read(self, length=1): 
    635545                 
    636                 # Sleep for 2 ms if buffer is empty 
    637                 # Based on 512 Hz refresh rate of NeuroSky MindSet device 
    638                 # (1/512) * 1000 = 1.9531250 
     546                # Sleep for 20 ms if buffer is empty 
     547                # Based on 50 Hz refresh rate of Blade MLP4DSM RC device 
     548                # (1/50) * 1000 = 20 
    639549                while len(self.buffer) < length: 
    640550                        QtCore.QThread.msleep(DEVICE_BUFFER_CHECK_TIMER) 
     
    699609                 
    700610                 
    701                 #self.exitThread(callThreadQuit=False) 
    702611                self.exitThread() 
    703612 
Note: See TracChangeset for help on using the changeset viewer.