From db83bc847bcabc17d8bb6fcbdd5cd9c3afff5f56 Mon Sep 17 00:00:00 2001
From: Vehicle Researcher <user@comma.ai>
Date: Sat, 28 Oct 2017 13:08:20 -0700
Subject: [PATCH] Tesla Model S support
---
common/dbc-0.35.py | 195 ++++++++++++++++
common/dbc.py | 29 ++-
common/fingerprints.py | 35 +--
selfdrive/car/__init__.py | 2 +
selfdrive/car/tesla/__init__.py | 0
selfdrive/car/tesla/can_parser.py | 131 +++++++++++
selfdrive/car/tesla/carcontroller.py | 209 ++++++++++++++++++
selfdrive/car/tesla/carstate.py | 254 +++++++++++++++++++++
selfdrive/car/tesla/interface.py | 293 +++++++++++++++++++++++++
selfdrive/car/tesla/teslacan.py | 113 ++++++++++
selfdrive/controls/controlsd.py | 31 +--
selfdrive/controls/lib/alertmanager.py | 6 +-
selfdrive/manager.py | 4 +-
13 files changed, 1253 insertions(+), 49 deletions(-)
create mode 100755 common/dbc-0.35.py
create mode 100644 selfdrive/car/tesla/__init__.py
create mode 100644 selfdrive/car/tesla/can_parser.py
create mode 100644 selfdrive/car/tesla/carcontroller.py
create mode 100644 selfdrive/car/tesla/carstate.py
create mode 100644 selfdrive/car/tesla/interface.py
create mode 100644 selfdrive/car/tesla/teslacan.py
diff --git a/common/dbc-0.35.py b/common/dbc-0.35.py
new file mode 100755
index 00000000000000..518890326b2bca
--- /dev/null
+++ b/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
+
diff --git a/common/dbc.py b/common/dbc.py
index 518890326b2bca..054da288ef98b5 100755
--- a/common/dbc.py
+++ b/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,9 +156,6 @@ 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
@@ -163,18 +163,15 @@ def decode(self, x, arr=None, debug=False):
# 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)
diff --git a/common/fingerprints.py b/common/fingerprints.py
index 298424940b5131..be19e9f158a520 100644
--- a/common/fingerprints.py
+++ b/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)
diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py
index e18afe12da9c34..0408398c03c010 100644
--- a/selfdrive/car/__init__.py
+++ b/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
diff --git a/selfdrive/car/tesla/__init__.py b/selfdrive/car/tesla/__init__.py
new file mode 100644
index 00000000000000..e69de29bb2d1d6
diff --git a/selfdrive/car/tesla/can_parser.py b/selfdrive/car/tesla/can_parser.py
new file mode 100644
index 00000000000000..bfd2552225acb6
--- /dev/null
+++ 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
diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py
new file mode 100644
index 00000000000000..4c5cdfdbee0786
--- /dev/null
+++ b/selfdrive/car/tesla/carcontroller.py
@@ -0,0 +1,209 @@
+from collections import namedtuple
+
+import common.numpy_fast as np
+from common.numpy_fast import clip, interp
+from common.realtime import sec_since_boot
+
+from selfdrive.config import CruiseButtons
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from selfdrive.controls.lib.drive_helpers import rate_limit
+
+from . import teslacan
+
+def actuator_hystereses(final_brake, braking, brake_steady, v_ego):
+ # hyst params... TODO: move these to VehicleParams
+ brake_hyst_on = 0.1 # to activate brakes exceed this value
+ brake_hyst_off = 0.005 # to deactivate brakes below this value
+ brake_hyst_gap = 0.01 # don't change brake command for small ocilalitons within this value
+
+ final_brake = 0.
+ braking = False
+ brake_steady = 0.
+
+
+ #*** histeresys logic to avoid brake blinking. go above 0.1 to trigger
+ #if (final_brake < brake_hyst_on and not braking) or final_brake < brake_hyst_off:
+ # final_brake = 0.
+ #braking = final_brake > 0.
+
+ # for small brake oscillations within brake_hyst_gap, don't change the brake command
+ #if final_brake == 0.:
+ # brake_steady = 0.
+ #elif final_brake > brake_steady + brake_hyst_gap:
+ # brake_steady = final_brake - brake_hyst_gap
+ #elif final_brake < brake_steady - brake_hyst_gap:
+ # brake_steady = final_brake + brake_hyst_gap
+ #final_brake = brake_steady
+
+ #if not civic:
+ # brake_on_offset_v = [.25, .15] # min brake command on brake activation. below this no decel is perceived
+ # brake_on_offset_bp = [15., 30.] # offset changes VS speed to not have too abrupt decels at high speeds
+ # offset the brake command for threshold in the brake system. no brake torque perceived below it
+ # brake_on_offset = interp(v_ego, brake_on_offset_bp, brake_on_offset_v)
+ # brake_offset = brake_on_offset - brake_hyst_on
+ # if final_brake > 0.0:
+ # final_brake += brake_offset
+
+ return final_brake, braking, brake_steady
+
+class AH:
+ #[alert_idx, value]
+ # See dbc files for info on values"
+ NONE = [0, 0]
+ FCW = [1, 0x8]
+ STEER = [2, 1]
+ BRAKE_PRESSED = [3, 10]
+ GEAR_NOT_D = [4, 6]
+ SEATBELT = [5, 5]
+ SPEED_TOO_HIGH = [6, 8]
+
+def process_hud_alert(hud_alert):
+ # initialize to no alert
+ fcw_display = 0
+ steer_required = 0
+ acc_alert = 0
+ if hud_alert == AH.NONE: # no alert
+ pass
+ elif hud_alert == AH.FCW: # FCW
+ fcw_display = hud_alert[1]
+ elif hud_alert == AH.STEER: # STEER
+ steer_required = hud_alert[1]
+ else: # any other ACC alert
+ acc_alert = hud_alert[1]
+
+ return fcw_display, steer_required, acc_alert
+
+#HUDData = namedtuple("HUDData",
+# ["pcm_accel", "v_cruise", "X2", "car", "X4", "X5",
+# "lanes", "beep", "X8", "chime", "acc_alert"])
+
+class CarController(object):
+ def __init__(self):
+ self.braking = False
+ self.brake_steady = 0.
+ self.final_brake_last = 0.
+
+ # redundant safety check with the board
+ self.controls_allowed = False
+
+ def update(self, sendcan, enabled, CS, frame, final_gas, final_brake, final_steer, \
+ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \
+ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert, \
+ snd_beep, snd_chime):
+ """ Controls thread """
+
+ # *** apply brake hysteresis ***
+ #final_brake, self.braking, self.brake_steady = actuator_hystereses(final_brake, self.braking, self.brake_steady, CS.v_ego)
+
+ # *** no output if not enabled ***
+ if not enabled:
+ final_gas = 0.
+ final_brake = 0.
+ final_steer = 0.
+
+ #Override for now
+ final_gas = 0.
+ final_brake = 0.
+
+ # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
+ #if CS.pcm_acc_status:
+ # pcm_cancel_cmd = True
+
+ # *** rate limit after the enable check ***
+ #final_brake = rate_limit(final_brake, self.final_brake_last, -2., 1./100)
+ self.final_brake_last = final_brake
+
+ # vehicle hud display, wait for one update from 10Hz 0x304 msg
+ #TODO: use enum!!
+ if hud_show_lanes:
+ hud_lanes = 0x04
+ else:
+ hud_lanes = 0x00
+
+ # TODO: factor this out better
+ if enabled:
+ if hud_show_car:
+ hud_car = 0xe0
+ else:
+ hud_car = 0xd0
+ else:
+ hud_car = 0xc0
+
+ # **** process the car messages ****
+
+ # *** compute control surfaces ***
+ tt = sec_since_boot()
+ GAS_MAX = 1004
+ BRAKE_MAX = 1024/4
+ SIGNAL_STEER_MAX = 16384
+ GAS_OFFSET = 328
+
+ if CS.v_ego < 16.7: #60.12 km/h divided by 3.6 = 16.7 meter per sec
+ USER_STEER_MAX = 180 # 180 degrees
+ elif CS.v_ego < 28: # 100.8 km/h
+ USER_STEER_MAX = 900 # 90 degrees
+ else:
+ USER_STEER_MAX = 300 # 30 degrees
+
+ apply_gas = 0 #int(clip(final_gas*GAS_MAX, 0, GAS_MAX-1))
+ apply_brake = 0 #int(clip(final_brake*BRAKE_MAX, 0, BRAKE_MAX-1))
+
+ #final_steer is between -1 and 1
+ if not enabled:
+ final_steer = 0
+ else:
+ # steer torque is converted back to CAN reference (positive when steering right)
+ final_steer = final_steer * -1
+ apply_steer = int(clip((final_steer * 100) + SIGNAL_STEER_MAX - (CS.angle_steers * 10), SIGNAL_STEER_MAX - USER_STEER_MAX, SIGNAL_STEER_MAX + USER_STEER_MAX))
+
+ # no gas if you are hitting the brake or the user is
+ if apply_gas > 0 and (apply_brake != 0 or CS.brake_pressed):
+ print "CANCELLING ACCELERATOR", apply_brake
+ apply_gas = 0
+
+ # no computer brake if the gas is being pressed
+ if CS.car_gas > 0 and apply_brake != 0:
+ print "CANCELLING BRAKE"
+ apply_brake = 0
+
+ # any other cp.vl[0x18F]['STEER_STATUS'] is common and can happen during user override. sending 0 torque to avoid EPS sending error 5
+ if CS.steer_not_allowed:
+ print "STEER ALERT, TORQUE INHIBITED"
+ apply_steer = 0
+
+ # *** entry into controls state ***
+ #if (CS.prev_cruise_buttons == 2) and CS.cruise_buttons == 0 and not self.controls_allowed:
+ if CS.cruise_buttons == 2 and not self.controls_allowed:
+ print "CONTROLS ARE LIVE"
+ self.controls_allowed = True
+
+ # *** exit from controls state on cancel, gas, or brake ***
+ if (CS.cruise_buttons == 1 or CS.brake_pressed ) and self.controls_allowed:
+ print "CONTROLS ARE DEAD (Cruise CANCEL or BRAKE pressed"
+ self.controls_allowed = False
+
+ # *** controls fail on steer error, brake error, or invalid can ***
+ if CS.steer_error:
+ print "STEER ERROR"
+ self.controls_allowed = False
+
+ if CS.brake_error:
+ print "BRAKE ERROR"
+ self.controls_allowed = False
+
+ #if not CS.can_valid and self.controls_allowed: # 200 ms
+ # print "CAN INVALID"
+ # self.controls_allowed = False
+
+ # Send CAN commands.
+ can_sends = []
+
+ # Send steering command.
+ if (frame % 5) == 0:
+ idx = (frame / 5) % 16
+ enable_steer_control = (not CS.right_blinker_on and not CS.left_blinker_on and self.controls_allowed and enabled)
+ can_sends.append(teslacan.create_steering_control(apply_steer, idx, enable_steer_control))
+ #if idx == 0: print "carcontroller.py: CS.angle_steers: %.2f CS.steer_override: %.2f final_steer: %.2f apply_steer: %.2f" % (CS.angle_steers, CS.steer_override, final_steer, apply_steer)
+ #print "carcontroller.py: apply_steer = " + str(apply_steer) + ", idx = " + str(idx)
+ sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
+
diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py
new file mode 100644
index 00000000000000..1689f5fd3f9167
--- /dev/null
+++ b/selfdrive/car/tesla/carstate.py
@@ -0,0 +1,254 @@
+import os
+import time
+
+import common.numpy_fast as np
+from common.realtime import sec_since_boot
+
+import selfdrive.messaging as messaging
+
+from common.realtime import sec_since_boot
+
+from selfdrive.car.tesla.can_parser import CANParser
+from selfdrive.can.parser import CANParser as CANParserC
+
+from selfdrive.services import service_list
+
+import zmq
+
+NEW_CAN = os.getenv("OLD_CAN") is None
+NEW_CAN = False
+
+def get_can_parser(CP):
+ # this function generates lists for signal, messages and initial values
+ if CP.carFingerprint == "TESLA CLASSIC MODEL S":
+ dbc_f = 'tesla_can.dbc'
+ signals = [
+ #("XMISSION_SPEED", 0x158, 0),
+ #("WHEEL_SPEED_FL", 0x1d0, 0),
+ #("WHEEL_SPEED_FR", 0x1d0, 0),
+ #("WHEEL_SPEED_RL", 0x1d0, 0),
+ #("WHEEL_SPEED_RR", 0x1d0, 0), #robertc (from JCT) -- might add later
+ #("STEER_ANGLE", 0x156, 0),
+ #("STEER_TORQUE_SENSOR", 0x18f, 0),
+ #("MCU_gpsHDOP", 760, 0),
+ ("MCU_gpsVehicleHeading", 760, 0),
+ ("MCU_gpsVehicleSpeed", 760, 0),
+ #("MCU_mppSpeedLimit", 760, 0),
+ #("MCU_speedLimitUnits", 760, 0),
+ #("MCU_userSpeedOffset", 760, 0),
+ #("MCU_userSpeedOffsetUnits", 760, 0),
+ #("MCU_clusterBrightnessLevel", 904, 0),
+ #("LgtSens_Tunnel", 643, 0),
+ #("LgtSens_Night", 643, 0),
+ #("LgtSens_Twlgt", 643, 0),
+ ("MCU_gpsAccuracy", 984, 0),
+ #("StW_Angl", 3, 0),
+ #("MC_STW_ANGL_STAT", 3, 0),
+ #("SECOND", 792,0),
+ #("MINUTE", 792,0),
+ ("MCU_latitude", 984, 0),
+ ("MCU_longitude", 984, 0),
+ ("DI_vehicleSpeed", 280, 0),
+ ("StW_AnglHP", 14, 0),
+ ("EPAS_torsionBarTorque", 880, 0), # Used in interface.py
+ ("EPAS_eacStatus", 880, 0),
+ ("EPAS_handsOnLevel", 880, 0),
+ ("DI_gear", 280, 3),
+ ("DOOR_STATE_FL", 792, 1),
+ ("DOOR_STATE_FR", 792, 1),
+ ("DOOR_STATE_RL", 792, 1),
+ ("DOOR_STATE_RR", 792, 1),
+ ("DI_stateCounter", 872, 0),
+ ("GTW_driverPresent", 840, 0),
+ ("DI_brakePedal", 280, 0),
+ ("SpdCtrlLvr_Stat", 69, 0),
+ #("EPAS_eacErrorCode", 880, 5),
+ #("ESP_brakeLamp", 309, 0),
+ #("ESP_espOffLamp", 309, 0),
+ #("ESP_tcOffLamp", 309, 0),
+ #("ESP_tcLampFlash", 309, 0),
+ #("ESP_espFaultLamp", 309, 0),
+ ("DI_cruiseSet", 872, 0),
+ ("DI_cruiseState", 872, 0),
+ #("DI_pedalPos", 264, 0),
+ ("VSL_Enbl_Rq", 69, 0), # Used in interface.py
+ ("TSL_RND_Posn_StW",109 , 0),
+ ("TSL_P_Psd_StW",109 , 0),
+ ("TurnIndLvr_Stat", 69, 0),
+ #("GTW_statusCounter", 840, 0),
+ ("DI_motorRPM", 264, 0)
+ ]
+ # robertc: all frequencies must be integers with new C CAN parser (2.5 => 2)
+ checks = [
+ (3, 175), #JCT Actual message freq is 35 Hz (0.0286 sec)
+ (14, 200), #JCT Actual message freq is 40 Hz (0.025 sec)
+ (69, 17), #JCT Actual message freq is 3.5 Hz (0.285 sec)
+ (109, 175), #JCT Actual message freq is 35 Hz (0.0286 sec)
+ (264, 59), #JCT Actual message freq is 11.8 Hz (0.084 sec)
+ (280, 18), #JCT Actual message freq is 3.7 Hz (0.275 sec)
+ (309, 2), #JCT Actual message freq is 0.487 Hz (2.05 sec)
+ (760, 2), #JCT Actual message freq is 0.487 Hz (2.05 sec)
+ (792, 16), #JCT Actual message freq is 3.3 Hz (0.3 sec)
+ (840, 2), #JCT Actual message freq is 0.5 Hz (2 sec)
+ (872, 5), #JCT Actual message freq is 1 Hz (1 sec)
+ (643, 5), #JCT Actual message freq is 1.3 Hz (0.76 sec)
+ (880, 5), #JCT Actual message freq is 1.3 Hz (0.76 sec)
+ (904, 5), #JCT Actual message freq is 1.3 Hz (0.76 sec)
+ (984, 5), #JCT Actual message freq is 1.3 Hz (0.76 sec)
+ ]
+
+ # add gas interceptor reading if we are using it
+ # robertc: What is this exactly?
+ #if CP.enableGas:
+ # signals.append(("INTERCEPTOR_GAS", 0x201, 0))
+ # checks.append((0x201, 50))
+
+ if NEW_CAN:
+ return CANParserC(os.path.splitext(dbc_f)[0], signals, checks, 0)
+ else:
+ return CANParser(dbc_f, signals, checks)
+
+class CarState(object):
+ def __init__(self, CP, logcan):
+ # robertc: got rid of all the Honda/Acura-specific stuff
+ self.brake_only = CP.enableCruise
+ self.CP = CP
+
+ # initialize can parser
+ self.cp = get_can_parser(CP)
+
+ self.user_gas, self.user_gas_pressed = 0., 0
+
+ self.cruise_buttons = 0
+ self.cruise_setting = 0
+ self.blinker_on = 0
+
+ self.log_interval = sec_since_boot()
+
+ #self.prev_user_brightness = 50
+ #self.user_brightness = 50
+ #self.display_brightness = 50
+ #self.day_or_night = 0
+
+ self.left_blinker_on = 0
+ self.right_blinker_on = 0
+
+ context = zmq.Context()
+ self.gps_sock = messaging.pub_sock(context, service_list['gpsLocationExternal'].port)
+
+ # TODO: actually make this work
+ self.a_ego = 0.
+
+ def update(self, can_pub_main=None):
+ cp = self.cp
+
+ if NEW_CAN:
+ cp.update(int(sec_since_boot() * 1e9), False)
+ else:
+ cp.update_can(can_pub_main)
+
+ # copy can_valid
+ self.can_valid = cp.can_valid
+
+ # car params
+ v_weight_v = [0., 1. ] # don't trust smooth speed at low values to avoid premature zero snapping
+ v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
+
+ # update prevs, update must run once per loop
+ self.prev_cruise_buttons = self.cruise_buttons
+ self.prev_cruise_setting = self.cruise_setting
+ self.prev_blinker_on = self.blinker_on
+
+ self.prev_left_blinker_on = self.left_blinker_on
+ self.prev_right_blinker_on = self.right_blinker_on
+
+ self.rpm = cp.vl[264]['DI_motorRPM']
+
+ # ******************* parse out can *******************
+ self.door_all_closed = not any([cp.vl[792]['DOOR_STATE_FL'], cp.vl[792]['DOOR_STATE_FR'],
+ cp.vl[792]['DOOR_STATE_RL'], cp.vl[792]['DOOR_STATE_RR']]) #JCT
+ self.seatbelt = cp.vl[840]['GTW_driverPresent']
+ self.steer_error = cp.vl[880]['EPAS_eacStatus'] not in [2,1]
+ self.steer_not_allowed = 0 #cp.vl[880]['EPAS_eacErrorCode'] != 0
+ self.brake_error = 0 #NOT WORKINGcp.vl[309]['ESP_brakeLamp'] #JCT
+ # JCT More ESP errors available, these needs to be added once car steers on its own to disable / alert driver
+ # JCT More ESP errors available, these needs to be added once car steers on its own to disable / alert driver
+ self.esp_disabled = 0 #NEED TO CORRECT DBC cp.vl[309]['ESP_espOffLamp'] or cp.vl[309]['ESP_tcOffLamp'] or cp.vl[309]['ESP_tcLampFlash'] or cp.vl[309]['ESP_espFaultLamp'] #JCT
+ # calc best v_ego estimate, by averaging two opposite corners
+ self.v_wheel = 0 #JCT
+ self.v_weight = 0 #JCT
+ self.v_ego = (cp.vl[280]['DI_vehicleSpeed'])*1.609/3.6 #JCT MPH_TO_MS. Tesla is in MPH, v_ego is expected in M/S
+ self.user_gas = 0 #for now
+ self.user_gas_pressed = 0 #for now
+ self.angle_steers = -(cp.vl[14]['StW_AnglHP']) #JCT polarity reversed from Honda/Acura
+ self.gear = 0
+ #JCT removed for now, will be used for cruise ON/OFF aka yellow led: self.cruise_setting = cp.vl[69]['VSL_Enbl_Rq']
+ self.cruise_buttons = cp.vl[69]['SpdCtrlLvr_Stat']
+ self.main_on = 1 #0 #JCT Need to find the Cruise ON/OFF switch that illuminate the yellow led on the cruise stalk
+ self.gear_shifter = cp.vl[280]['DI_gear']
+ self.gear_shifter_valid = (cp.vl[109]['TSL_RND_Posn_StW'] == 0) and (cp.vl[109]['TSL_P_Psd_StW'] ==0) and (cp.vl[280]['DI_gear'] ==4) #JCT PND stalk idle and Park button not pressed and in DRIVE
+ self.blinker_on = 0 #cp.vl[69]['TurnIndLvr_Stat'] in [1,2]
+ self.left_blinker_on = cp.vl[69]['TurnIndLvr_Stat'] == 1
+ self.right_blinker_on = cp.vl[69]['TurnIndLvr_Stat'] == 2
+ self.car_gas = 0 #cp.vl[264]['DI_pedalPos']
+ self.steer_override = abs(cp.vl[880]['EPAS_handsOnLevel']) > 0 #JCT need to be tested and adjusted. 8=38.7% of range as george uses. Range -20.4 to 20.4 Nm
+ self.brake_pressed = cp.vl[280]['DI_brakePedal']
+ self.user_brake = cp.vl[280]['DI_brakePedal']
+ self.standstill = cp.vl[280]['DI_vehicleSpeed'] == 0
+ self.v_cruise_pcm = cp.vl[872]['DI_cruiseSet']
+ self.pcm_acc_status = cp.vl[872]['DI_cruiseState']
+ self.pedal_gas = 0 #cp.vl[264]['DI_pedalPos']
+ self.hud_lead = 0 #JCT TBD
+ self.counter_pcm = cp.vl[872]['DI_stateCounter']
+ #self.display_brightness = cp.vl[904]['MCU_clusterBrightnessLevel']
+ #self.day_or_night = cp.vl[643]['LgtSens_Night']
+ #self.tunnel = cp.vl[643]['LgtSens_Tunnel']
+ #self.LgtSens_Twlgt = cp.vl[643]['LgtSens_Twlgt']
+
+
+ ## JCT Action every 1 sec
+ if (sec_since_boot() - self.log_interval) > 1:
+
+ # GPS Addon
+ dat = messaging.new_message()
+ dat.init('gpsLocation')
+ dat.gpsLocation.source = 2 # getting gps from car (on CAN bus)
+ dat.gpsLocation.flags = 0x1D # see line 89 of https://source.android.com/devices/halref/gps_8h_source.html#l00531
+ dat.gpsLocation.latitude = cp.vl[984]['MCU_latitude'] # in degrees
+ dat.gpsLocation.longitude = cp.vl[984]['MCU_longitude'] # in degrees
+ dat.gpsLocation.altitude = 0 # Flags += 2 once available (will give flags=0x1F) in meters above the WGS 84 reference ellipsoid
+ dat.gpsLocation.speed = (cp.vl[760]['MCU_gpsVehicleSpeed'])*1./3.6 # in meters per second
+ dat.gpsLocation.bearing = cp.vl[760]['MCU_gpsVehicleHeading'] # in degrees
+ dat.gpsLocation.accuracy = cp.vl[984]['MCU_gpsAccuracy'] # in meters
+ dat.gpsLocation.timestamp = int(time.time()) # Milliseconds since January 1, 1970. (GPS time N/A for now)
+ self.gps_sock.send(dat.to_bytes())
+
+ #if not self.day_or_night and not self.tunnel:
+ # self.user_brightness = self.display_brightness if self.display_brightness > 50 else 50 # During day anything bellow 50% is too dim
+ #else:
+ # self.user_brightness = self.display_brightness # Full brightness range (0-100%) if night or tunnel
+ #
+ #if self.user_brightness != self.prev_user_brightness:
+ # f2 = open('/sdcard/brightness', 'w')
+ # s = "%3.1f" % (self.user_brightness)
+ # f2.write(s)
+ # f2.close()
+
+ #self.prev_user_brightness = self.user_brightness
+
+ #if self.gear_shifter == 4:
+ # gear = 'D'
+ #elif self.gear_shifter == 1:
+ # gear = 'P'
+ #elif self.gear_shifter == 2:
+ # gear = 'R'
+ #else:
+ # gear = '?'
+ #f = open('/sdcard/candiac.log', 'a')
+ #s = "SPD=%3.1f RPM=%5d ANGLE=%5.1f GEAR=%s DOORS=%2d Standstill=%d Gas=%3.1f Brake=%1d GearValid=%1d D=%3.1f%% Night=%d Tunnel=%d Twlgt=%d\n" % (self.v_ego*3.6, self.rpm, self.angle_steers, gear, self.door_all_closed, self.standstill, self.car_gas, self.user_brake, self.gear_shifter_valid, self.display_brightness, self.day_or_night, self.tunnel, self.LgtSens_Twlgt)
+ #f.write(s)
+ #f.close()
+
+ self.log_interval = sec_since_boot()
+
+
diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py
new file mode 100644
index 00000000000000..f63e0c4914f8bd
--- /dev/null
+++ b/selfdrive/car/tesla/interface.py
@@ -0,0 +1,293 @@
+#!/usr/bin/env python
+import os
+import time
+import common.numpy_fast as np
+
+from selfdrive.config import Conversions as CV
+from .carstate import CarState
+from .carcontroller import CarController, AH
+from selfdrive.boardd.boardd import can_capnp_to_can_list
+
+from cereal import car
+
+from selfdrive.services import service_list
+import selfdrive.messaging as messaging
+
+NEW_CAN = os.getenv("OLD_CAN") is None
+NEW_CAN = False
+
+# Car button codes
+#JCT from dbc: VAL_ 69 SpdCtrlLvr_Stat 32 "DN_1ST" 16 "UP_1ST" 8 "DN_2ND" 4 "UP_2ND" 2 "RWD" 1 "FWD" 0 "IDLE" ;
+class CruiseButtons:
+ DN_1ST = 32 # Set / remove 1 km/h if already set
+ UP_1ST = 16 # Set / add 1 km/h if already set
+ DN_2ND = 8 # Remove 10 km/h if already set
+ UP_2ND = 4 # Add 10 km/h if already set
+ RWD = 2 # Resume (twice would eventually be enable openpilot)
+ FWD = 1 # Cancel
+ IDLE = 0
+
+#car chimes: enumeration from dbc file. Chimes are for alerts and warnings
+class CM:
+ MUTE = 0
+ SINGLE = 3
+ DOUBLE = 4
+ REPEATED = 1
+ CONTINUOUS = 2
+
+#car beepss: enumeration from dbc file. Beeps are for activ and deactiv
+class BP:
+ MUTE = 0
+ SINGLE = 3
+ TRIPLE = 2
+ REPEATED = 1
+
+class AH:
+ #[alert_idx, value]
+ # See dbc files for info on values"
+ NONE = [0, 0]
+ FCW = [1, 0x8]
+ STEER = [2, 1]
+ BRAKE_PRESSED = [3, 10]
+ GEAR_NOT_D = [4, 6]
+ SEATBELT = [5, 5]
+ SPEED_TOO_HIGH = [6, 8]
+
+class CarInterface(object):
+ def __init__(self, CP, logcan, sendcan=None):
+ self.logcan = logcan
+ self.CP = CP
+
+ self.frame = 0
+ self.can_invalid_count = 0
+
+ # *** init the major players ***
+ self.CS = CarState(CP, self.logcan)
+
+ # sending if read only is False
+ if sendcan is not None:
+ self.sendcan = sendcan
+ self.CC = CarController()
+
+ @staticmethod
+ def get_params(candidate, fingerprint):
+
+ # pedal
+ brake_only = True
+
+ ret = car.CarParams.new_message()
+
+ ret.carName = "tesla"
+ ret.radarName = "bosch"
+ ret.carFingerprint = candidate
+
+ ret.enableSteer = True
+ ret.enableBrake = True
+ ret.enableGas = not brake_only
+ ret.enableCruise = brake_only
+
+ if candidate == "TESLA CLASSIC MODEL S":
+ ret.wheelBase = 2.959
+ ret.slipFactor = 0.000713841
+ ret.steerRatio = 12
+ #ret.steerKp, ret.steerKi = 2.5, 0.15 # 2017-07-11am was at 2.5/0.25
+ ret.steerKp, ret.steerKi = 1.25, 0.2 # 2017-07-11am was at 2.5/0.25
+ else:
+ raise ValueError("unsupported car %s" % candidate)
+
+ return ret
+
+ # returns a car.CarState
+ def update(self):
+ # ******************* do can recv *******************
+ can_pub_main = []
+ canMonoTimes = []
+
+ if NEW_CAN:
+ self.CS.update(can_pub_main)
+ else:
+ for a in messaging.drain_sock(self.logcan):
+ canMonoTimes.append(a.logMonoTime)
+ can_pub_main.extend(can_capnp_to_can_list(a.can, [0,2]))
+ self.CS.update(can_pub_main)
+
+ # create message
+ ret = car.CarState.new_message()
+
+ # speeds
+ ret.vEgo = self.CS.v_ego
+ ret.wheelSpeeds.fl = self.CS.v_ego #JCT TODO while we find these self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FL']
+ ret.wheelSpeeds.fr = self.CS.v_ego #JCT TODO while we find these self.CS.cp.vl[0x1D0]['WHEEL_SPEED_FR']
+ ret.wheelSpeeds.rl = self.CS.v_ego #JCT TODO while we find these self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RL']
+ ret.wheelSpeeds.rr = self.CS.v_ego #JCT TODO while we find these self.CS.cp.vl[0x1D0]['WHEEL_SPEED_RR']
+
+ # gas pedal
+ ret.gas = 0 #self.CS.car_gas / 100.0 #JCT Tesla scaling is 0-100, Honda was 0-255
+ if not self.CP.enableGas:
+ ret.gasPressed = self.CS.pedal_gas > 0
+ else:
+ ret.gasPressed = self.CS.user_gas_pressed
+
+ # brake pedal
+ ret.brake = self.CS.user_brake
+ ret.brakePressed = self.CS.brake_pressed != 0
+
+ # steering wheel
+ # TODO: units
+ ret.steeringAngle = self.CS.angle_steers #JCT degree range -819.2|819
+ ret.steeringTorque = self.CS.cp.vl[880]['EPAS_torsionBarTorque']
+ ret.steeringPressed = self.CS.steer_override
+
+ # cruise state
+ #ret.cruiseState.enabled = self.CS.pcm_acc_status == 2
+ #ret.cruiseState.enabled = self.CS.cruise_buttons == 2 #CruiseButtons.RWD
+ ret.cruiseState.enabled = True #(self.CS.cp.vl[69]['VSL_Enbl_Rq'] == 0) #cruise light off on stalk
+ #print "VSL_Enbl_Rq: " + str(self.CS.cp.vl[69]['VSL_Enbl_Rq'])
+ #print "cruiseState.enabled: " + str(ret.cruiseState.enabled)
+ ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS
+ #ret.right_blinker = self.CS.right_blinker_on
+ #ret.left_blinker = self.CS.left_blinker_on
+ # TODO: for now, set to same as cruise light, check if car is in D?
+ ret.cruiseState.available = True #(self.CS.cp.vl[69]['VSL_Enbl_Rq'] == 0)
+
+ # TODO: button presses
+ buttonEvents = []
+
+ if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
+ be = car.CarState.ButtonEvent.new_message()
+ be.type = 'leftBlinker'
+ be.pressed = self.CS.left_blinker_on != 0
+ buttonEvents.append(be)
+
+ if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
+ be = car.CarState.ButtonEvent.new_message()
+ be.type = 'rightBlinker'
+ be.pressed = self.CS.right_blinker_on != 0
+ buttonEvents.append(be)
+
+ if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
+ #print "Cruise Buttons Debug: " + str(self.CS.cruise_buttons) + " Prev: " + str(self.CS.prev_cruise_buttons)
+ be = car.CarState.ButtonEvent.new_message()
+ be.type = 'unknown'
+ if self.CS.cruise_buttons != 0:
+ be.pressed = True
+ but = self.CS.cruise_buttons
+ else:
+ be.pressed = False
+ but = self.CS.prev_cruise_buttons
+ if but == CruiseButtons.UP_1ST:
+ be.type = 'accelCruise'
+ elif but == CruiseButtons.DN_1ST:
+ be.type = 'decelCruise'
+ elif but == CruiseButtons.FWD:
+ be.type = 'cancel'
+ elif but == 2:
+ be.type = 'altButton1'
+ #print "altButton1"
+ elif but == CruiseButtons.IDLE:
+ be.type = 'altButton3'
+ buttonEvents.append(be)
+
+ # JCT TODO This will be used to select openpilot or just regular cruise control via cruise ON/OFF button
+ # on cruise OFF (yellow led off) openpilot will be used if we can make it work even with led off, we'll see...
+ #if self.CS.cruise_setting != self.CS.prev_cruise_setting:
+ # be = car.CarState.ButtonEvent.new_message()
+ # be.type = 'unknown'
+ # if self.CS.cruise_setting != 0:
+ # be.pressed = True
+ # but = self.CS.cruise_setting
+ # else:
+ # be.pressed = False
+ # but = self.CS.prev_cruise_setting
+ # if but == 1:
+ # be.type = 'altButton1'
+ # # TODO: more buttons?
+ # buttonEvents.append(be)
+ ret.buttonEvents = buttonEvents
+
+ # errors
+ # TODO: I don't like the way capnp does enums
+ # These strings aren't checked at compile time
+ errors = []
+ if not self.CS.can_valid:
+ self.can_invalid_count =0#+= 1
+ #print "interface.py line 165, can_invalid_count = " + str(self.can_invalid_count)
+ #if self.can_invalid_count >= 5:
+ # errors.append('commIssue')
+ else:
+ self.can_invalid_count = 0
+ if self.CS.steer_error:
+ errors.append('steerUnavailable')
+ elif self.CS.steer_not_allowed:
+ errors.append('steerTempUnavailable')
+ if self.CS.brake_error:
+ errors.append('brakeUnavailable')
+ if not self.CS.gear_shifter_valid:
+ errors.append('wrongGear')
+ if not self.CS.door_all_closed:
+ errors.append('doorOpen')
+ if not self.CS.seatbelt:
+ errors.append('seatbeltNotLatched')
+ if self.CS.esp_disabled:
+ errors.append('espDisabled')
+ if not self.CS.main_on:
+ errors.append('wrongCarMode')
+ if self.CS.gear_shifter == 2:
+ errors.append('reverseGear')
+
+ ret.errors = errors
+ ret.canMonoTimes = canMonoTimes
+
+ # cast to reader so it can't be modified
+ #print ret
+ return ret.as_reader()
+
+ # pass in a car.CarControl
+ # to be called @ 100hz
+ def apply(self, c):
+ #print c
+
+ if c.hudControl.speedVisible:
+ hud_v_cruise = c.hudControl.setSpeed * CV.MS_TO_KPH
+ else:
+ hud_v_cruise = 255
+
+ hud_alert = {
+ "none": AH.NONE,
+ "fcw": AH.FCW,
+ "steerRequired": AH.STEER,
+ "brakePressed": AH.BRAKE_PRESSED,
+ "wrongGear": AH.GEAR_NOT_D,
+ "seatbeltUnbuckled": AH.SEATBELT,
+ "speedTooHigh": AH.SPEED_TOO_HIGH}[str(c.hudControl.visualAlert)]
+
+ snd_beep, snd_chime = {
+ "none": (BP.MUTE, CM.MUTE),
+ "beepSingle": (BP.SINGLE, CM.MUTE),
+ "beepTriple": (BP.TRIPLE, CM.MUTE),
+ "beepRepeated": (BP.REPEATED, CM.MUTE),
+ "chimeSingle": (BP.MUTE, CM.SINGLE),
+ "chimeDouble": (BP.MUTE, CM.DOUBLE),
+ "chimeRepeated": (BP.MUTE, CM.REPEATED),
+ "chimeContinuous": (BP.MUTE, CM.CONTINUOUS)}[str(c.hudControl.audibleAlert)]
+
+ pcm_accel = int(np.clip(c.cruiseControl.accelOverride/1.4,0,1)*0xc6)
+
+ read_only = False
+ # sending if read only is False
+ if not read_only:
+ self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, \
+ c.gas, c.brake, c.steeringTorque, \
+ c.cruiseControl.speedOverride, \
+ c.cruiseControl.override, \
+ c.cruiseControl.cancel, \
+ pcm_accel, \
+ hud_v_cruise, c.hudControl.lanesVisible, \
+ hud_show_car = c.hudControl.leadVisible, \
+ hud_alert = hud_alert, \
+ snd_beep = snd_beep, \
+ snd_chime = snd_chime)
+
+ self.frame += 1
+ return not (c.enabled and not self.CC.controls_allowed)
+
diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py
new file mode 100644
index 00000000000000..64d2b87e6c2efd
--- /dev/null
+++ b/selfdrive/car/tesla/teslacan.py
@@ -0,0 +1,113 @@
+import struct
+
+import common.numpy_fast as np
+from selfdrive.config import Conversions as CV
+
+
+# *** Honda specific ***
+def can_cksum(mm):
+ s = 0
+ for c in mm:
+ c = ord(c)
+ s += (c>>4)
+ s += c & 0xF
+ s = 8-s
+ s %= 0x10
+ return s
+
+def fix(msg, addr):
+ msg2 = msg[0:-1] + chr(ord(msg[-1]) | can_cksum(struct.pack("I", addr)+msg))
+ return msg2
+
+def make_can_msg(addr, dat, idx, alt):
+ if idx is not None:
+ dat += chr(idx << 4)
+ dat = fix(dat, addr)
+ return [addr, 0, dat, alt]
+
+def create_brake_command(apply_brake, pcm_override, pcm_cancel_cmd, chime, idx):
+ """Creates a CAN message for the Honda DBC BRAKE_COMMAND."""
+ pump_on = apply_brake > 0
+ brakelights = apply_brake > 0
+ brake_rq = apply_brake > 0
+
+ pcm_fault_cmd = False
+ amount = struct.pack("!H", (apply_brake << 6) + pump_on)
+ msg = amount + struct.pack("BBB", (pcm_override << 4) |
+ (pcm_fault_cmd << 2) |
+ (pcm_cancel_cmd << 1) | brake_rq, 0x80,
+ brakelights << 7) + chr(chime) + "\x00"
+ return make_can_msg(0x1fa, msg, idx, 0)
+
+def create_gas_command(gas_amount, idx):
+ """Creates a CAN message for the Tesla DBC acceleration command."""
+ msg = struct.pack("!H", gas_amount)
+ return make_can_msg(0x200, msg, idx, 0)
+
+def create_sound_packet(command):
+ msg = struct.pack("B", command)
+ return [0x720, 0, msg, 0]
+
+def create_steering_control(apply_steer, idx, controls_allowed):
+ """Creates a CAN message for the Tesla EPAS STEERING_CONTROL."""
+ """BO_ 1160 DAS_steeringControl: 4 NEO
+ SG_ DAS_steeringControlType : 23|2@0+ (1,0) [0|0] "" EPAS
+ SG_ DAS_steeringControlChecksum : 31|8@0+ (1,0) [0|0] "" EPAS
+ SG_ DAS_steeringControlCounter : 19|4@0+ (1,0) [0|0] "" EPAS
+ SG_ DAS_steeringAngleRequest : 6|15@0+ (0.1,-1638.35) [-1638.35|1638.35] "deg" EPAS
+ SG_ DAS_steeringHapticRequest : 7|1@0+ (1,0) [0|0] "" EPAS"""
+ if controls_allowed == False:
+ steering_type = 0
+ else:
+ steering_type = 1
+ type_counter = steering_type << 6
+ type_counter += idx
+ checksum = ((apply_steer & 0xFF) + ((apply_steer >> 8) & 0xFF) + type_counter + 0x8C) & 0xFF
+ msg = struct.pack("!hBB", apply_steer, type_counter, checksum)
+ #TODO: change 0x489 to 0x488 for production
+ return [0x488, 0, msg, 1]
+
+def create_ui_commands(pcm_speed, hud, idx):
+ """Creates an iterable of CAN messages for the UIs."""
+ commands = []
+ pcm_speed_real = np.clip(int(round(pcm_speed / 0.002759506)), 0,
+ 64000) # conversion factor from dbc file
+ msg_0x30c = struct.pack("!HBBBBB", pcm_speed_real, hud.pcm_accel,
+ hud.v_cruise, hud.X2, hud.car, hud.X4)
+ commands.append(make_can_msg(0x30c, msg_0x30c, idx, 0))
+
+ msg_0x33d = chr(hud.X5) + chr(hud.lanes) + chr(hud.beep) + chr(hud.X8)
+ commands.append(make_can_msg(0x33d, msg_0x33d, idx, 0))
+ #if civic: # 2 more msgs
+ # msg_0x35e = chr(0) * 7
+ # commands.append(make_can_msg(0x35e, msg_0x35e, idx, 0))
+ #if civic or accord:
+ # msg_0x39f = (
+ # chr(0) * 2 + chr(hud.acc_alert) + chr(0) + chr(0xff) + chr(0x7f) + chr(0)
+ # )
+ # commands.append(make_can_msg(0x39f, msg_0x39f, idx, 0))
+ return commands
+
+def create_radar_commands(v_ego, idx):
+ """Creates an iterable of CAN messages for the radar system."""
+ commands = []
+ v_ego_kph = np.clip(int(round(v_ego * CV.MS_TO_KPH)), 0, 255)
+ speed = struct.pack('!B', v_ego_kph)
+ msg_0x300 = ("\xf9" + speed + "\x8a\xd0" +\
+ ("\x20" if idx == 0 or idx == 3 else "\x00") +\
+ "\x00\x00")
+ #if civic:
+ # msg_0x301 = "\x02\x38\x44\x32\x4f\x00\x00"
+ # add 8 on idx.
+ # commands.append(make_can_msg(0x300, msg_0x300, idx + 8, 1))
+ #elif accord:
+ # 0300( 768)( 69) f9008ad0100000ef
+ # 0301( 769)( 69) 0ed8522256000029
+ # msg_0x301 = "\x0e\xd8\x52\x22\x56\x00\x00"
+ # add 0xc on idx? WTF is this?
+ # commands.append(make_can_msg(0x300, msg_0x300, idx + 0xc, 1))
+ #else:
+ # msg_0x301 = "\x0f\x18\x51\x02\x5a\x00\x00"
+ # commands.append(make_can_msg(0x300, msg_0x300, idx, 1))
+ commands.append(make_can_msg(0x301, msg_0x301, idx, 1))
+ return commands
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 9d447954b806b1..ac49d5fa6f87ff 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -92,7 +92,8 @@ def __init__(self, gctx, rate=100):
# rear view camera state
self.rear_view_toggle = False
self.rear_view_allowed = bool(params.get("IsRearViewMirror"))
-
+ self.rear_view_allowed = False
+
self.v_cruise_kph = 255
# 0.0 - 1.0
@@ -165,8 +166,11 @@ def state_control(self):
else:
self.rear_view_toggle = False
+ # if b.type == "altButton1" and b.pressed:
+ # self.rear_view_toggle = not self.rear_view_toggle
if b.type == "altButton1" and b.pressed:
- self.rear_view_toggle = not self.rear_view_toggle
+ print("Activating openpilot.")
+ enable_request = True
if not self.CP.enableCruise and self.enabled and not b.pressed:
if b.type == "accelCruise":
@@ -175,8 +179,8 @@ def state_control(self):
self.v_cruise_kph -= (self.v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA
self.v_cruise_kph = clip(self.v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
- if not self.enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed:
- enable_request = True
+ #if not self.enabled and b.type in ["accelCruise", "decelCruise"] and not b.pressed:
+ # enable_request = True
# do disable on button down
if b.type == "cancel" and b.pressed:
@@ -185,16 +189,17 @@ def state_control(self):
self.prof.checkpoint("Buttons")
# *** health checking logic ***
- hh = messaging.recv_sock(self.health)
- if hh is not None:
+ #hh = messaging.recv_sock(self.health)
+ #if hh is not None:
# if the board isn't allowing controls but somehow we are enabled!
# TODO: this should be in state transition with a function follower logic
- if not hh.health.controlsAllowed and self.enabled:
- self.AM.add("controlsMismatch", self.enabled)
+ #if not hh.health.controlsAllowed and self.enabled:
+ # self.AM.add("controlsMismatch", self.enabled)
# disable if the pedals are pressed while engaged, this is a user disable
if self.enabled:
- if self.CS.gasPressed or self.CS.brakePressed or not self.CS.cruiseState.available:
+ #if self.CS.gasPressed or self.CS.brakePressed or not self.CS.cruiseState.available:
+ if self.CS.brakePressed or not self.CS.cruiseState.available:
self.AM.add("disable", self.enabled)
# it can happen that car cruise disables while comma system is enabled: need to
@@ -223,8 +228,8 @@ def state_control(self):
else:
enable_condition = enable_request
- if self.CP.enableCruise and self.CS.cruiseState.enabled:
- self.v_cruise_kph = self.CS.cruiseState.speed * CV.MS_TO_KPH
+ #if self.CP.enableCruise and self.CS.cruiseState.enabled:
+ self.v_cruise_kph = self.CS.cruiseState.speed * CV.MS_TO_KPH
self.prof.checkpoint("AdaptiveCruise")
@@ -243,8 +248,8 @@ def state_control(self):
for alert in self.CS.errors:
self.AM.add(alert, self.enabled)
- if not self.plan.longitudinalValid:
- self.AM.add("radarCommIssue", self.enabled)
+ #if not self.plan.longitudinalValid:
+ # self.AM.add("radarCommIssue", self.enabled)
if self.cal_status != Calibration.CALIBRATED:
if self.cal_status == Calibration.UNCALIBRATED:
diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py
index 162a1977ed2168..bad49d6870cbdc 100644
--- a/selfdrive/controls/lib/alertmanager.py
+++ b/selfdrive/controls/lib/alertmanager.py
@@ -40,7 +40,8 @@ class AlertManager(object):
"pedalPressed": alert("Comma Unavailable", "Pedal Pressed", ET.NO_ENTRY, "brakePressed", "chimeDouble", .4, 2., 3.),
"preDriverDistracted": alert("Take Control ", "User Distracted", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.),
"driverDistracted": alert("Take Control to Regain Speed", "User Distracted", ET.WARNING, "steerRequired", "chimeRepeated", .5, .5, .5),
- "steerSaturated": alert("Take Control", "Turn Exceeds Limit", ET.WARNING, "steerRequired", "chimeSingle", 1., 2., 3.),
+ #"steerSaturated": alert("Take Control", "Turn Exceeds Limit", ET.WARNING, "steerRequired", "chimeSingle", 1., 2., 3.),
+ "steerSaturated": alert("Control on standby", "Lane change in progress", ET.WARNING, None, "chimeSingle", .2, 0., 0.),
"overheat": alert("Take Control Immediately", "System Overheated", ET.SOFT_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"controlsMismatch": alert("Take Control Immediately", "Controls Mismatch", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"radarCommIssue": alert("Take Control Immediately", "Radar Error: Restart the Car",ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
@@ -51,7 +52,8 @@ class AlertManager(object):
"fcw": alert("", "", ET.WARNING, None, None, .1, .1, .1),
# car errors
"commIssue": alert("Take Control Immediately","CAN Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
- "steerUnavailable": alert("Take Control Immediately","Steer Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+ #"steerUnavailable": alert("Take Control Immediately","Steer Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
+ "steerUnavailable": alert("Control soft disable","User has taken over control", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"steerTempUnavailable": alert("Take Control", "Steer Temporarily Unavailable", ET.WARNING, "steerRequired", "chimeDouble", .4, 2., 3.),
"brakeUnavailable": alert("Take Control Immediately","Brake Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
"gasUnavailable": alert("Take Control Immediately","Gas Error: Restart the Car", ET.IMMEDIATE_DISABLE, "steerRequired", "chimeRepeated", 1., 3., 3.),
diff --git a/selfdrive/manager.py b/selfdrive/manager.py
index 136cd7345707a2..0857f0deef4788 100755
--- a/selfdrive/manager.py
+++ b/selfdrive/manager.py
@@ -42,7 +42,7 @@
managed_processes = {
"uploader": "selfdrive.loggerd.uploader",
"controlsd": "selfdrive.controls.controlsd",
- "radard": "selfdrive.controls.radard",
+# "radard": "selfdrive.controls.radard",
"loggerd": ("loggerd", ["./loggerd"]),
"logmessaged": "selfdrive.logmessaged",
"tombstoned": "selfdrive.tombstoned",
@@ -67,7 +67,7 @@ def get_running():
'controlsd',
'loggerd',
'sensord',
- 'radard',
+# 'radard',
'visiond',
'proclogd',
]