Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- def recv_messages( self ):
- # read updated write-pointer
- widx = self.ddr_widx.value
- assert widx < self.NUM_MSGS # sanity-check
- if widx == self.ridx:
- return # nothing to do
- txdata = ''
- # process batch of messages
- # note: it may be faster to copy a batch of messages from shared memory
- # instead of directly accessing individual messages and their fields.
- while self.ridx != widx:
- msg = self.msgbuf[ self.ridx ]
- # sanity-check that message id increments monotonically
- # (i.e. no messages from pru got dropped)
- dropped = ( msg.id - self.lastid - 1 ) & 0xffffffff
- if dropped != 0:
- raise RuntimeError( f'{dropped} messages dropped by PRU (0x{self.lastid:08x} -> 0x{msg.id:08x})' )
- self.lastid = msg.id
- # get 32-bit timestamp (in cycles) from message and unwrap it:
- self.timestamp_cycles += ( msg.timestamp - self.timestamp_cycles ) & 0xfffffff
- position = msg.position
- angle = self._Count_to_angle( position )
- force_adj = self._Force_correction( angle )
- force = msg.force - force_adj
- txmsg = [ self.timestamp_cycles, force, angle ]
- txdata += map( str, txmsg ).join(',') + '\n'
- # or just: txdata += f'{self.timestamp_cycles},{force},{angle}\n'
- # consume message and update read pointer
- self.ridx += 1
- if (self.ridx == self.NUM_MSGS):
- self.ridx = 0
- del msg
- self.shmem.ridx = self.ridx # direct access to message forbidden beyond this point
- # send data to client
- self.writer.write( txdata.encode('ascii') )
- # TODO maybe bail out with an error if too much data buffered in the writer?
- # you can check with writer.get_write_buffer_size()
- # update user interface
- timestamp_ms = self.timestamp_cycles // 200000
- timestamp_s = ( timestamp_ms % 60000 ) / 1000
- timestamp_m = timestamp_ms // 60000
- timestamp_h = timestamp_m // 60
- timestamp_m = timestamp_m % 60
- timestamp_label = f'{timestamp_h:02d}:{timestamp_m:02d}:{timestamp_s:06.3f}'
- 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