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 20f726461..69c4e62a3 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -5,6 +5,7 @@ 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() @@ -111,9 +112,16 @@ 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 + 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 diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 0e01e1b59..2dc5a5b80 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -33,7 +33,11 @@ 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 + # 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): @@ -250,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) @@ -351,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])) @@ -374,7 +386,7 @@ class CarInterface(object): if not self.dragon_toyota_stock_dsu: # 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/controls/controlsd.py b/selfdrive/controls/controlsd.py index bcdf6c28d..3e097b763 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -479,9 +479,17 @@ def controlsd_thread(gctx=None): prof = Profiler(False) # off by default - dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True + # 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) 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