python_can.py

Go to the documentation of this file.

#
# Copyright (C) 2019 UAVCAN Development Team  <uavcan.org>
#
# This software is distributed under the terms of the MIT License.
#
# Author: Jos Vos <jos.vos@demcon.nl>
#
 
from __future__ import division, absolute_import, print_function, unicode_literals
import os
import errno
import time
import threading
from logging import getLogger
 
from .common import DriverError, TxQueueFullError, CANFrame, AbstractDriver
from .timestamp_estimator import TimestampEstimator
 
try:
    import queue
except ImportError:
    # noinspection PyPep8Naming,PyUnresolvedReferences
    import Queue as queue
 
logger = getLogger(__name__)
 
try:
    import can
except ImportError:
    PythonCAN = None
else:
    class PythonCAN(AbstractDriver):
        TX_QUEUE_SIZE = 1000
 
        def __init__(self, channel, **_extras):
            super(PythonCAN, self).__init__()
 
            try:
                if channel is None:
                    self._bus = can.interface.Bus() # get bus from environment's config file
                else:
                    self._bus = can.interface.Bus(channel=channel, bustype=_extras['bustype'], bitrate=_extras['bitrate'])
            except Exception as ex:
                logger.exception("Could not instantiate a python-can driver")
                raise
 
            self._writer_thread_should_stop = False
            self._write_queue = queue.Queue(self.TX_QUEUE_SIZE)
            self._write_feedback_queue = queue.Queue()
 
            self._writer_thread = threading.Thread(
                target=self._writer_thread_loop, name="pythoncan_writer"
            )
            self._writer_thread.daemon = True
            self._writer_thread.start()
 
            ppm = lambda x: x / 1e6
            milliseconds = lambda x: x * 1e-3
 
            # We're using this thing to estimate the difference between monotonic and real clocks
            # See http://stackoverflow.com/questions/35426864 (at the time of writing the question was unanswered)
            self._mono_to_real_estimator = TimestampEstimator(
                max_rate_error=ppm(100),
                fixed_delay=milliseconds(0.001),
                max_phase_error_to_resync=milliseconds(50),
            )
 
        def _convert_real_to_monotonic(self, value):
            mono = time.monotonic()  # est_real is the best guess about real timestamp here
            real = time.time()
            est_real = self._mono_to_real_estimator.update(mono, real)
            mono_to_real_offset = est_real - mono
            return value - mono_to_real_offset
 
        def _writer_thread_loop(self):
            while not self._writer_thread_should_stop:
                try:
                    frame = self._write_queue.get(timeout=0.1)
                except queue.Empty:
                    continue
 
                try:
                    while not self._writer_thread_should_stop:
                        try:
                            msg = can.Message(
                                arbitration_id=frame.id,
                                extended_id=frame.extended,
                                dlc=len(frame.data),
                                data=list(frame.data),
                            )
                            self._bus.send(msg)
                            self._bus.flush_tx_buffer()
 
                            frame.ts_monotonic = time.monotonic()
                            frame.ts_real = time.time()
                            self._write_feedback_queue.put(frame)
                        except OSError as ex:
                            if ex.errno == errno.ENOBUFS:
                                time.sleep(0.0002)
                            else:
                                raise
                        else:
                            break
                except Exception as ex:
                    self._write_feedback_queue.put(ex)
 
        def _check_write_feedback(self):
            try:
                item = self._write_feedback_queue.get_nowait()
            except queue.Empty:
                return
 
            if isinstance(item, Exception):
                raise item
 
            if isinstance(item, CANFrame):
                self._tx_hook(item)
            else:
                raise DriverError("Unexpected item in write feedback queue: %r" % item)
 
        def close(self):
            self._writer_thread_should_stop = True
            self._writer_thread.join()
            self._bus.shutdown()
 
        def receive(self, timeout=None):
            self._check_write_feedback()
 
            timeout = -1 if timeout is None else (timeout * 1000)
 
            try:
                msg = self._bus.recv(timeout=timeout)
                if msg is not None:
                    ts_mono = time.monotonic()
                    ts_real = time.time()
 
                    if ts_real and not ts_mono:
                        ts_mono = self._convert_real_to_monotonic(ts_real)
 
                    # timestamps are ugly still
                    frame = CANFrame(
                        msg.arbitration_id,
                        msg.data,
                        True,
                        ts_monotonic=ts_mono,
                        ts_real=ts_real,
                    )
                    self._rx_hook(frame)
                    return frame
            except Exception as ex:
                logger.error("Receive exception", exc_info=True)
 
        def send(self, message_id, message, extended=False):
            self._check_write_feedback()
            try:
                self._write_queue.put_nowait(CANFrame(message_id, message, extended))
            except queue.Full:
                raise TxQueueFullError()