packet.py
Go to the documentation of this file.
1 #
2 # Copyright 2019-2020 TDK Corporation
3 #
4 # Redistribution and use in source and binary forms, with or without
5 # modification, are permitted provided that the following conditions
6 # are met:
7 #
8 # 1. Redistributions of source code must retain the above copyright
9 # notice, this list of conditions and the following disclaimer.
10 #
11 # 2. Redistributions in binary form must reproduce the above
12 # copyright notice, this list of conditions and the following
13 # disclaimer in the documentation and/or other materials provided
14 # with the distribution.
15 #
16 # 3. Neither the name of the copyright holder nor the names of its
17 # contributors may be used to endorse or promote products derived
18 # from this software without specific prior written permission.
19 #
20 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 # COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 # POSSIBILITY OF SUCH DAMAGE.
32 
33 import struct
34 
35 SUPPORTED_PROTOCOL_VERSION = 3
36 
37 class PacketType:
38  RESERVED = 0x00
39 
40  #/* COMMAND packets (HOST -> DEVICE) */
41  CMD_GET_PROTOCOL_VERSION = 0x01
42  CMD_SOFT_RESET = 0x02
43  CMD_RESET_SENSOR = 0x03
44  CMD_RESET_PDALGO = 0x04
45  CMD_ENABLE_PDALGO = 0x05
46  CMD_ENABLE_IQSTREAM = 0x06
47  CMD_START = 0x07
48  CMD_STOP = 0x08
49  CMD_ENABLE_ASIC_DATA = 0x09
50  CMD_ENABLE_IMU = 0x0a
51 
52  CMD_GET_SENSOR_PARAM = 0x20
53  CMD_GET_VERSION = 0x21
54  CMD_GET_SENSORS = 0x22
55  CMD_GET_ALGO_CONFIG = 0x23
56  CMD_GET_STATUS = 0x24
57 
58  CMD_SET_ALGO_CONFIG = 0x30
59  CMD_SET_ODR = 0x31
60  CMD_SET_RANGE_MM = 0x32
61  CMD_SET_SAMPLE_RANGE = 0x33
62  CMD_SET_PULSE_LENGTH = 0x34
63  CMD_SET_LOW_GAIN_RXLEN = 0x35
64 
65  #/* RESPONSE packets (DEVICE -> HOST) */
66  RESP_GET_PROTOCOL_VERSION = 0x81
67  RESP_SOFT_RESET = 0x82
68  RESP_RESET_SENSOR = 0x83
69  RESP_RESET_PDALGO = 0x84
70  RESP_ENABLE_PDALGO = 0x85
71  RESP_ENABLE_IQSTREAM = 0x86
72  RESP_START = 0x87
73  RESP_STOP = 0x88
74  RESP_ENABLE_ASIC_DATA = 0x89
75  RESP_ENABLE_IMU = 0x8a
76 
77  RESP_GET_SENSOR_PARAM = 0xa0
78  RESP_GET_VERSION = 0xa1
79  RESP_GET_SENSORS = 0xa2
80  RESP_GET_ALGO_CONFIG = 0xa3
81  RESP_GET_STATUS = 0xa4
82 
83  RESP_SET_ALGO_CONFIG = 0xb0
84  RESP_SET_ODR = 0xb1
85  RESP_SET_RANGE_MM = 0xb2
86  RESP_SET_SAMPLE_RANGE = 0xb3
87  RESP_SET_PULSE_LENGTH = 0xb4
88  RESP_SET_LOW_GAIN_RXLEN = 0xb5
89 
90  #/* ASYNCRONOUS packets (DEVICE -> HOST) */
91  ASYNC_IMU_DATA = 0xf9
92  ASYNC_CH_DATA = 0xfa
93  ASYNC_PD_OUT = 0xfb
94  ASYNC_STATUS = 0xfc
95  ASYNC_DEBUG_MESSAGE = 0xfd
96  ASYNC_GRV_DATA = 0xfe
97 
98 class PacketDecoder():
99  HEADER = (0x55, 0xaa)
100  FOOTER = (0xbe, 0xef)
101  def __init__(self):
102  self.head = 0
103  self.size = 0
104 
105  self.decoders = {
106  PacketType.RESP_GET_PROTOCOL_VERSION: (self.generic_decoder, (
107  ('protocol_version', 'B', 1, 1, True),
108  )),
109  PacketType.RESP_GET_VERSION: (self.get_version_decoder, None),
110  PacketType.ASYNC_IMU_DATA: (self.generic_decoder, (
111  ('timestamp', 'Q', 8, 1, True),
112  ('acc_x', 'h', 2, 1, True),
113  ('acc_y', 'h', 2, 1, True),
114  ('acc_z', 'h', 2, 1, True),
115  ('gyr_x', 'h', 2, 1, True),
116  ('gyr_y', 'h', 2, 1, True),
117  ('gyr_z', 'h', 2, 1, True),
118  )),
119  PacketType.ASYNC_CH_DATA: (self.chdata_decoder, (
120  ('target_detected', 'I', 4, 1, True),
121  ('range_cm', 'I', 4, 1, True),
122  ('amplitude', 'I', 4, 1, True),
123  )),
124  PacketType.ASYNC_DEBUG_MESSAGE: (self.dmsg_decoder, None),
125  PacketType.ASYNC_PD_OUT: (self.generic_decoder, (
126  ('sensor_id', 'B', 1, 1, True),
127  ('timestamp', 'Q', 8, 1, True),
128  ('presence', 'i', 4, 1, True),
129  ('rmin', 'i', 4, 1, True),
130  ('rmax', 'i', 4, 1, True),
131  ('score', 'f', 4, 1, True),
132  )),
133  PacketType.ASYNC_STATUS: (self.generic_decoder, (
134  ('timestamp', 'Q', 8, 1, True),
135  ('status', 'i', 4, 1, True),
136  ('error_count', 'i', 4, 1, True),
137  )),
138  PacketType.RESP_GET_SENSORS: (self.get_connected_sensors, None),
139  PacketType.RESP_GET_SENSOR_PARAM: (self.generic_decoder, (
140  ('sensor_id', 'L', 4, 1, True),
141  ('op_freq_hz', 'L', 4, 1, True),
142  ('nb_samples', 'L', 4, 1, True),
143  ('odr_ms', 'L', 4, 1, True),
144  ('range_mm', 'L', 4, 1, True),
145  ('pulse_length', 'L', 4, 1, True),
146  ('low_gain_rxlen', 'L', 4, 1, True),
147  )),
148  PacketType.RESP_GET_ALGO_CONFIG: (self.generic_decoder, (
149  ('sensor_id', 'L', 4, 1, True),
150  ('odr_us', 'L', 4, 1, True),
151  ('nb_samples', 'L', 4, 1, True),
152  ('max_range', 'L', 4, 1, True),
153  ('min_range', 'L', 4, 1, True),
154  ('op_freq_hz', 'L', 4, 1, True),
155  ('sensitivity', 'L', 4, 1, True),
156  ('range_offset', 'L', 4, 1, True),
157  ('range_nb_zones', 'L', 4, 1, True),
158  ('range_hysteresis', 'L', 4, 1, True),
159  ('range_interval', 'L', 4, 1, True),
160  )),
161  PacketType.RESP_GET_STATUS: (self.array_decoder, (
162  ('status', 'B', 1, 1, True),
163  )),
164  PacketType.ASYNC_GRV_DATA: [self.generic_decoder, (
165  ('timestamp', 'Q', 8, 1, True),
166  ('grv_w', 'i', 4, 1, True),
167  ('grv_x', 'i', 4, 1, True),
168  ('grv_y', 'i', 4, 1, True),
169  ('grv_z', 'i', 4, 1, True),
170  )],
171  PacketType.RESP_SOFT_RESET: (self.simple_acknowledge, None ),
172  PacketType.RESP_RESET_SENSOR: (self.simple_acknowledge, None ),
173  PacketType.RESP_RESET_PDALGO: (self.simple_acknowledge, None ),
174  PacketType.RESP_ENABLE_PDALGO: (self.simple_acknowledge, None ),
175  PacketType.RESP_ENABLE_IQSTREAM: (self.simple_acknowledge, None ),
176  PacketType.RESP_ENABLE_IMU: (self.simple_acknowledge, None ),
177  PacketType.RESP_ENABLE_ASIC_DATA: (self.simple_acknowledge, None ),
178  PacketType.RESP_START: (self.simple_acknowledge, None ),
179  PacketType.RESP_STOP: (self.simple_acknowledge, None ),
180  PacketType.RESP_SET_ALGO_CONFIG: (self.simple_acknowledge, None ),
181  PacketType.RESP_SET_ODR: (self.simple_acknowledge, None ),
182  PacketType.RESP_SET_RANGE_MM: (self.simple_acknowledge, None ),
183  PacketType.RESP_SET_SAMPLE_RANGE: (self.simple_acknowledge, None ),
184  PacketType.RESP_SET_PULSE_LENGTH: (self.simple_acknowledge, None ),
185  PacketType.RESP_SET_LOW_GAIN_RXLEN: (self.simple_acknowledge, None ),
186  }
187 
188  @property
189  def rsize(self):
190  return (self.size - self.head)
191 
192  def update(self, buffer_in, pkt_handler):
193  if self.rsize:
194  buffer = self.buffer[self.head:] + buffer_in
195  else:
196  buffer = buffer_in
197 
198  #print('Redswallow:RX buffer> ', ' '.join(['{:02x}'.format(b) for b in buffer]))
199 
200  self.buffer = buffer
201  self.head = 0
202  self.size = len(self.buffer)
203 
204  while self.parseInputBuffer(pkt_handler) > 0:
205  pass
206 
207  def reset(self):
208  self.head = 0
209  self.size = 0
210 
211  def parseInputBuffer(self, pkt_handler):
212  FRAME_MIN_SIZE = 2+2+2 # Header, size, footer
213  head0 = self.head
214 
215  if self.rsize >= FRAME_MIN_SIZE:
216  if (tuple(self.buffer[self.head+0:self.head+2]) != self.HEADER):
217  print("Invalid input frame. HEADER missmatch.")
218  while(self.head != self.size):
219  if(tuple(self.buffer[self.head+0:self.head+2]) != self.HEADER):
220  self.head += 1
221  else:
222  break
223  if(self.head == self.size):
224  return -1
225 
226  frameLen = self.buffer[self.head+2] | (self.buffer[self.head+3] << 8)
227  psize = self.head+4+frameLen
228 
229  if self.rsize >= (FRAME_MIN_SIZE+frameLen):
230  if (tuple(self.buffer[self.head+frameLen+4:self.head+frameLen+6]) != self.FOOTER):
231  print("Invalid input frame. FOOTER missmatch.")
232  self.head = self.size
233  return -1
234  else:
235  self.head += 4
236  while (psize-self.head) > 0:
237  if self.parseInputPacket(pkt_handler) <= 0:
238  break
239  self.head += 2
240 
241  return self.head - head0
242 
243  def parseInputPacket(self, pkt_handler):
244  head0 = self.head
245  if self.rsize <= 0:
246  return 0
247 
248  command_code = self.buffer[self.head]
249  self.head += 1
250 
251  decoder, args = self.decoders.get(command_code, (self.unknown_packet_decoder, command_code))
252  ret_dict = decoder(args)
253 
254  pkt_handler(command_code, ret_dict)
255 
256  return self.head - head0
257 
258  def simple_acknowledge(self, *args):
259  status = self.buffer[self.head]
260  self.head += 1
261  return { 'status': status }
262 
263  def generic_decoder(self, decoding_descriptor):
264  ret_dict = {}
265  for key, type, word_size, size, scalar in decoding_descriptor:
266  value = [struct.unpack_from(
267  '<' + type, self.buffer, offset=self.head + i * word_size)[0] for i in range(size)]
268  self.head += size * word_size
269  if key != '_':
270  ret_dict[key] = value[0] if size == 1 and scalar else value
271  return ret_dict
272 
273  def dmsg_decoder(self, decoding_descriptor):
274  data_size = self.buffer[self.head]
275  ret_dict = {'s': self.buffer[self.head+1:self.head+1+data_size]}
276  self.head += 1 + data_size
277  return ret_dict
278 
279  def array_decoder(self, decoding_descriptor):
280  ret_dict = {}
281 
282  key, type, word_size, _, _ = decoding_descriptor[0]
283  size = self.buffer[self.head]
284 
285  ret_dict[key] = [struct.unpack_from('<' + type, self.buffer, offset=self.head+1+i*word_size)[0] for i in range(size)]
286 
287  self.head += size + 1
288  return ret_dict
289 
290  def chdata_decoder(self, asic_decoding_descriptor):
291  rx_sensor_id = struct.unpack("<B", self.buffer[self.head+0: self.head+1])[0]
292  tx_sensor_id = struct.unpack("<B", self.buffer[self.head+1: self.head+2])[0]
293  timestamp = struct.unpack("<Q", self.buffer[self.head+2: self.head+10])[0]
294  flags = struct.unpack("<B", self.buffer[self.head+10: self.head+11])[0]
295  self.head += 11
296 
297  if flags & 0x01:
298  data_size = struct.unpack("<h", self.buffer[self.head+0: self.head+2])[0]
299  iq_bytes = self.buffer[self.head+2:self.head+2+data_size]
300  qdata = [struct.unpack("<h", x[0:2])[0] for x in [iq_bytes[pos:pos + 4] for pos in range(0, len(iq_bytes), 4)]]
301  idata = [struct.unpack("<h", x[2:4])[0] for x in [iq_bytes[pos:pos + 4] for pos in range(0, len(iq_bytes), 4)]]
302  self.head += 2 + data_size
303  else:
304  qdata = []
305  idata = []
306 
307  if flags & 0x02:
308  ret_dict = self.generic_decoder(asic_decoding_descriptor)
309  else:
310  ret_dict = {key: -1 for key, _, _, _, _ in asic_decoding_descriptor if key != '_'}
311 
312  if ret_dict['target_detected'] == 0:
313  ret_dict['range_cm'] = 0
314  ret_dict['amplitude'] = 0
315  else:
316  ret_dict['range_cm'] = round(ret_dict['range_cm'] / 32 / 10, 1)
317 
318  ret_dict.update({
319  'rx_sensor_id': rx_sensor_id,
320  'tx_sensor_id': tx_sensor_id,
321  'timestamp': timestamp,
322  'idata': idata,
323  'qdata': qdata,
324  })
325 
326  return ret_dict
327 
328  def get_version_decoder(self, decoding_descriptor):
329  nb_version, = struct.unpack_from('B', self.buffer, self.head)
330  self.head += 1
331 
332  ret_dict = {}
333 
334  for i in range(nb_version):
335  slabel, sversion = struct.unpack_from('<BB', self.buffer, self.head)
336  self.head += 2
337  label, version = struct.unpack_from('<{}sx{}sx'.format(slabel-1, sversion-1), self.buffer, self.head)
338  self.head += slabel + sversion
339  ret_dict[label.decode('ascii')] = version.decode('ascii')
340 
341  return ret_dict
342 
343  def get_connected_sensors(self, decoding_descriptor):
344  nb_sensors, = struct.unpack_from('<B', self.buffer, self.head)
345  ret_dict = {'nb_sensors': nb_sensors }
346  ret_dict['sensor_ids'] = struct.unpack_from(
347  '{}B'.format(nb_sensors), self.buffer, self.head+1)
348  self.head += (nb_sensors + 1)
349  return ret_dict
350 
351  def unknown_packet_decoder(self, command_code):
352  print("Unknown packet", hex(command_code))
353  self.head = self.size - 2
354 
355 
356 class PacketFormater:
357  @staticmethod
358  def format_cmd_packet(cmd, cmd_args_code="", *cmd_args):
359  agrs_size = struct.calcsize("<B" + cmd_args_code)
360  lcmd = [0x55, 0xaa, agrs_size, cmd ] + list(cmd_args) + [0xbe, 0xef]
361  bcmd = struct.pack("<BBHB" + cmd_args_code + "BB", *lcmd)
362  return bcmd
redswallow.packet.PacketDecoder.update
def update(self, buffer_in, pkt_handler)
Definition: packet.py:192
redswallow.packet.PacketDecoder.chdata_decoder
def chdata_decoder(self, asic_decoding_descriptor)
Definition: packet.py:290
redswallow.packet.PacketDecoder.array_decoder
def array_decoder(self, decoding_descriptor)
Definition: packet.py:279
redswallow.packet.PacketDecoder.parseInputBuffer
def parseInputBuffer(self, pkt_handler)
Definition: packet.py:211
redswallow.packet.PacketDecoder.parseInputPacket
def parseInputPacket(self, pkt_handler)
Definition: packet.py:243
redswallow.packet.PacketDecoder.get_version_decoder
def get_version_decoder(self, decoding_descriptor)
Definition: packet.py:328
redswallow.packet.PacketDecoder.simple_acknowledge
def simple_acknowledge(self, args)
Definition: packet.py:258
redswallow.packet.PacketDecoder.get_connected_sensors
def get_connected_sensors(self, decoding_descriptor)
Definition: packet.py:343
redswallow.packet.PacketFormater.format_cmd_packet
def format_cmd_packet(cmd, cmd_args_code="", cmd_args)
Definition: packet.py:358
redswallow.packet.PacketDecoder.generic_decoder
def generic_decoder(self, decoding_descriptor)
Definition: packet.py:263
redswallow.packet.PacketDecoder.unknown_packet_decoder
def unknown_packet_decoder(self, command_code)
Definition: packet.py:351
redswallow.packet.PacketDecoder.dmsg_decoder
def dmsg_decoder(self, decoding_descriptor)
Definition: packet.py:273

tdk_robokit
Author(s): TDK-OpenSource
autogenerated on Sat Jan 11 2020 03:18:39