commaai · bitshop · Jan 24, 2018 · Jan 24, 2018 · Jan 24, 2018 · Jan 25, 2018
diff --git a/common/fingerprints.py b/common/fingerprints.py
@@ -1,4 +1,6 @@
 import os
+from selfdrive.car.toyota.values import CAR as TOYOTA
+from selfdrive.car.tesla.values  import CAR as TESLA
 
 _FINGERPRINTS = {
   "ACURA ILX 2016 ACURAWATCH PLUS": {
@@ -19,15 +21,21 @@
   "HONDA ODYSSEY 2018 EX-L": {
     57L: 3, 148L: 8, 228L: 5, 229L: 4, 316L: 8, 342L: 6, 344L: 8, 380L: 8, 399L: 7, 411L: 5, 419L: 8, 420L: 8, 427L: 3, 432L: 7, 450L: 8, 463L: 8, 464L: 8, 476L: 4, 490L: 8, 506L: 8, 542L: 7, 545L: 6, 597L: 8, 662L: 4, 773L: 7, 777L: 8, 780L: 8, 795L: 8, 800L: 8, 804L: 8, 806L: 8, 808L: 8, 817L: 4, 819L: 7, 821L: 5, 825L: 4, 829L: 5, 837L: 5, 856L: 7, 862L: 8, 871L: 8, 881L: 8, 882L: 4, 884L: 8, 891L: 8, 892L: 8, 905L: 8, 923L: 2, 927L: 8, 929L: 8, 963L: 8, 965L: 8, 966L: 8, 967L: 8, 983L: 8, 985L: 3, 1029L: 8, 1036L: 8, 1052L: 8, 1064L: 7, 1088L: 8, 1089L: 8, 1092L: 1, 1108L: 8, 1110L: 8, 1125L: 8, 1296L: 8, 1302L: 8, 1600L: 5, 1601L: 8, 1612L: 5, 1613L: 5, 1614L: 5, 1615L: 8, 1616L: 5, 1619L: 5, 1623L: 5, 1668L: 5
   },
-  "TOYOTA RAV4 2017": {
+  TOYOTA.RAV4: {
     36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 4, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
   },
-  "TOYOTA RAV4 2017 HYBRID": {
+  TOYOTA.RAV4H: {
    36L: 8, 37L: 8, 170L: 8, 180L: 8, 186L: 4, 426L: 6, 452L: 8, 464L: 8, 466L: 8, 467L: 8, 547L: 8, 548L: 8, 552L: 4, 562L: 4, 608L: 8, 610L: 5, 643L: 7, 705L: 8, 725L: 2, 740L: 5, 800L: 8, 835L: 8, 836L: 8, 849L: 4, 869L: 7, 870L: 7, 871L: 2, 896L: 8, 897L: 8, 900L: 6, 902L: 6, 905L: 8, 911L: 8, 916L: 3, 918L: 7, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 951L: 8, 955L: 8, 956L: 8, 979L: 2, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1008L: 2, 1014L: 8, 1017L: 8, 1041L: 8, 1042L: 8, 1043L: 8, 1044L: 8, 1056L: 8, 1059L: 1, 1114L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1176L: 8, 1177L: 8, 1178L: 8, 1179L: 8, 1180L: 8, 1181L: 8, 1190L: 8, 1191L: 8, 1192L: 8, 1196L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1263L: 8, 1279L: 8, 1408L: 8, 1409L: 8, 1410L: 8, 1552L: 8, 1553L: 8, 1554L: 8, 1555L: 8, 1556L: 8, 1557L: 8, 1561L: 8, 1562L: 8, 1568L: 8, 1569L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1584L: 8, 1589L: 8, 1592L: 8, 1593L: 8, 1595L: 8, 1596L: 8, 1597L: 8, 1600L: 8, 1656L: 8, 1664L: 8, 1728L: 8, 1745L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8, 581L: 5, 296: 8, 552L: 8, 560L: 7, 552L: 4, 713L: 8, 550L: 8, 608L: 8, 37L: 8, 36L: 8, 950L: 8, 1198L: 8, 1197L: 8, 1199L: 8, 1212L: 8, 953L: 3, 1264L: 8, 1184L: 8, 1005L: 2, 1185L: 8, 1232L: 8, 1186L: 8
  },
-  "TOYOTA PRIUS 2017": {
+  TOYOTA.PRIUS: {
     36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2, 898L: 8, 900L: 6, 902L: 6, 905L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
   },
+  TOYOTA.PRIUSP: {
+     36L: 8, 37L: 8, 166L: 8, 170L: 8, 180L: 8, 295L: 8, 296L: 8, 426L: 6, 452L: 8, 466L: 8, 467L: 8, 550L: 8, 552L: 4, 560L: 7, 562L: 6, 581L: 5, 608L: 8, 610L: 8, 614L: 8, 643L: 7, 658L: 8, 713L: 8, 740L: 5, 742L: 8, 743L: 8, 800L: 8, 810L: 2, 814L: 8, 824L: 2, 829L: 2, 830L: 7, 835L: 8, 836L: 8, 863L: 8, 869L: 7, 870L: 7, 871L: 2,898L: 8, 900L: 6, 902L: 6, 905L: 8, 913L: 8, 918L: 8, 921L: 8, 933L: 8, 944L: 8, 945L: 8, 950L: 8, 951L: 8, 953L: 8, 955L: 8, 956L: 8, 971L: 7, 974L: 8, 975L: 5, 993L: 8, 998L: 5, 999L: 7, 1000L: 8, 1001L: 8, 1014L: 8, 1017L: 8, 1020L: 8, 1041L: 8, 1042L: 8, 1044L: 8, 1056L: 8, 1057L: 8, 1059L: 1, 1071L: 8, 1076L: 8, 1077L: 8, 1082L: 8, 1083L: 8, 1084L: 8, 1085L: 8, 1086L: 8, 1114L: 8, 1132L: 8, 1161L: 8, 1162L: 8, 1163L: 8, 1164L: 8, 1165L: 8, 1166L: 8, 1167L: 8, 1175L: 8, 1227L: 8, 1228L: 8, 1235L: 8, 1237L: 8, 1279L: 8, 1552L: 8, 1553L: 8, 1556L: 8, 1557L: 8, 1568L: 8, 1570L: 8, 1571L: 8, 1572L: 8, 1595L: 8, 1777L: 8, 1779L: 8, 1904L: 8, 1912L: 8, 1990L: 8, 1998L: 8
+    },
+  TESLA.S_CLASSIC: {
+      1: 8, 3: 8, 14: 8, 21: 4, 69: 8, 109: 4, 257: 3, 264: 8, 267: 5, 277: 4, 280: 6, 283: 5, 293: 4, 296: 4, 309: 5, 336: 8, 341: 8, 357: 8, 360: 7, 415: 8, 513: 5, 516: 8, 520: 4, 522: 8, 524: 8, 527: 8, 536: 8, 551: 4, 552: 2, 556: 8, 568: 8, 582: 5, 638: 8, 643: 8, 693: 8, 696: 8, 712: 8, 728: 8, 744: 8, 760: 8, 771: 2, 772: 8, 775: 8, 776: 8, 778: 8, 780: 2, 783: 8, 785: 8, 787: 8, 788: 8, 791: 8, 792: 8, 796: 2, 799: 8, 804: 8, 807: 8, 808: 1, 812: 8, 815: 8, 820: 8, 823: 8, 824: 8, 836: 8, 840: 8, 856: 4, 863: 8, 872: 8, 880: 8, 888: 8, 896: 8, 904: 3, 920: 8, 936: 8, 952: 8, 953: 6, 984: 8, 1026: 8, 1028: 8, 1029: 8, 1030: 8, 1032: 1, 1034: 8, 1048: 1, 1281: 8, 1332: 8, 1335: 8, 1368: 8, 1436: 8, 1456: 8, 1800: 4, 1804: 8, 1812: 8, 1815: 8, 1816: 8, 1831: 8, 1832: 8, 1864: 8, 1880: 8, 1892: 8, 1896: 8, 1912: 8, 1960: 8, 1992: 8, 2008: 3, 2043: 5
+      }
 }
 
 # support additional internal only fingerprints
@@ -57,6 +65,7 @@ def eliminate_incompatible_cars(msg, candidate_cars):
       pass
       #isin = adr in _FINGERPRINTS[car_name]
       #print "eliminate", car_name, hex(adr), isin, len(msg.dat), msg.dat.encode("hex")
+  print compatible_cars
   return compatible_cars
 
 def all_known_cars():

diff --git a/opendbc/tesla_can.dbc b/opendbc/tesla_can.dbc
@@ -33,15 +33,7 @@ NS_ :
 
 BS_:
 
-BU_:
-    NEO
-    MCU
-    GTW
-    EPAS
-    DI
-    ESP
-    SBW
-    STW
+BU_: NEO MCU GTW EPAS DI ESP SBW STW
 
 VAL_TABLE_ StW_AnglHP_Spd 16383 "SNA" ;
 VAL_TABLE_ DI_aebFaultReason 15 "DI_AEB_FAULT_DAS_REQ_DI_UNAVAIL" 14 "DI_AEB_FAULT_ACCEL_REQ_INVALID" 13 "DI_AEB_FAULT_MIN_TIME_BTWN_EVENTS" 12 "DI_AEB_FAULT_ESP_MIA" 11 "DI_AEB_FAULT_ESP_FAULT" 10 "DI_AEB_FAULT_EPB_NOT_PARKED" 9 "DI_AEB_FAULT_ACCEL_OUT_OF_BOUNDS" 8 "DI_AEB_FAULT_PM_REQUEST" 7 "DI_AEB_FAULT_VEL_EST_ABNORMAL" 6 "DI_AEB_FAULT_DAS_SNA" 5 "DI_AEB_FAULT_DAS_CONTROL_MIA" 4 "DI_AEB_FAULT_SPEED_DELTA" 3 "DI_AEB_FAULT_EBR_FAULT" 2 "DI_AEB_FAULT_PM_MIA" 1 "DI_AEB_FAULT_EPB_MIA" 0 "DI_AEB_FAULT_NONE" ;

diff --git a/panda/board/main.c b/panda/board/main.c
@@ -105,6 +105,8 @@ int get_health_pkt(void *dat) {
 #ifdef PANDA
   health->current = adc_get(ADCCHAN_CURRENT);
   health->started = (GPIOA->IDR & (1 << 1)) == 0;
+  // This is a hack for Tesla temporarily
+  health->started = can_live;
 #else
   health->current = 0;
   health->started = (GPIOC->IDR & (1 << 13)) != 0;

diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py
@@ -3,39 +3,10 @@
 
 from common.realtime import sec_since_boot
 from common.fingerprints import eliminate_incompatible_cars, all_known_cars
+from selfdrive.car.interfaces import interfaces
 
 from selfdrive.swaglog import cloudlog
 import selfdrive.messaging as messaging
-from selfdrive.car.honda.interface import CarInterface as HondaInterface
-from selfdrive.car.toyota.interface import CarInterface as ToyotaInterface
-from selfdrive.car.mock.interface import CarInterface as MockInterface
-
-try:
-  from .simulator.interface import CarInterface as SimInterface
-except ImportError:
-  SimInterface = None
-
-try:
-  from .simulator2.interface import CarInterface as Sim2Interface
-except ImportError:
-  Sim2Interface = None
-
-
-interfaces = {
-  "HONDA CIVIC 2016 TOURING": HondaInterface,
-  "ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface,
-  "HONDA ACCORD 2016 TOURING": HondaInterface,
-  "HONDA CR-V 2016 TOURING": HondaInterface,
-  "HONDA ODYSSEY 2018 EX-L": HondaInterface,
-  "TOYOTA PRIUS 2017": ToyotaInterface,
-  "TOYOTA RAV4 2017": ToyotaInterface,
-  "TOYOTA RAV4 2017 HYBRID": ToyotaInterface,
-
-  "simulator": SimInterface,
-  "simulator2": Sim2Interface,
-
-  "mock": MockInterface
-}
 
 # **** for use live only ****
 def fingerprint(logcan, timeout):

diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
@@ -0,0 +1,36 @@
+from selfdrive.car.honda.interface import CarInterface as HondaInterface
+from selfdrive.car.toyota.interface import CarInterface as ToyotaInterface
+from selfdrive.car.mock.interface import CarInterface as MockInterface
+from selfdrive.car.tesla.interface import CarInterface as TeslaInterface
+from selfdrive.car.toyota.values import CAR as TOYOTA
+from selfdrive.car.tesla.values import CAR as TESLA
+
+try:
+  from .simulator.interface import CarInterface as SimInterface
+except ImportError:
+  SimInterface = None
+
+try:
+  from .simulator2.interface import CarInterface as Sim2Interface
+except ImportError:
+  Sim2Interface = None
+
+
+interfaces = {
+  "HONDA CIVIC 2016 TOURING": HondaInterface,
+  "ACURA ILX 2016 ACURAWATCH PLUS": HondaInterface,
+  "HONDA ACCORD 2016 TOURING": HondaInterface,
+  "HONDA CR-V 2016 TOURING": HondaInterface,
+  "HONDA ODYSSEY 2018 EX-L": HondaInterface,
+  TOYOTA.PRIUS: ToyotaInterface,
+  TOYOTA.PRIUSP: ToyotaInterface,
+  TOYOTA.RAV4: ToyotaInterface,
+  TOYOTA.RAV4H: ToyotaInterface,
+  TESLA.S_CLASSIC: TeslaInterface,
+
+  "simulator": SimInterface,
+  "simulator2": Sim2Interface,
+
+  "mock": MockInterface
+}
+
diff --git a/selfdrive/car/tesla/__init__.py b/selfdrive/car/tesla/__init__.py
diff --git a/selfdrive/car/tesla/can_parser.py b/selfdrive/car/tesla/can_parser.py
@@ -0,0 +1,131 @@
+import os
+import opendbc
+from collections import defaultdict
+
+from selfdrive.car.tesla.teslacan import fix
+from common.realtime import sec_since_boot
+from common.dbc import dbc
+
+class CANParser(object):
+  def __init__(self, dbc_f, signals, checks=[]):
+    ### input:
+    # dbc_f   : dbc file
+    # signals : List of tuples (name, address, ival) where
+    #             - name is the signal name.
+    #             - address is the corresponding message address.
+    #             - ival is the initial value.
+    # checks  : List of pairs (address, frequency) where
+    #             - address is the message address of a message for which health should be
+    #               monitored.
+    #             - frequency is the frequency at which health should be monitored.
+
+    self.msgs_ck = set([check[0] for check in checks])
+    self.frqs = dict(checks)
+    self.can_valid = False  # start with False CAN assumption
+    # list of received msg we want to monitor counter and checksum for
+    # read dbc file
+    self.can_dbc = dbc(os.path.join(opendbc.DBC_PATH, dbc_f))
+    # initialize variables to initial values
+    self.vl = {}    # signal values
+    self.ts = {}    # time stamp recorded in log
+    self.ct = {}    # current time stamp
+    self.ok = {}    # valid message?
+    self.cn = {}    # message counter
+    self.cn_vl = {} # message counter mismatch value
+    self.ck = {}    # message checksum status
+
+    for _, addr, _ in signals:
+      self.vl[addr] = {}
+      self.ts[addr] = 0
+      self.ct[addr] = sec_since_boot()
+      self.ok[addr] = False
+      self.cn[addr] = 0
+      self.cn_vl[addr] = 0
+      self.ck[addr] = False
+
+    for name, addr, ival in signals:
+      self.vl[addr][name] = ival
+
+    self._msgs = [s[1] for s in signals]
+    self._sgs = [s[0] for s in signals]
+
+    self._message_indices = defaultdict(list)
+    for i, x in enumerate(self._msgs):
+      self._message_indices[x].append(i)
+
+  def update_can(self, can_recv):
+    msgs_upd = []
+    cn_vl_max = 5   # no more than 5 wrong counter checks
+
+    self.sec_since_boot_cached = sec_since_boot()
+
+    # we are subscribing to PID_XXX, else data from USB
+    for msg, ts, cdat, _ in can_recv:
+      idxs = self._message_indices[msg]
+      if idxs:
+        msgs_upd.append(msg)
+        # read the entire message
+        out = self.can_dbc.decode((msg, 0, cdat))[1]
+        # checksum check
+        self.ck[msg] = True
+        if "CHECKSUM" in out.keys() and msg in self.msgs_ck:
+          # remove checksum (half byte)
+          ck_portion = cdat[:-1] + chr(ord(cdat[-1]) & 0xF0)
+          # recalculate checksum
+          msg_vl = fix(ck_portion, msg)
+          # compare recalculated vs received checksum
+          if msg_vl != cdat:
+            print "CHECKSUM FAIL: " + hex(msg)
+            self.ck[msg] = False
+            self.ok[msg] = False
+        # counter check
+        cn = 0
+        if "COUNTER" in out.keys():
+          cn = out["COUNTER"]
+        # check counter validity if it's a relevant message
+        if cn != ((self.cn[msg] + 1) % 4) and msg in self.msgs_ck and "COUNTER" in out.keys():
+          #print hex(msg), "FAILED COUNTER!"
+          self.cn_vl[msg] += 1   # counter check failed
+        else:
+          self.cn_vl[msg] -= 1   # counter check passed
+        # message status is invalid if we received too many wrong counter values
+        if self.cn_vl[msg] >= cn_vl_max:
+          print "COUNTER WRONG: " + hex(msg)
+          self.ok[msg] = False
+
+        # update msg time stamps and counter value
+        self.ts[msg] = ts
+        self.ct[msg] = self.sec_since_boot_cached
+        self.cn[msg] = cn
+        self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max)
+
+        # set msg valid status if checksum is good and wrong counter counter is zero
+        if self.ck[msg] and self.cn_vl[msg] == 0:
+          self.ok[msg] = True
+
+        #robertc (from JCT) TEMPORARY remove counter and checksum check
+        #robertc (from JCT) This needs to be re-done before openpilot is activated to steer the car
+        self.ok[msg] = True
+
+        # update value of signals in the
+        for ii in idxs:
+          sg = self._sgs[ii]
+          self.vl[msg][sg] = out[sg]
+
+    # for each message, check if it's too long since last time we received it
+    self._check_dead_msgs()
+
+    # assess overall can validity: if there is one relevant message invalid, then set can validity flag to False
+    self.can_valid = True
+    if False in self.ok.values():
+      #print "CAN INVALID!"
+      self.can_valid = False
+
+    return msgs_upd
+
+  def _check_dead_msgs(self):
+    ### input:
+    ## simple stuff for now: msg is not valid if a message isn't received for 10 consecutive steps
+    for msg in set(self._msgs):
+      if msg in self.msgs_ck and self.sec_since_boot_cached - self.ct[msg] > 10./self.frqs[msg]:
+        self.ok[msg] = False