python_can.py
Go to the documentation of this file.
1 #
2 # Copyright (C) 2019 UAVCAN Development Team <uavcan.org>
3 #
4 # This software is distributed under the terms of the MIT License.
5 #
6 # Author: Jos Vos <jos.vos@demcon.nl>
7 #
8 
9 from __future__ import division, absolute_import, print_function, unicode_literals
10 import os
11 import errno
12 import time
13 import threading
14 from logging import getLogger
15 
16 from .common import DriverError, TxQueueFullError, CANFrame, AbstractDriver
17 from .timestamp_estimator import TimestampEstimator
18 
19 try:
20  import queue
21 except ImportError:
22  # noinspection PyPep8Naming,PyUnresolvedReferences
23  import Queue as queue
24 
25 logger = getLogger(__name__)
26 
27 try:
28  import can
29 except ImportError:
30  PythonCAN = None
31 else:
33  TX_QUEUE_SIZE = 1000
34 
35  def __init__(self, channel, **_extras):
36  super(PythonCAN, self).__init__()
37 
38  try:
39  if channel is None:
40  self._bus = can.interface.Bus() # get bus from environment's config file
41  else:
42  self._bus = can.interface.Bus(channel=channel, bustype=_extras['bustype'], bitrate=_extras['bitrate'])
43  except Exception as ex:
44  logger.exception("Could not instantiate a python-can driver")
45  raise
46 
48  self._write_queue = queue.Queue(self.TX_QUEUE_SIZE)
49  self._write_feedback_queue = queue.Queue()
50 
51  self._writer_thread = threading.Thread(
52  target=self._writer_thread_loop, name="pythoncan_writer"
53  )
54  self._writer_thread.daemon = True
55  self._writer_thread.start()
56 
57  ppm = lambda x: x / 1e6
58  milliseconds = lambda x: x * 1e-3
59 
60  # We're using this thing to estimate the difference between monotonic and real clocks
61  # See http://stackoverflow.com/questions/35426864 (at the time of writing the question was unanswered)
63  max_rate_error=ppm(100),
64  fixed_delay=milliseconds(0.001),
65  max_phase_error_to_resync=milliseconds(50),
66  )
67 
68  def _convert_real_to_monotonic(self, value):
69  mono = time.monotonic() # est_real is the best guess about real timestamp here
70  real = time.time()
71  est_real = self._mono_to_real_estimator.update(mono, real)
72  mono_to_real_offset = est_real - mono
73  return value - mono_to_real_offset
74 
76  while not self._writer_thread_should_stop:
77  try:
78  frame = self._write_queue.get(timeout=0.1)
79  except queue.Empty:
80  continue
81 
82  try:
83  while not self._writer_thread_should_stop:
84  try:
85  msg = can.Message(
86  arbitration_id=frame.id,
87  extended_id=frame.extended,
88  dlc=len(frame.data),
89  data=list(frame.data),
90  )
91  self._bus.send(msg)
92  self._bus.flush_tx_buffer()
93 
94  frame.ts_monotonic = time.monotonic()
95  frame.ts_real = time.time()
96  self._write_feedback_queue.put(frame)
97  except OSError as ex:
98  if ex.errno == errno.ENOBUFS:
99  time.sleep(0.0002)
100  else:
101  raise
102  else:
103  break
104  except Exception as ex:
105  self._write_feedback_queue.put(ex)
106 
108  try:
109  item = self._write_feedback_queue.get_nowait()
110  except queue.Empty:
111  return
112 
113  if isinstance(item, Exception):
114  raise item
115 
116  if isinstance(item, CANFrame):
117  self._tx_hook(item)
118  else:
119  raise DriverError("Unexpected item in write feedback queue: %r" % item)
120 
121  def close(self):
122  self._writer_thread_should_stop = True
123  self._writer_thread.join()
124  self._bus.shutdown()
125 
126  def receive(self, timeout=None):
127  self._check_write_feedback()
128 
129  timeout = -1 if timeout is None else (timeout * 1000)
130 
131  try:
132  msg = self._bus.recv(timeout=timeout)
133  if msg is not None:
134  ts_mono = time.monotonic()
135  ts_real = time.time()
136 
137  if ts_real and not ts_mono:
138  ts_mono = self._convert_real_to_monotonic(ts_real)
139 
140  # timestamps are ugly still
141  frame = CANFrame(
142  msg.arbitration_id,
143  msg.data,
144  True,
145  ts_monotonic=ts_mono,
146  ts_real=ts_real,
147  )
148  self._rx_hook(frame)
149  return frame
150  except Exception as ex:
151  logger.error("Receive exception", exc_info=True)
152 
153  def send(self, message_id, message, extended=False):
154  self._check_write_feedback()
155  try:
156  self._write_queue.put_nowait(CANFrame(message_id, message, extended))
157  except queue.Full:
158  raise TxQueueFullError()
def __init__(self, channel, _extras)
Definition: python_can.py:35
def send(self, message_id, message, extended=False)
Definition: python_can.py:153
def _convert_real_to_monotonic(self, value)
Definition: python_can.py:68
def receive(self, timeout=None)
Definition: python_can.py:126


uavcan_communicator
Author(s):
autogenerated on Wed Jan 11 2023 03:59:39