Advertisement
Guest User

Untitled

a guest
Dec 9th, 2021
101
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 2.44 KB | None | 0 0
  1.     def recv_messages( self ):
  2.  
  3.         # read updated write-pointer
  4.         widx = self.ddr_widx.value
  5.         assert widx < self.NUM_MSGS  # sanity-check
  6.  
  7.         if widx == self.ridx:
  8.             return  # nothing to do
  9.  
  10.  
  11.         txdata = ''
  12.  
  13.         # process batch of messages
  14.  
  15.         # note: it may be faster to copy a batch of messages from shared memory
  16.         # instead of directly accessing individual messages and their fields.
  17.         while self.ridx != widx:
  18.             msg = self.msgbuf[ self.ridx ]
  19.  
  20.             # sanity-check that message id increments monotonically
  21.             # (i.e. no messages from pru got dropped)
  22.             dropped = ( msg.id - self.lastid - 1 ) & 0xffffffff
  23.             if dropped != 0:
  24.                 raise RuntimeError( f'{dropped} messages dropped by PRU (0x{self.lastid:08x} -> 0x{msg.id:08x})' )
  25.             self.lastid = msg.id
  26.  
  27.             # get 32-bit timestamp (in cycles) from message and unwrap it:
  28.             self.timestamp_cycles += ( msg.timestamp - self.timestamp_cycles ) & 0xfffffff
  29.  
  30.             position = msg.position
  31.             angle = self._Count_to_angle( position )
  32.             force_adj = self._Force_correction( angle )
  33.             force = msg.force - force_adj
  34.  
  35.             txmsg = [ self.timestamp_cycles, force, angle ]
  36.             txdata += map( str, txmsg ).join(',') + '\n'
  37.             # or just:  txdata += f'{self.timestamp_cycles},{force},{angle}\n'
  38.  
  39.             # consume message and update read pointer
  40.             self.ridx += 1
  41.             if (self.ridx == self.NUM_MSGS):
  42.                 self.ridx = 0
  43.             del msg
  44.             self.shmem.ridx = self.ridx  # direct access to message forbidden beyond this point
  45.  
  46.  
  47.         # send data to client
  48.         self.writer.write( txdata.encode('ascii') )
  49.  
  50.         # TODO maybe bail out with an error if too much data buffered in the writer?
  51.         # you can check with writer.get_write_buffer_size()
  52.  
  53.  
  54.         # update user interface
  55.         timestamp_ms = self.timestamp_cycles // 200000
  56.         timestamp_s = ( timestamp_ms % 60000 ) / 1000
  57.         timestamp_m = timestamp_ms // 60000
  58.         timestamp_h = timestamp_m // 60
  59.         timestamp_m = timestamp_m % 60
  60.         timestamp_label = f'{timestamp_h:02d}:{timestamp_m:02d}:{timestamp_s:06.3f}'
  61.         print( f'\ridx=0x{self.ridx:04x} id=0x{self.lastid:08x} time={timestamp_label} angle={angle} force={force:08d}', end='\x1b[K', flush=True )
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement