diff --git a/common/params.py b/common/params.py index c1faa903f..072961adb 100755 --- a/common/params.py +++ b/common/params.py @@ -101,6 +101,7 @@ keys = { "DragonAllowGas": [TxType.PERSISTENT], "DragonBBUI": [TxType.PERSISTENT], "DragonToyotaStockDSU": [TxType.PERSISTENT], + "DragonLatCtrl": [TxType.PERSISTENT], } diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index d28bd7787..cd4e8a8fa 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -89,6 +89,7 @@ class CarController(object): 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, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ @@ -98,6 +99,7 @@ class CarController(object): 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 + self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "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) @@ -165,6 +167,9 @@ class CarController(object): self.turning_signal_timer -= 1 lkas_active = False + if not self.dragon_lat_ctrl: + lkas_active = False + # Send steering command. idx = frame % 4 can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 96bdc6df1..7bc38491a 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -130,6 +130,7 @@ class CarController(object): 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, pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, @@ -138,6 +139,7 @@ class CarController(object): 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 + self.dragon_lat_ctrl = False if params.get("DragonLatCtrl") == "0" else True # *** compute control surfaces *** @@ -217,6 +219,20 @@ class CarController(object): self.turning_signal_timer -= 1 apply_steer_req = 0 + if not self.dragon_lat_ctrl: + apply_steer_req = 0 + + if CS.v_ego > 12.5 and (not self.dragon_lat_ctrl or not enabled): + if right_lane_depart and not CS.right_blinker_on: + apply_steer = self.last_steer + 3 + apply_steer = min(apply_steer , 800) + apply_steer_req = 1 + + if left_lane_depart and not CS.left_blinker_on: + apply_steer = self.last_steer - 3 + apply_steer = max(apply_steer , -800) + apply_steer_req = 1 + #*** control msgs *** #print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor) diff --git a/selfdrive/dragonpilot/dragonconf/__init__.py b/selfdrive/dragonpilot/dragonconf/__init__.py index 7757d516c..6edb15c98 100644 --- a/selfdrive/dragonpilot/dragonconf/__init__.py +++ b/selfdrive/dragonpilot/dragonconf/__init__.py @@ -25,6 +25,7 @@ default_conf = { 'DragonAllowGas': '0', 'DragonBBUI': '0', 'DragonToyotaStockDSU': '0', + 'DragonLatCtrl': '1', } deprecated_conf = {