Add back stock toyota LDA

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