Tesla Model S support

common/dbc-0.35.py

@@ -0,0 +1,195 @@
+import re
+import os
+import struct
+import bitstring
+from collections import namedtuple
+
+def int_or_float(s):
+  # return number, trying to maintain int format
+  try:
+    return int(s)
+  except ValueError:
+    return float(s)
+
+DBCSignal = namedtuple(
+  "DBCSignal", ["name", "start_bit", "size", "is_little_endian", "is_signed",
+                "factor", "offset", "tmin", "tmax", "units"])
+
+class dbc(object):
+  def __init__(self, fn):
+    self.name, _ = os.path.splitext(os.path.basename(fn))
+    with open(fn) as f:
+      self.txt = f.read().split("\n")
+    self._warned_addresses = set()
+
+    # regexps from https://github.com/ebroecker/canmatrix/blob/master/canmatrix/importdbc.py
+    bo_regexp = re.compile("^BO\_ (\w+) (\w+) *: (\w+) (\w+)")
+    sg_regexp = re.compile("^SG\_ (\w+) : (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
+    sgm_regexp = re.compile("^SG\_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)")
+
+    # A dictionary which maps message ids to tuples ((name, size), signals).
+    #   name is the ASCII name of the message.
+    #   size is the size of the message in bytes.
+    #   signals is a list signals contained in the message.
+    # signals is a list of DBCSignal in order of increasing start_bit.
+    self.msgs = {}
+
+    # lookup to bit reverse each byte
+    self.bits_index = [(i & ~0b111) + ((-i-1) & 0b111) for i in xrange(64)]
+
+    for l in self.txt:
+      l = l.strip()
+
+      if l.startswith("BO_ "):
+        # new group
+        dat = bo_regexp.match(l)
+
+        if dat is None:
+          print "bad BO", l
+        name = dat.group(2)
+        size = int(dat.group(3))
+        ids = int(dat.group(1), 0) # could be hex
+
+        self.msgs[ids] = ((name, size), [])
+
+      if l.startswith("SG_ "):
+        # new signal
+        dat = sg_regexp.match(l)
+        go = 0
+        if dat is None:
+          dat = sgm_regexp.match(l)
+          go = 1
+        if dat is None:
+          print "bad SG", l
+
+        sgname = dat.group(1)
+        start_bit = int(dat.group(go+2))
+        signal_size = int(dat.group(go+3))
+        is_little_endian = int(dat.group(go+4))==1
+        is_signed = dat.group(go+5)=='-'
+        factor = int_or_float(dat.group(go+6))
+        offset = int_or_float(dat.group(go+7))
+        tmin = int_or_float(dat.group(go+8))
+        tmax = int_or_float(dat.group(go+9))
+        units = dat.group(go+10)
+
+        self.msgs[ids][1].append(
+          DBCSignal(sgname, start_bit, signal_size, is_little_endian,
+                    is_signed, factor, offset, tmin, tmax, units))
+
+    for msg in self.msgs.viewvalues():
+      msg[1].sort(key=lambda x: x.start_bit)
+
+  def encode(self, msg_id, dd):
+    """Encode a CAN message using the dbc.
+
+       Inputs:
+        msg_id: The message ID.
+        dd: A dictionary mapping signal name to signal data.
+    """
+    # TODO: Stop using bitstring, which is super slow.
+    msg_def = self.msgs[msg_id]
+    size = msg_def[0][1]
+
+    bsf = bitstring.Bits(hex="00"*size)
+    for s in msg_def[1]:
+      ival = dd.get(s.name)
+      if ival is not None:
+        ival = (ival / s.factor) - s.offset
+        ival = int(round(ival))
+
+        # should pack this
+        if s.is_little_endian:
+          ss = s.start_bit
+        else:
+          ss = self.bits_index[s.start_bit]
+
+
+        if s.is_signed:
+          tbs = bitstring.Bits(int=ival, length=s.size)
+        else:
+          tbs = bitstring.Bits(uint=ival, length=s.size)
+
+        lpad = bitstring.Bits(bin="0b"+"0"*ss)
+        rpad = bitstring.Bits(bin="0b"+"0"*(8*size-(ss+s.size)))
+        tbs = lpad+tbs+rpad
+
+        bsf |= tbs
+    return bsf.tobytes()
+
+  def decode(self, x, arr=None, debug=False):
+    """Decode a CAN message using the dbc.
+
+       Inputs:
+        x: A collection with elements (address, time, data), where address is
+           the CAN address, time is the bus time, and data is the CAN data as a
+           hex string.
+        arr: Optional list of signals which should be decoded and returned.
+        debug: True to print debugging statements.
+
+       Returns:
+        A tuple (name, data), where name is the name of the CAN message and data
+        is the decoded result. If arr is None, data is a dict of properties.
+        Otherwise data is a list of the same length as arr.
+
+        Returns (None, None) if the message could not be decoded.
+    """
+
+    if arr is None:
+      out = {}
+    else:
+      out = [None]*len(arr)
+
+    msg = self.msgs.get(x[0])
+    if msg is None:
+      if x[0] not in self._warned_addresses:
+        print("WARNING: Unknown message address {}".format(x[0]))
+        self._warned_addresses.add(x[0])
+      return None, None
+
+    name = msg[0][0]
+    if debug:
+      print name
+
+    blen = 8*len(x[2])
+
+    st = x[2].rjust(8, '\x00')
+    le, be = None, None
+
+    for s in msg[1]:
+      if arr is not None and s[0] not in arr:
+        continue
+
+      # big or little endian?
+      #   see http://vi-firmware.openxcplatform.com/en/master/config/bit-numbering.html
+      if s[3] is False:
+        ss = self.bits_index[s[1]]
+        if be is None:
+          be = struct.unpack(">Q", st)[0]
+        x2_int = be
+        data_bit_pos = (blen - (ss + s[2]))
+      else:
+        if le is None:
+          le = struct.unpack("<Q", st)[0]
+        x2_int = le
+        ss = s[1]
+        data_bit_pos = ss
+
+      if data_bit_pos < 0:
+        continue
+      ival = (x2_int >> data_bit_pos) & ((1 << (s[2])) - 1)
+
+      if s[4] and (ival & (1<<(s[2]-1))): # signed
+        ival -= (1<<s[2])
+
+      # control the offset
+      ival = (ival * s[5]) + s[6]
+      #if debug:
+      #  print "%40s  %2d %2d  %7.2f %s" % (s[0], s[1], s[2], ival, s[-1])
+
+      if arr is None:
+        out[s[0]] = ival
+      else:
+        out[arr.index(s[0])] = ival
+    return name, out
+

common/dbc.py

@@ -1,7 +1,6 @@
 import re
-import os
-import struct
 import bitstring
+from binascii import hexlify
 from collections import namedtuple
 
 def int_or_float(s):
@@ -17,7 +16,6 @@ def int_or_float(s):
 
 class dbc(object):
   def __init__(self, fn):
-    self.name, _ = os.path.splitext(os.path.basename(fn))
     with open(fn) as f:
       self.txt = f.read().split("\n")
     self._warned_addresses = set()
@@ -34,8 +32,10 @@ def __init__(self, fn):
     # signals is a list of DBCSignal in order of increasing start_bit.
     self.msgs = {}
 
-    # lookup to bit reverse each byte
-    self.bits_index = [(i & ~0b111) + ((-i-1) & 0b111) for i in xrange(64)]
+    self.bits = []
+    for i in range(0, 64, 8):
+      for j in range(7, -1, -1):
+        self.bits.append(i+j)
 
     for l in self.txt:
       l = l.strip()
@@ -102,7 +102,7 @@ def encode(self, msg_id, dd):
         if s.is_little_endian:
           ss = s.start_bit
         else:
-          ss = self.bits_index[s.start_bit]
+          ss = self.bits.index(s.start_bit)
 
 
         if s.is_signed:
@@ -135,6 +135,9 @@ def decode(self, x, arr=None, debug=False):
         Returns (None, None) if the message could not be decoded.
     """
 
+    def swap_order(d, wsz=4, gsz=2 ):
+      return "".join(["".join([m[i:i+gsz] for i in range(wsz-gsz,-gsz,-gsz)]) for m in [d[i:i+wsz] for i in range(0,len(d),wsz)]])
+
     if arr is None:
       out = {}
     else:
@@ -153,28 +156,22 @@ def decode(self, x, arr=None, debug=False):
 
     blen = 8*len(x[2])
 
-    st = x[2].rjust(8, '\x00')
-    le, be = None, None
-
     for s in msg[1]:
       if arr is not None and s[0] not in arr:
         continue
 
       # big or little endian?
       #   see http://vi-firmware.openxcplatform.com/en/master/config/bit-numbering.html
       if s[3] is False:
-        ss = self.bits_index[s[1]]
-        if be is None:
-          be = struct.unpack(">Q", st)[0]
-        x2_int = be
+        ss = self.bits.index(s[1])
+        x2_int = int(hexlify(x[2]), 16)
         data_bit_pos = (blen - (ss + s[2]))
       else:
-        if le is None:
-          le = struct.unpack("<Q", st)[0]
-        x2_int = le
+        x2_int = int(swap_order(hexlify(x[2]), 16, 2), 16)
         ss = s[1]
         data_bit_pos = ss
 
+
       if data_bit_pos < 0:
         continue
       ival = (x2_int >> data_bit_pos) & ((1 << (s[2])) - 1)

common/fingerprints.py

@@ -61,22 +61,25 @@ def fingerprint(logcan):
   print "waiting for fingerprint..."
   candidate_cars = all_known_cars()
   finger = {}
-  st = None
-  while 1:
-    for a in messaging.drain_sock(logcan, wait_for_one=True):
-      if st is None:
-        st = sec_since_boot()
-      for can in a.can:
-        if can.src == 0:
-          finger[can.address] = len(can.dat)
-        candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
+
+  #st = None
+  #while 1:
+  #  for a in messaging.drain_sock(logcan, wait_for_one=True):
+  #    if st is None:
+  #      st = sec_since_boot()
+  #    for can in a.can:
+  #      if can.src == 0:
+  #        finger[can.address] = len(can.dat)
+  #      candidate_cars = eliminate_incompatible_cars(can, candidate_cars)
 
     # if we only have one car choice and it's been 100ms since we got our first message, exit
-    if len(candidate_cars) == 1 and st is not None and (sec_since_boot()-st) > 0.1:
-      break
-    elif len(candidate_cars) == 0:
-      print map(hex, finger.keys())
-      raise Exception("car doesn't match any fingerprints")
+  #  if len(candidate_cars) == 1 and st is not None and (sec_since_boot()-st) > 0.1:
+  #    break
+  #  elif len(candidate_cars) == 0:
+  #    print map(hex, finger.keys())
+  #    raise Exception("car doesn't match any fingerprints")
 
-  print "fingerprinted", candidate_cars[0]
-  return (candidate_cars[0], finger)
+  #print "fingerprinted", candidate_cars[0]
+
+  #return (candidate_cars[0], finger)
+  return ("TESLA CLASSIC MODEL S", finger)

selfdrive/car/__init__.py

@@ -1,6 +1,7 @@
 from common.fingerprints import fingerprint
 
 from .honda.interface import CarInterface as HondaInterface
+from .tesla.interface import CarInterface as TeslaInterface
 
 try:
   from .simulator.interface import CarInterface as SimInterface
@@ -17,6 +18,7 @@
   "ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface,
   "HONDA ACCORD 2016 TOURING": HondaInterface,
   "HONDA CR-V 2016 TOURING": HondaInterface,
+  "TESLA CLASSIC MODEL S": TeslaInterface,
 
   "simulator": SimInterface,
   "simulator2": Sim2Interface