From 4b54ecdf2fea705cd56e360e2c44c040cf9fc8ae Mon Sep 17 00:00:00 2001 From: Dragonpilot Date: Mon, 29 Jul 2019 13:26:30 +1000 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E5=85=A5=20Toyota/Lexus=20=E5=8E=9F?= =?UTF-8?q?=E5=BB=A0=20DSU=20=E6=A8=A1=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- common/params.py | 1 + selfdrive/car/toyota/carstate.py | 12 ++++++++++ selfdrive/car/toyota/interface.py | 25 +++++++++++--------- selfdrive/controls/controlsd.py | 9 ++++--- selfdrive/dragonpilot/dragonconf/__init__.py | 1 + 5 files changed, 34 insertions(+), 14 deletions(-) diff --git a/common/params.py b/common/params.py index 04fd8b962..73579a796 100755 --- a/common/params.py +++ b/common/params.py @@ -98,6 +98,7 @@ keys = { "DragonCachedVIN": [TxType.PERSISTENT], "DragonAllowGas": [TxType.PERSISTENT], "DragonBBUI": [TxType.PERSISTENT], + "DragonToyotaStockDSU": [TxType.PERSISTENT], } diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 40be422db..20f726461 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -5,6 +5,9 @@ from selfdrive.can.parser import CANParser from selfdrive.config import Conversions as CV from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR +from common.params import Params +params = Params() + def parse_gear_shifter(gear, vals): val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral', @@ -108,6 +111,8 @@ class CarState(object): K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 + self.dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True + def update(self, cp): # update prevs, update must run once per loop self.prev_left_blinker_on = self.left_blinker_on @@ -173,3 +178,10 @@ class CarState(object): self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 else: self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM']) + + if self.dragon_toyota_stock_dsu and self.generic_toggle and self.main_on: + self.pcm_acc_active = True + if self.standstill: + self.pcm_acc_status = 7 + else: + self.pcm_acc_status = 1 \ No newline at end of file diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 44cb89068..d1b8397aa 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -33,6 +33,8 @@ class CarInterface(object): if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) + self.dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True + @staticmethod def compute_gb(accel, speed): return float(accel) / 3.0 @@ -370,18 +372,19 @@ class CarInterface(object): elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # DragonAllowGas - if params.get("DragonAllowGas") == "0": - # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + if self.dragon_toyota_stock_dsu: + # DragonAllowGas + if params.get("DragonAllowGas") == "0": + # disable on pedals rising edge or when brake is pressed and speed isn't zero + if (ret.gasPressed and not self.gas_pressed_prev) or \ + (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - else: - if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + if ret.gasPressed: + events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) + else: + if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) ret.events = events diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b2892b49d..1b2d3facd 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -479,6 +479,8 @@ def controlsd_thread(gctx=None): prof = Profiler(False) # off by default + dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True + while True: start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) @@ -505,9 +507,10 @@ def controlsd_thread(gctx=None): if not CS.canValid: events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - # Only allow engagement with brake pressed when stopped behind another stopped car - if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: - events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if dragon_toyota_stock_dsu: + # Only allow engagement with brake pressed when stopped behind another stopped car + if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: + events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if not read_only: # update control state diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 6bdb0179f..8adcc72cd 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -24,6 +24,7 @@ default_conf = { 'DragonCachedVIN': '', # for cache car 'DragonAllowGas': '0', 'DragonBBUI': '0', + 'DragonToyotaStockDSU': '0', } deprecated_conf = {