Merge branch 'devel-en' into devel-zhs

# Conflicts:
#	apk/ai.comma.plus.offroad.apk

common/params.py

@@ -98,6 +98,7 @@ keys = {
   "DragonCachedVIN": [TxType.PERSISTENT],
   "DragonAllowGas": [TxType.PERSISTENT],
   "DragonBBUI": [TxType.PERSISTENT],
+  "DragonToyotaStockDSU": [TxType.PERSISTENT],
 }
 
 

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,

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)):

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
 

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

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
 

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

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

selfdrive/dragonpilot/dragonconf/__init__.py

@@ -24,6 +24,7 @@ default_conf = {
   'DragonCachedVIN': '', # for cache car
   'DragonAllowGas': '0',
   'DragonBBUI': '0',
+  'DragonToyotaStockDSU': '0',
 }
 
 deprecated_conf = {