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/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 512b2779d..78ba67c4a 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -87,11 +87,17 @@ class CarController(object): # dragonpilot self.turning_signal_timer = 0 + self.dragon_enable_steering_on_signal = False + self.dragon_allow_gas = False def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, \ hud_alert, snd_beep, snd_chime): + # dragonpilot, don't check for param too often as it's a kernel call + if frame % 100 == 0: + self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True + self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True # *** apply brake hysteresis *** brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) @@ -152,7 +158,7 @@ class CarController(object): can_sends = [] # dragonpilot - if enabled and (CS.left_blinker_on > 0 or CS.right_blinker_on > 0) and params.get("DragonEnableSteeringOnSignal") == "1": + if enabled and (CS.left_blinker_on > 0 or CS.right_blinker_on > 0) and self.dragon_enable_steering_on_signal: self.turning_signal_timer = 100 if self.turning_signal_timer > 0: @@ -189,7 +195,7 @@ class CarController(object): gasPressed = CS.pedal_gas > 0 else: gasPressed = CS.user_gas_pressed - if params.get("DragonAllowGas") == "1" and gasPressed: + if self.dragon_allow_gas and gasPressed: apply_brake = 0 apply_gas = 0 can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 5e108354b..522b0d0c3 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -98,6 +98,11 @@ class CarInterface(object): else: self.compute_gb = compute_gb_honda + # dragonpilot + self.dragon_enable_steering_on_signal = False + self.dragon_allow_gas = False + self.ts_last_check = 0. + @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): @@ -361,6 +366,13 @@ class CarInterface(object): # returns a car.CarState def update(self, c, can_strings): + # dragonpilot, don't check for param too often as it's a kernel call + ts = sec_since_boot() + if ts - self.ts_last_check > 1.: + self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True + self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True + self.ts_last_check = ts + # ******************* do can recv ******************* self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings) self.cp_cam.update_strings(int(sec_since_boot() * 1e9), can_strings) @@ -476,7 +488,7 @@ class CarInterface(object): events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if not self.CS.lkMode: events.append(create_event('manualSteeringRequired', [ET.WARNING])) - elif self.CS.lkMode and (self.CS.left_blinker_on or self.CS.right_blinker_on) and params.get("DragonEnableSteeringOnSignal") == "1": + elif self.CS.lkMode and (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal: events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) elif self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) @@ -505,7 +517,7 @@ class CarInterface(object): events.append(create_event('speedTooLow', [ET.NO_ENTRY])) # DragonAllowGas - if params.get("DragonAllowGas") == "0": + if not self.dragon_allow_gas: # 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)): diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 9a62990a5..96bdc6df1 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -128,10 +128,16 @@ class CarController(object): # dragonpilot self.turning_signal_timer = 0 + self.dragon_enable_steering_on_signal = False + self.dragon_allow_gas = False def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, left_line, right_line, lead, left_lane_depart, right_lane_depart): + # dragonpilot, don't check for param too often as it's a kernel call + if frame % 100 == 0: + self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True + self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True # *** compute control surfaces *** @@ -204,7 +210,7 @@ class CarController(object): can_sends = [] # dragonpilot - if enabled and (CS.left_blinker_on or CS.right_blinker_on) and params.get("DragonEnableSteeringOnSignal") == "1": + if enabled and (CS.left_blinker_on or CS.right_blinker_on) and self.dragon_enable_steering_on_signal: self.turning_signal_timer = 100 if self.turning_signal_timer > 0: @@ -238,7 +244,7 @@ class CarController(object): else: gasPressed = CS.pedal_gas > 0 - if params.get("DragonAllowGas") == "1" and gasPressed: + if self.dragon_allow_gas and gasPressed: apply_accel = 0 apply_gas = 0 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 40be422db..69c4e62a3 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -5,6 +5,10 @@ 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.realtime import sec_since_boot +from common.params import Params +params = Params() + def parse_gear_shifter(gear, vals): val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral', @@ -108,7 +112,16 @@ class CarState(object): K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 + self.dragon_toyota_stock_dsu = False + self.ts_last_check = 0. + def update(self, cp): + # dragonpilot, don't check for param too often as it's a kernel call + ts = sec_since_boot() + if ts - self.ts_last_check > 1.: + self.dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True + self.ts_last_check = ts + # update prevs, update must run once per loop self.prev_left_blinker_on = self.left_blinker_on self.prev_right_blinker_on = self.right_blinker_on @@ -173,3 +186,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..2dc5a5b80 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -33,6 +33,12 @@ class CarInterface(object): if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) + # dragonpilot + self.dragon_toyota_stock_dsu = False + self.dragon_enable_steering_on_signal = False + self.dragon_allow_gas = False + self.ts_last_check = 0. + @staticmethod def compute_gb(accel, speed): return float(accel) / 3.0 @@ -248,6 +254,14 @@ class CarInterface(object): # returns a car.CarState def update(self, c, can_strings): + # dragonpilot, don't check for param too often as it's a kernel call + ts = sec_since_boot() + if ts - self.ts_last_check > 1.: + self.dragon_enable_steering_on_signal = False if params.get("DragonEnableSteeringOnSignal") == "0" else True + self.dragon_allow_gas = False if params.get("DragonAllowGas") == "0" else True + self.dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True + self.ts_last_check = ts + # ******************* do can recv ******************* self.cp.update_strings(int(sec_since_boot() * 1e9), can_strings) @@ -349,7 +363,7 @@ class CarInterface(object): events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == 'reverse' and self.CP.enableDsu: events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if (self.CS.left_blinker_on or self.CS.right_blinker_on) and params.get("DragonEnableSteeringOnSignal") == "1": + if (self.CS.left_blinker_on or self.CS.right_blinker_on) and self.dragon_enable_steering_on_signal: events.append(create_event('manualSteeringRequiredBlinkersOn', [ET.WARNING])) elif self.CS.steer_error: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) @@ -370,18 +384,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 not self.dragon_toyota_stock_dsu: + # DragonAllowGas + if not self.dragon_allow_gas: + # 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..3e097b763 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -479,7 +479,17 @@ def controlsd_thread(gctx=None): prof = Profiler(False) # off by default + # dragonpilot + ts_last_check = 0. + dragon_toyota_stock_dsu = False + while True: + # dragonpilot, don't check for param too often as it's a kernel call + ts = sec_since_boot() + if ts - ts_last_check > 1.: + dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True + ts_last_check = ts + start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) @@ -505,9 +515,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 not 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/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 5ffde39ed..f73805bbc 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -74,6 +74,10 @@ class DriverStatus(): self.terminal_alert_cnt = 0 self._set_timers() + # dragonpilot + self.dp_last_check = 0. + self.dragon_enable_driver_safety_check = True + def _reset_filters(self): self.driver_distraction_filter.x = 0. self.variance_filter.x = 0. @@ -135,6 +139,11 @@ class DriverStatus(): def update(self, events, driver_engaged, ctrl_active, standstill): + # don't check for param too often as it's a kernel call + ts = sec_since_boot() + if ts - self.dp_last_check > 1.: + self.dragon_enable_driver_safety_check = False if params.get("DragonEnableDriverSafetyCheck") == "0" else True + self.dp_last_check = ts driver_engaged |= (self.driver_distraction_filter.x < 0.37 and self.monitor_on) awareness_prev = self.awareness @@ -160,7 +169,7 @@ class DriverStatus(): elif self.awareness <= self.threshold_pre: # pre green alert alert = 'preDriverDistracted' if self.monitor_on else 'preDriverUnresponsive' - if alert is not None and params.get("DragonEnableDriverSafetyCheck") == "1": + if alert is not None and self.dragon_enable_driver_safety_check: events.append(create_event(alert, [ET.WARNING])) return events 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 = {