diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 1eed3b0f8..ae5c65ce4 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -95,7 +95,6 @@ class CarController(): # dragonpilot self.turning_signal_timer = 0 self.dragon_enable_steering_on_signal = False - self.dragon_allow_gas = False self.dragon_lat_ctrl = True def update(self, enabled, CS, frame, actuators, \ @@ -104,7 +103,6 @@ class CarController(): # dragonpilot, don't check for param too often as it's a kernel call if frame % 500 == 0: self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False - 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 # *** apply brake hysteresis *** diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 0a6144bab..8b7f35688 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -2,6 +2,8 @@ from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11 from selfdrive.car.hyundai.values import Buttons, SteerLimitParams from opendbc.can.packer import CANPacker +from common.params import Params +params = Params() class CarController(): @@ -14,7 +16,17 @@ class CarController(): self.packer = CANPacker(dbc_name) self.steer_rate_limited = False - def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): + # dragonpilot + self.turning_signal_timer = 0 + self.dragon_enable_steering_on_signal = False + self.dragon_lat_ctrl = True + + def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert): + + # dragonpilot, don't check for param too often as it's a kernel call + if frame % 500 == 0: + 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 ### Steering Torque new_steer = actuators.steer * SteerLimitParams.STEER_MAX @@ -30,6 +42,17 @@ class CarController(): can_sends = [] + # dragonpilot + 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: + self.turning_signal_timer -= 1 + steer_req = 0 + + if not self.dragon_lat_ctrl: + steer_req = 0 + self.lkas11_cnt = self.cnt % 0x10 self.clu11_cnt = self.cnt % 0x10 diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 50c31af3d..6494bc806 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 from cereal import car +from common.realtime import sec_since_boot from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -7,6 +8,8 @@ from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_ from selfdrive.car.hyundai.values import ECU, ECU_FINGERPRINT, CAR, get_hud_alerts, FEATURES, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase +from common.params import Params +params = Params() GearShifter = car.CarState.GearShifter ButtonType = car.CarState.ButtonEvent.Type @@ -33,6 +36,13 @@ class CarInterface(CarInterfaceBase): if CarController is not None: self.CC = CarController(self.cp.dbc_name, 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 + @staticmethod def compute_gb(accel, speed): return float(accel) / 3.0 @@ -150,6 +160,14 @@ 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.: + 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.ts_last_check = ts + # ******************* do can recv ******************* self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) @@ -245,7 +263,11 @@ class CarInterface(CarInterfaceBase): events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) if ret.gearShifter == GearShifter.reverse: events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if self.CS.steer_error: + 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])) + elif self.CS.steer_error: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if ret.cruiseState.enabled and not self.cruise_enabled_prev: @@ -253,13 +275,17 @@ class CarInterface(CarInterfaceBase): elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # 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.vEgoRaw > 0.1)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + # DragonAllowGas + if not self.dragon_allow_gas: + if (ret.gasPressed and not self.gas_pressed_prev) or \ + (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1)): + 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])) + else: + if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.1): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.low_speed_alert: events.append(create_event('belowSteerSpeed', [ET.WARNING])) @@ -276,7 +302,7 @@ class CarInterface(CarInterfaceBase): hud_alert = get_hud_alerts(c.hudControl.visualAlert) - can_sends = self.CC.update(c.enabled, self.CS, c.actuators, + can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_alert) - + self.frame += 1 return can_sends diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index e3910928e..11db669eb 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -112,7 +112,6 @@ class CarController(): # dragonpilot self.turning_signal_timer = 0 self.dragon_enable_steering_on_signal = False - # self.dragon_allow_gas = False self.dragon_lat_ctrl = True self.dragon_lane_departure_warning = True self.dragon_toyota_sng_mod = False @@ -123,7 +122,6 @@ class CarController(): # dragonpilot, don't check for param too often as it's a kernel call if frame % 500 == 0: self.dragon_enable_steering_on_signal = True if params.get("DragonEnableSteeringOnSignal", encoding='utf8') == "1" else False - # 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.dragon_lane_departure_warning = False if params.get("DragonToyotaLaneDepartureWarning", encoding='utf8') == "0" else True self.dragon_toyota_sng_mod = True if params.get("DragonToyotaSnGMod", encoding='utf8') == "1" else False diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d30056548..b04341ba0 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -304,7 +304,7 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 # Gas/Brake PID loop - actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, + actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.gasPressed, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 8910747cd..80675a2d6 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -71,7 +71,7 @@ class LongControl(): self.pid.reset() self.v_pid = v_pid - def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP): + def update(self, active, v_ego, gas_pressed, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP): """Update longitudinal control. This updates the state machine and runs a PID loop""" # Actuation limits gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) @@ -85,7 +85,7 @@ class LongControl(): v_ego_pid = max(v_ego, MIN_CAN_SPEED) # Without this we get jumps, CAN bus reports 0 when speed < 0.3 - if self.long_control_state == LongCtrlState.off: + if self.long_control_state == LongCtrlState.off or gas_pressed: self.v_pid = v_ego_pid self.pid.reset() output_gb = 0.