Changeset 207 for trunk


Ignore:
Timestamp:
11/15/10 18:09:50 (11 years ago)
Author:
sc
Message:

Brainstorms/Helicopter_Control.py:

  • protocol handling added
  • PROTOCOL_SYNC header and tail matching added
File:
1 edited

Legend:

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

    r206 r207  
    3636##################################################################### 
    3737 
    38 DEBUG = 3 
     38DEBUG = 1 
    3939 
    4040DEFAULT_COMMAND = 'hover' 
     
    4242SERIAL_DEVICE = '/dev/ttyUSB0' 
    4343DEFAULT_SERIAL_BAUDRATE = 115200 
     44 
     45PROTOCOL_SYNC_HEAD = '\x80' 
     46PROTOCOL_SYNC_TAIL = '\xff' 
     47#PACKET_LENGTH = 10 
    4448 
    4549DEVICE_BUFFER_CHECK_TIMER = 20 # Check buffer size once every 20ms 
     
    122126                                parent=self) 
    123127                 
     128                print "here" 
     129                 
    124130                self.serial_device.start() 
    125131                 
    126                 #self.protocol = \ 
    127                         #ProtocolHandler( \ 
    128                                 #self.log, \ 
    129                                 #self.serial_device, \ 
    130                                 #DEBUG=self.DEBUG, \ 
    131                                 #parent=self) 
    132                  
    133                 #self.protocol.start() 
     132                print "there" 
     133                 
     134                self.protocol = \ 
     135                        ProtocolHandler( \ 
     136                                self.log, \ 
     137                                self.serial_device, \ 
     138                                DEBUG=self.DEBUG, \ 
     139                                parent=self) 
     140                 
     141                self.protocol.start() 
    134142         
    135143         
     
    253261                 
    254262                self.keep_running = True 
     263         
     264         
     265        ################################################################## 
     266         
     267        def processDataPayload(self, data_payload, payload_timestamp): 
     268         
     269                if self.DEBUG: 
     270                        print "data_paylod:", 
     271                        #print data_payload 
     272                        print data_payload.encode("hex") 
    255273         
    256274         
     
    273291                        # Read from stream until two consecutive [SYNC] bytes are found 
    274292                        byte = self.device.read() 
    275                         if (byte != PROTOCOL_SYNC): 
     293                        if (byte != PROTOCOL_SYNC_TAIL): 
    276294                                continue 
    277295                         
    278296                        byte = self.device.read() 
    279                         if (byte != PROTOCOL_SYNC): 
     297                        if (byte != PROTOCOL_SYNC_HEAD): 
    280298                                continue 
     299                                 
     300                        #if len(self.device.buffer) < PACKET_LENGTH: 
     301                                #continue 
    281302                         
    282303                        payload_timestamp = time.time() 
     
    291312                        # The Packet's complete length will always be [PLENGTH] + 4. 
    292313                         
    293                         byte = self.device.read() 
    294                         packet_length = byte.encode("hex") 
    295                         packet_length = int(packet_length, 16) 
    296                          
    297                         if (packet_length > 170): 
    298                                 self.bad_packets += 1 
    299                                 if self.DEBUG: 
    300                                         print "ERROR: packet length bad" 
    301                                         continue 
     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 
    302323                         
    303324                         
    304325                        # Collect [PAYLOAD...] bytes 
    305                         data_payload = self.device.read(packet_length) 
     326                        #data_payload = self.device.read(packet_length) 
     327                         
     328                        data_payload = self.device.readBuffer() 
     329                         
     330                        # Remove PROTOCOL_SYNC_TAIL 
     331                        data_payload = data_payload[:-1] 
    306332                         
    307333                         
     
    315341                        #     on those lowest 8 bits 
    316342                         
    317                         payload_checksum = 0 
    318                         for byte in data_payload: 
    319                                 value = byte.encode("hex") 
    320                                 value = int(value, 16) 
    321                                 payload_checksum += value 
     343                        #payload_checksum = 0 
     344                        #for byte in data_payload: 
     345                                #value = byte.encode("hex") 
     346                                #value = int(value, 16) 
     347                                #payload_checksum += value 
    322348                         
    323349                         
    324350                        # Take the lowest 8 bits of the calculated payload_checksum 
    325351                        # and invert them. Serious C code mojo follows. 
    326                         payload_checksum &= 0xff 
    327                         payload_checksum = ~payload_checksum & 0xff 
     352                        #payload_checksum &= 0xff 
     353                        #payload_checksum = ~payload_checksum & 0xff 
    328354                         
    329355                         
    330356                        # Parse [CKSUM] byte 
    331                         packet_checksum = self.device.read() 
    332                         packet_checksum = packet_checksum.encode("hex") 
    333                         packet_checksum = int(packet_checksum, 16) 
    334                          
    335                          
    336                         # Verify [CKSUM] byte against calculated [PAYLOAD...] checksum 
    337                         if packet_checksum != payload_checksum: 
    338                                 self.bad_packets += 1 
    339                                 if self.DEBUG > 1: 
    340                                         print "ERROR: packet checksum does not match" 
    341                                         print "       packet_checksum:", 
    342                                         print packet_checksum 
    343                                         print "       payload_checksum:", 
    344                                         print payload_checksum 
     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 
    345371                                         
    346                                         #self.device.checkBuffer() 
     372                                        ##self.device.checkBuffer() 
    347373                                 
    348                                 continue 
    349                          
    350                          
    351                         else: 
    352                                 # Since [CKSUM] is OK, parse the Data Payload 
    353                                 if self.DEBUG > 3: 
    354                                         print "packet checksum correct" 
     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" 
    355381                                 
    356382                                 
    357                                 self.processDataPayload(data_payload, payload_timestamp) 
     383                        self.processDataPayload(data_payload, payload_timestamp) 
    358384                                 
    359385                                 
     
    587613        ################################################################### 
    588614         
    589         def reset(self): 
     615        def readBuffer(self): 
     616                 
     617                data_payload = self.buffer 
     618                 
     619                self.resetBuffer() 
     620                 
     621                 
     622                return(data_payload) 
     623         
     624         
     625        ################################################################### 
     626         
     627        def resetBuffer(self): 
    590628                 
    591629                self.buffer = '' 
     
    594632        ################################################################### 
    595633         
    596         #def read(self, length=1): 
    597                  
    598                 ## Sleep for 2 ms if buffer is empty 
    599                 ## Based on 512 Hz refresh rate of NeuroSky MindSet device 
    600                 ## (1/512) * 1000 = 1.9531250 
    601                 #while len(self.buffer) < length: 
    602                         #QtCore.QThread.msleep(2) 
    603                          
    604                 #bytes = self.buffer[:length] 
    605                  
    606                 #self.buffer = self.buffer[length:] 
    607                  
    608                 #return(bytes) 
     634        def read(self, length=1): 
     635                 
     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 
     639                while len(self.buffer) < length: 
     640                        QtCore.QThread.msleep(DEVICE_BUFFER_CHECK_TIMER) 
     641                         
     642                bytes = self.buffer[:length] 
     643                 
     644                self.buffer = self.buffer[length:] 
     645                 
     646                return(bytes) 
    609647         
    610648         
     
    637675        ################################################################### 
    638676         
    639         #def run(self): 
    640         def start(self): 
     677        #def start(self): 
     678        def run(self): 
    641679                 
    642680                self.buffer = '' 
Note: See TracChangeset for help on using the changeset viewer.