add subaru support:

* steering on signal.
* ability to disable steer ctrl.
* allow gas.

selfdrive/car/subaru/carcontroller.py

@@ -3,7 +3,9 @@ from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.subaru import subarucan
 from selfdrive.car.subaru.values import DBC
 from opendbc.can.packer import CANPacker
-
+from common.params import Params
+params = Params()
+from selfdrive.dragonpilot.dragonconf import dp_get_last_modified
 
 class CarControllerParams():
   def __init__(self, car_fingerprint):
@@ -32,9 +34,23 @@ class CarController():
     self.params = CarControllerParams(car_fingerprint)
     self.packer = CANPacker(DBC[car_fingerprint]['pt'])
 
+    # dragonpilot
+    self.turning_signal_timer = 0
+    self.dragon_enable_steering_on_signal = False
+    self.dragon_lat_ctrl = True
+    self.dp_last_modified = None
+
   def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
     """ Controls thread """
 
+    # dragonpilot, don't check for param too often as it's a kernel call
+    if frame % 500 == 0:
+      modified = dp_get_last_modified()
+      if self.dp_last_modified != modified:
+        self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False
+        self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True
+        self.dp_last_modified = modified
+
     P = self.params
 
     # Send CAN commands.
@@ -58,6 +74,23 @@ class CarController():
       if not lkas_enabled:
         apply_steer = 0
 
+      # dragonpilot
+      if enabled:
+        if self.dragon_enable_steering_on_signal:
+          if CS.left_blinker_on == 0 and CS.right_blinker_on == 0:
+            self.turning_signal_timer = 0
+          else:
+            self.turning_signal_timer = 100
+
+          if self.turning_signal_timer > 0:
+            self.turning_signal_timer -= 1
+            apply_steer = 0
+        else:
+          self.turning_signal_timer = 0
+
+        if not self.dragon_lat_ctrl:
+          apply_steer = 0
+
       can_sends.append(subarucan.create_steering_control(self.packer, CS.CP.carFingerprint, apply_steer, frame, P.STEER_STEP))
 
       self.apply_steer_last = apply_steer

selfdrive/car/subaru/interface.py

@@ -7,6 +7,10 @@ from selfdrive.car.subaru.values import CAR
 from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.car.interfaces import CarInterfaceBase
+from common.realtime import sec_since_boot
+from common.params import Params
+params = Params()
+from selfdrive.dragonpilot.dragonconf import dp_get_last_modified
 
 ButtonType = car.CarState.ButtonEvent.Type
 
@@ -30,6 +34,14 @@ class CarInterface(CarInterfaceBase):
     if CarController is not None:
       self.CC = CarController(CP.carFingerprint)
 
+    # dragonpilot
+    self.frame = 0
+    self.dragon_enable_steering_on_signal = False
+    self.dragon_allow_gas = False
+    self.ts_last_check = 0.
+    self.dragon_lat_ctrl = True
+    self.dp_last_modified = None
+
   @staticmethod
   def compute_gb(accel, speed):
     return float(accel) / 4.0
@@ -95,6 +107,17 @@ class CarInterface(CarInterfaceBase):
 
   # 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 >= 5.:
+      modified = dp_get_last_modified()
+      if self.dp_last_modified != modified:
+        self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else True
+        self.dragon_allow_gas = True if params.get("DragonAllowGas", encoding='utf8') == "1" else False
+        self.dragon_lat_ctrl = False if params.get("DragonLatCtrl", encoding='utf8') == "0" else True
+        self.dp_last_modified = modified
+      self.ts_last_check = ts
+
     self.pt_cp.update_strings(can_strings)
     self.cam_cp.update_strings(can_strings)
 
@@ -166,17 +189,24 @@ class CarInterface(CarInterfaceBase):
     if ret.doorOpen:
       events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
 
+    if not self.dragon_lat_ctrl:
+      events.append(create_event('manualSteeringRequired', [ET.WARNING]))
+    elif (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]))
+
     if self.CS.acc_active and not self.acc_active_prev:
       events.append(create_event('pcmEnable', [ET.ENABLE]))
     if not self.CS.acc_active:
       events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
 
-    # disable on gas pedal rising edge
-    if (ret.gasPressed and not self.gas_pressed_prev):
-      events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
+    # DragonAllowGas
+    if not self.dragon_allow_gas:
+      # disable on gas pedal rising edge
+      if (ret.gasPressed and not self.gas_pressed_prev):
+        events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
 
-    if ret.gasPressed:
-      events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
+      if ret.gasPressed:
+        events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
 
     ret.events = events