control.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 from .packet import SUPPORTED_PROTOCOL_VERSION
34 from .packet import PacketType
35 from .packet import PacketDecoder
36 from .packet import PacketFormater
37 
38 from .uart_interface import UartInterface
39 from .chsensors import *
40 
41 from .constants import SPEED_OF_SOUND
42 
43 import time
44 import threading
45 import sys
46 if sys.version_info >= (3, 0):
47  import queue
48  get_tick = time.monotonic
49 else:
50  import Queue as queue
51  get_tick = time.time
52 
53 import copy
54 import functools
55 import struct
56 
57 class PulseConverter:
58  @staticmethod
59  def samples2us(samples, fop):
60  return samples * 1e6 / fop
61  @staticmethod
62  def us2samples(t_us, fop):
63  return int(round(t_us / 1e6 * fop))
64  @staticmethod
65  def samples2cm(samples, fop):
66  return samples / fop * SPEED_OF_SOUND * 100.0
67 
68 class PollerThread(threading.Thread):
69  def __init__(self, do_process, poll_interval=30.0):
70  super(PollerThread, self).__init__(name="_DeviceControlPollerThread")
71  self.exit_flag = threading.Event()
72  self.poll_interval_sec = poll_interval / 1000.0
73  self.do_process = do_process
74 
75  def run(self):
76  while self.exit_flag.is_set() is False:
77  start = get_tick()
78  self.do_process()
79  delta = self.poll_interval_sec - (get_tick() - start)
80  if delta > 0:
81  time.sleep(delta)
82 
83  def stop(self):
84  self.exit_flag.set()
85 
86 
87 class DeviceControl(object):
88  """
89  Public interface for controlling the device.
90  Handlers can be set to process the data from the board.
91  """
92 
93  class EventHandler(object):
94  """
95  Subclass it to catch event and from the device.
96  """
97 
98  def ch_data_handler(self, rx_sensor_id, tx_sensor_id, timestamp, idata, qdata, target_detected, range_cm, amplitude):
99  pass
100 
101  def pd_data_handler(self, sensor_id, timestamp, presence, rmin, rmax, score):
102  pass
103 
104  def imu_data_handler(self, timestamp, acc_x, acc_y, acc_z, gyr_x, gyr_y, gyr_z):
105  pass
106 
107  def grv_data_handler(self, timestamp, grv_w, grv_x, grv_y, grv_z):
108  pass
109 
110  def device_msg_handler(self, s):
111  pass
112 
113  def status_handler(self, timestamp, status, error_count):
114  pass
115 
116  def exception_handler(self, exception):
117  pass
118 
119  def __init__(self, vid_pid=None, com_port='', baudrate=1000000, event_handler=EventHandler()):
120  self.vid_pid = vid_pid
121  self.com_port = com_port
122  self.baudrate = baudrate
123  self.serial = None
124  self.openned = False
125  self.packet_decoder = PacketDecoder()
126  self.init_status = None
127  self.init_done_event = threading.Event()
128  self.event_handlers = {
129  PacketType.ASYNC_CH_DATA: functools.partial(self._user_event_handler, event_handler.ch_data_handler),
130  PacketType.ASYNC_PD_OUT: functools.partial(self._user_event_handler, event_handler.pd_data_handler),
131  PacketType.ASYNC_IMU_DATA: functools.partial(self._user_event_handler, event_handler.imu_data_handler),
132  PacketType.ASYNC_GRV_DATA: functools.partial(self._user_event_handler, event_handler.grv_data_handler),
133  PacketType.ASYNC_STATUS: functools.partial(self._user_event_handler, event_handler.status_handler),
134  PacketType.ASYNC_DEBUG_MESSAGE: functools.partial(self._user_event_handler, event_handler.device_msg_handler),
135  PacketType.RESP_ENABLE_IQSTREAM: functools.partial(self._resp_handler, PacketType.RESP_ENABLE_IQSTREAM),
136  PacketType.RESP_ENABLE_PDALGO: functools.partial(self._resp_handler, PacketType.RESP_ENABLE_PDALGO),
137  PacketType.RESP_ENABLE_IMU: functools.partial(self._resp_handler, PacketType.RESP_ENABLE_IMU),
138  PacketType.RESP_ENABLE_ASIC_DATA: functools.partial(self._resp_handler, PacketType.RESP_ENABLE_ASIC_DATA),
139  PacketType.RESP_GET_ALGO_CONFIG: functools.partial(self._resp_handler, PacketType.RESP_GET_ALGO_CONFIG),
140  PacketType.RESP_GET_PROTOCOL_VERSION: functools.partial(self._resp_handler, PacketType.RESP_GET_PROTOCOL_VERSION),
141  PacketType.RESP_GET_SENSOR_PARAM: functools.partial(self._resp_handler, PacketType.RESP_GET_SENSOR_PARAM),
142  PacketType.RESP_GET_SENSORS: functools.partial(self._resp_handler, PacketType.RESP_GET_SENSORS),
143  PacketType.RESP_GET_VERSION: functools.partial(self._resp_handler, PacketType.RESP_GET_VERSION),
144  PacketType.RESP_GET_STATUS: functools.partial(self._resp_handler, PacketType.RESP_GET_STATUS),
145  PacketType.RESP_SOFT_RESET: functools.partial(self._resp_handler, PacketType.RESP_SOFT_RESET),
146  PacketType.RESP_RESET_PDALGO: functools.partial(self._resp_handler, PacketType.RESP_RESET_PDALGO),
147  PacketType.RESP_RESET_SENSOR: functools.partial(self._resp_handler, PacketType.RESP_RESET_SENSOR),
148  PacketType.RESP_SET_ALGO_CONFIG: functools.partial(self._resp_handler, PacketType.RESP_SET_ALGO_CONFIG),
149  PacketType.RESP_SET_ODR: functools.partial(self._resp_handler, PacketType.RESP_SET_ODR),
150  PacketType.RESP_SET_RANGE_MM: functools.partial(self._resp_handler, PacketType.RESP_SET_RANGE_MM),
151  PacketType.RESP_SET_SAMPLE_RANGE: functools.partial(self._resp_handler, PacketType.RESP_SET_SAMPLE_RANGE),
152  PacketType.RESP_SET_PULSE_LENGTH: functools.partial(self._resp_handler, PacketType.RESP_SET_PULSE_LENGTH),
153  PacketType.RESP_SET_LOW_GAIN_RXLEN: functools.partial(self._resp_handler, PacketType.RESP_SET_LOW_GAIN_RXLEN),
154  PacketType.RESP_START: functools.partial(self._resp_handler, PacketType.RESP_START),
155  PacketType.RESP_STOP: functools.partial(self._resp_handler, PacketType.RESP_STOP),
156  }
157  self.exception_handler = event_handler.exception_handler
158  self.poller_thread = None
159  self.worker_thread = None
160  self.worker_queue = None
161  self.response_event = threading.Event()
162  self.response_pkt = None
163  self.response_data = None
164 
165  def __del__(self):
166  self.close()
167 
168  def open(self, **knargs):
169  try:
170  self._connect()
171  self.checkProtocolVersion()
172  self.reset()
173  self.checkStatus()
174  self.connected_sensors = self._sync_bin_command(PacketType.CMD_GET_SENSORS)
175  self.openned = True
176  except Exception as e:
177  self._disconnect()
178  raise e
179 
180  def close(self):
181  try:
182  self.reset()
183  except:
184  pass
185  finally:
186  self.openned = False
187  self._disconnect()
188 
189  def reset(self):
190  resp = self._sync_bin_command(PacketType.CMD_SOFT_RESET, timeout=6)
191  if resp['status'] != 0:
192  raise Exception("Failed to initialize the board after reset")
193 
194 
195  def checkProtocolVersion(self):
196  resp_data = self._sync_bin_command(PacketType.CMD_GET_PROTOCOL_VERSION)
197  if resp_data['protocol_version'] != SUPPORTED_PROTOCOL_VERSION:
198  raise Exception("Protocol version missmatch (board supports version {}, expected version {})".format(
199  resp_data['protocol_version'], SUPPORTED_PROTOCOL_VERSION))
200 
201  def checkStatus(self):
202  status = self._sync_bin_command(PacketType.CMD_GET_STATUS)['status']
203  if status[0] != 0:
204  #raise Exception(f'Failed to initialize the board, status=0x{status[0]:02x}')
205  raise Exception('Failed to initialize the board, status=' + str(hex(status[0])))
206 
207  def enableIQStream(self):
208  self._sync_bin_command(PacketType.CMD_ENABLE_IQSTREAM, "B", args_tuple = (1,))
209 
210  def disableIQStream(self):
211  self._sync_bin_command(PacketType.CMD_ENABLE_IQSTREAM, "B", args_tuple = (0,))
212 
213  def enableImu(self):
214  self._sync_bin_command(PacketType.CMD_ENABLE_IMU, "B", args_tuple = (1,))
215 
216  def disableImu(self):
217  self._sync_bin_command(PacketType.CMD_ENABLE_IMU, "B", args_tuple = (0,))
218 
219  def enableAsicData(self):
220  self._sync_bin_command(PacketType.CMD_ENABLE_ASIC_DATA, "B", args_tuple = (1,))
221 
222  def disableAsicData(self):
223  self._sync_bin_command(PacketType.CMD_ENABLE_ASIC_DATA, "B", args_tuple = (0,))
224 
225  def getSensorConfig(self, sensor_id):
226  return self._sync_bin_command(PacketType.CMD_GET_SENSOR_PARAM, 'B', args_tuple = (sensor_id,))
227 
228  def getAlgoConfig(self, sensor_id):
229  return self._sync_bin_command(PacketType.CMD_GET_ALGO_CONFIG, "B", args_tuple = (sensor_id,))
230 
231  def setAlgoConfig(self, algo_config):
232  resp = self._sync_bin_command(command=PacketType.CMD_SET_ALGO_CONFIG, code="I"*11, args_tuple=(int(algo_config['sensor_id']),
233  int(algo_config['odr_us']), int(algo_config['nb_samples']), int(algo_config['max_range']),
234  int(algo_config['min_range']), int(algo_config['op_freq_hz']), int(algo_config['sensitivity']),
235  int(algo_config['range_offset']), int(algo_config['range_nb_zones']),
236  int(algo_config['range_hysteresis']), int(algo_config['range_interval'])))
237  #print("Redswallow:SetConfig> ", algo_config)
238  if resp['status'] != 0:
239  raise Exception("Failed to set algo config for sensor {}".format(algo_config['sensor_id']))
240 
241  def setRange(self, range_mm, sensor_id=None):
242  sensor_ids = self.connected_sensors['sensor_ids'] if sensor_id is None else list(sensor_id)
243  for sensor_id in sensor_ids:
244  self._sync_bin_command(command=PacketType.CMD_SET_RANGE_MM, code="BI", args_tuple=(sensor_id, int(range_mm)))
245  algo_config = self.getAlgoConfig(sensor_id)
246  sensor_config = self.getSensorConfig(sensor_id)
247  algo_config['nb_samples'] = sensor_config['nb_samples']
248  self.setAlgoConfig(algo_config)
249 
250  def setSRange(self, nsamples, sensor_id=None):
251  sensor_ids = self.connected_sensors['sensor_ids'] if sensor_id is None else list(sensor_id)
252  for sensor_id in sensor_ids:
253  self._sync_bin_command(command=PacketType.CMD_SET_SAMPLE_RANGE, code="BI", args_tuple=(sensor_id, int(nsamples)))
254  algo_config = self.getAlgoConfig(sensor_id)
255  sensor_config = self.getSensorConfig(sensor_id)
256  algo_config['nb_samples'] = sensor_config['nb_samples']
257  self.setAlgoConfig(algo_config)
258 
259  def startDeviceProcessing(self):
260  self._sync_bin_command(PacketType.CMD_START)
261 
262  def stopDeviceProcessing(self):
263  self._sync_bin_command(PacketType.CMD_STOP)
264 
265  def setODR(self, odr_ms):
266  for sensor_id in self.connected_sensors['sensor_ids']:
267  resp = self._sync_bin_command(PacketType.CMD_SET_ODR, "BI", args_tuple=(sensor_id, int(odr_ms)))
268  if resp['status'] != 0:
269  raise Exception("Failed to set ODR for sensor {}".format(sensor_id))
270  algo_config = self.getAlgoConfig(sensor_id)
271  algo_config['odr_us'] = 1000 * odr_ms
272  self.setAlgoConfig(algo_config)
273 
274  def getInfo(self):
275  device_info = {
276  'versions': {},
277  'nb_sensors': 0,
278  'sensors': {}
279  }
280 
281  device_info['versions'] = self._sync_bin_command(PacketType.CMD_GET_VERSION)
282  connected_sensors = self._sync_bin_command(PacketType.CMD_GET_SENSORS)
283  device_info['nb_sensors'] = connected_sensors['nb_sensors']
284 
285  for sensor_id in connected_sensors['sensor_ids']:
286  sensor_config = self.getSensorConfig(sensor_id)
287  sensor_config['pulse_length_us'] = round(PulseConverter.samples2us(sensor_config['pulse_length'],
288  sensor_config['op_freq_hz']), 2)
289  device_info['sensors'][sensor_id] = {k: v for k, v in sensor_config.items() if k != 'sensor_id' }
290 
291  if 'ch101fw' in device_info['versions']:
292  device_info['chsensor'] = Ch101Sensor()
293  elif 'ch201fw' in device_info['versions']:
294  device_info['chsensor'] = Ch201Sensor()
295  else:
296  device_info['chsensor'] = ChSensor()
297 
298  return device_info
299 
300  def _connect(self):
301  self._disconnect()
302  self.packet_decoder.reset()
303  self.serial = UartInterface.setup(self.vid_pid, self.com_port, self.baudrate)
304  self.worker_queue = queue.Queue()
305  self.worker_thread = threading.Thread(target=self._user_event_worker, name="DeviceControlWorkerThread")
306  self.worker_thread.start()
307  self.poller_thread = PollerThread(do_process=self._read_from_serial, poll_interval=30)
308  self.poller_thread.start()
309  time.sleep(0.5) # Wait for FTDI setup
310 
311  def _disconnect(self):
312  if self.poller_thread:
313  self.poller_thread.stop()
314  self.poller_thread.join()
315  self.poller_thread = None
316 
317  if self.worker_queue:
318  self.worker_queue.put(None)
319  self.worker_queue.join()
320  self.worker_queue = None
321 
322  if self.worker_thread:
323  self.worker_thread.join()
324  self.worker_thread = None
325 
326  try:
327  self.serial.close()
328  del self.serial
329  except Exception:
330  pass
331 
332  def _send_bin_command(self, command, code="", args_tuple = ()):
333  bcmd = PacketFormater.format_cmd_packet(command, code, *args_tuple)
334  #print('Redswallow:TX buffer> ', ' '.join(['{:02x}'.format(b) for b in bcmd]))
335  return self.serial.write(bcmd)
336 
337  def _sync_bin_command(self, command, code="", timeout=2, nb_retry=0, args_tuple = ()):
338  expected_pkt = command | 0x80
339 
340  while True:
341  try:
342  self.response_pkt = None
343  self.response_event.clear()
344  self.response_data = None
345  self._send_bin_command(command, code, args_tuple)
346  if not self.response_event.wait(timeout):
347  raise Exception("Timeout after sending command 0x{:02x}".format(command))
348  if expected_pkt and self.response_pkt != expected_pkt:
349  raise Exception("Unexpected response. Receive 0x{:02x} instead of 0x{:02x}".format(expected_pkt, self.response_pkt))
350  except Exception as e:
351  if nb_retry == 0:
352  raise e
353  nb_retry -= 1
354  else:
355  break
356  return copy.deepcopy(self.response_data)
357 
358  def _read_from_serial(self):
359  try:
360  self.serial.set_mode_nonblocking()
361  payload = self.serial.read(4096)
362  if len(payload) > 0:
363  self.packet_decoder.update(payload, self._pkt_handler)
364  except Exception as e:
365  self.worker_queue.put(self.exception_handler, e)
366  raise e
367 
368  def _pkt_handler(self, pkt_id, pkt_data):
369  handler = self.event_handlers.get(pkt_id, None)
370  if handler:
371  handler(pkt_data)
372 
373  def _init_done_handler(self, event_data):
374  if not self.init_done_event.is_set():
375  self.init_status = event_data['status']
376  self.init_done_event.set()
377 
378  def _resp_handler(self, event_id, event_data):
379  if not self.response_event.is_set():
380  if event_id >= 0x81 and event_id < 0xe0: #If it's a response packet (not an asynchrone one)
381  self.response_pkt = event_id
382  self.response_data = event_data
383  self.response_event.set()
384 
385  def _user_event_handler(self, user_event_cb, event_data):
386  self.worker_queue.put((user_event_cb, event_data, ))
387 
388  def _user_event_worker(self):
389  exit_loop = False
390  while not exit_loop:
391  item = self.worker_queue.get()
392  if item is None:
393  exit_loop = True
394  else:
395  try:
396  item[0](**item[1])
397  except Exception as e:
398  self.exception_handler(e)
399  self.worker_queue.task_done()
400 
redswallow.control.DeviceControl.EventHandler.pd_data_handler
def pd_data_handler(self, sensor_id, timestamp, presence, rmin, rmax, score)
Definition: control.py:101
redswallow.control.DeviceControl.setSRange
def setSRange(self, nsamples, sensor_id=None)
Definition: control.py:250
redswallow.control.DeviceControl.EventHandler.grv_data_handler
def grv_data_handler(self, timestamp, grv_w, grv_x, grv_y, grv_z)
Definition: control.py:107
redswallow.control.DeviceControl.EventHandler.imu_data_handler
def imu_data_handler(self, timestamp, acc_x, acc_y, acc_z, gyr_x, gyr_y, gyr_z)
Definition: control.py:104
redswallow.control.DeviceControl.EventHandler.status_handler
def status_handler(self, timestamp, status, error_count)
Definition: control.py:113
redswallow.control.DeviceControl._user_event_handler
def _user_event_handler(self, user_event_cb, event_data)
Definition: control.py:385
redswallow.control.DeviceControl.open
def open(self, knargs)
Definition: control.py:168
redswallow.control.PulseConverter.samples2cm
def samples2cm(samples, fop)
Definition: control.py:65
redswallow.control.DeviceControl._init_done_handler
def _init_done_handler(self, event_data)
Definition: control.py:373
redswallow.control.PulseConverter.samples2us
def samples2us(samples, fop)
Definition: control.py:59
redswallow.control.PollerThread.__init__
def __init__(self, do_process, poll_interval=30.0)
Definition: control.py:69
redswallow.control.DeviceControl.setRange
def setRange(self, range_mm, sensor_id=None)
Definition: control.py:241
redswallow.control.DeviceControl.__init__
def __init__(self, vid_pid=None, com_port='', baudrate=1000000, event_handler=EventHandler())
Definition: control.py:119
redswallow.control.DeviceControl.setAlgoConfig
def setAlgoConfig(self, algo_config)
Definition: control.py:231
redswallow.control.DeviceControl._sync_bin_command
def _sync_bin_command(self, command, code="", timeout=2, nb_retry=0, args_tuple=())
Definition: control.py:337
redswallow.control.DeviceControl._send_bin_command
def _send_bin_command(self, command, code="", args_tuple=())
Definition: control.py:332
redswallow.control.DeviceControl.EventHandler.ch_data_handler
def ch_data_handler(self, rx_sensor_id, tx_sensor_id, timestamp, idata, qdata, target_detected, range_cm, amplitude)
Definition: control.py:98
redswallow.control.DeviceControl.setODR
def setODR(self, odr_ms)
Definition: control.py:265
redswallow.control.DeviceControl.getSensorConfig
def getSensorConfig(self, sensor_id)
Definition: control.py:225
redswallow.control.DeviceControl._pkt_handler
def _pkt_handler(self, pkt_id, pkt_data)
Definition: control.py:368
redswallow.control.DeviceControl._resp_handler
def _resp_handler(self, event_id, event_data)
Definition: control.py:378
redswallow.control.DeviceControl.getAlgoConfig
def getAlgoConfig(self, sensor_id)
Definition: control.py:228

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