From ae193f20cae636c3f4e3ce4e7a1683a100b854ea Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Tue, 10 Mar 2020 16:26:49 +1000 Subject: [PATCH] add subaru support: * steering on signal. * ability to disable steer ctrl. * allow gas. --- selfdrive/car/subaru/carcontroller.py | 35 ++++++++++++++++++++++- selfdrive/car/subaru/interface.py | 40 +++++++++++++++++++++++---- 2 files changed, 69 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index dce4df810..fe1f309b0 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/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 diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index d4388727b..021adb1a9 100644 --- a/selfdrive/car/subaru/interface.py +++ b/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