From 4bc5f49c42765123c179e7c0dbba4c89bb3c1965 Mon Sep 17 00:00:00 2001 From: Arne Schwarck Date: Fri, 27 Mar 2020 16:27:33 +0100 Subject: [PATCH] Add back stock toyota LDA --- selfdrive/car/toyota/carcontroller.py | 41 +++++++++++++++++++++++---- 1 file changed, 35 insertions(+), 6 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index b1393888bb6553..4ee94215955a95 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -7,6 +7,9 @@ create_accel_command, create_acc_cancel_command, create_fcw_command from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, SteerLimitParams, TSS2_CAR from opendbc.can.packer import CANPacker +from common.op_params import opParams + +op_params = opParams() VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -73,7 +76,11 @@ def __init__(self, dbc_name, CP, VM): self.blindspot_blink_counter_right = 0 self.blindspot_debug_enabled_left = False self.blindspot_debug_enabled_right = False - + + self.sm = messaging.SubMaster(['pathPlan']) + self.rightLaneDepart = False + self.leftLaneDepart = False + self.last_fault_frame = -200 self.steer_rate_limited = False @@ -94,11 +101,19 @@ def __init__(self, dbc_name, CP, VM): def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, left_line, right_line, lead, left_lane_depart, right_lane_depart): - + self.sm.update(0) + if self.sm.updated['pathPlan']: + blinker = CS.out.leftBlinker or CS.out.rightBlinker + ldw_allowed = CS.out.vEgo > 12.5 and not blinker + CAMERA_OFFSET = op_params.get('camera_offset', 0.06) + right_lane_visible = sm['pathPlan'].rProb > 0.5 + left_lane_visible = sm['pathPlan'].lProb > 0.5 + self.rightLaneDepart = bool(ldw_allowed and sm['pathPlan'].rPoly[3] > -(0.93 + CAMERA_OFFSET) and right_lane_visible) + self.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (0.93 - CAMERA_OFFSET) and left_lane_visible) # *** compute control surfaces *** # gas and brake - + apply_gas = clip(actuators.gas, 0., 1.) if CS.CP.enableGasInterceptor: @@ -130,13 +145,27 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, if CS.steer_state in [9, 25]: self.last_fault_frame = frame - # Cut steering for 2s after fault - if (frame - self.last_fault_frame < 200) or abs(CS.out.steeringRate) > 100 or abs(CS.out.steeringAngle) > 100: + # Cut steering for 1s after fault + if (frame - self.last_fault_frame < 100) or abs(CS.out.steeringRate) > 100 or abs(CS.out.steeringAngle) > 100: new_steer = 0 apply_steer_req = 0 else: apply_steer_req = 1 - + + if not enabled and self.rightLaneDepart and CS.out.vEgo > 12.5 and not CS.out.rightBlinker: + apply_steer = self.last_steer + 3 + apply_steer = min(apply_steer , 800) + #print ("right") + #print (apply_steer) + apply_steer_req = 1 + + if not enabled and self.leftLaneDepart and CS.v_ego > 12.5 and not CS.out.leftBlinker: + apply_steer = self.last_steer - 3 + apply_steer = max(apply_steer , -800) + #print ("left") + #print (apply_steer) + apply_steer_req = 1 + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer