diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 02bd893eeb..d4cc71640f 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -104,8 +104,8 @@ class CarState(CarStateBase): self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] if self.CP.carFingerprint in TSS2_CAR: - ret.leftBlindspot = cp.vl["BSM"]['L_ADJACENT'] == 1 - ret.rightBlindspot = cp.vl["BSM"]['R_ADJACENT'] == 1 + ret.leftBlindspot = (cp.vl["BSM"]['L_ADJACENT'] == 1) or (cp.vl["BSM"]['L_APPROACHING'] == 1) + ret.rightBlindspot = (cp.vl["BSM"]['R_ADJACENT'] == 1) or (cp.vl["BSM"]['R_APPROACHING'] == 1) return ret @@ -173,7 +173,9 @@ class CarState(CarStateBase): if CP.carFingerprint in TSS2_CAR: signals += [("L_ADJACENT", "BSM", 0)] + signals += [("L_APPROACHING", "BSM", 0)] signals += [("R_ADJACENT", "BSM", 0)] + signals += [("R_APPROACHING", "BSM", 0)] return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9de1069169..86f6ff0f56 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -183,10 +183,15 @@ class Controls: # Handle lane change if self.sm['pathPlan'].laneChangeState == LaneChangeState.preLaneChange: - if self.sm['pathPlan'].laneChangeDirection == LaneChangeDirection.left: - self.events.add(EventName.preLaneChangeLeft) + direction = self.sm['pathPlan'].laneChangeDirection + if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ + (CS.rightBlindspot and direction == LaneChangeDirection.right): + self.events.add(EventName.laneChangeBlocked) else: - self.events.add(EventName.preLaneChangeRight) + if direction == LaneChangeDirection.left: + self.events.add(EventName.preLaneChangeLeft) + else: + self.events.add(EventName.preLaneChangeRight) elif self.sm['pathPlan'].laneChangeState in [LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing]: self.events.add(EventName.laneChange) diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 805471903c..afa18ee207 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -206,8 +206,6 @@ EVENTS = { EventName.gasPressed: {ET.PRE_ENABLE: None}, - EventName.laneChangeBlocked: {}, - # ********** events only containing alerts displayed in all states ********** EventName.debugAlert: { @@ -443,6 +441,14 @@ EVENTS = { Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), }, + EventName.laneChangeBlocked: { + ET.WARNING: Alert( + "Car Detected in Blindspot", + "Monitor Other Vehicles", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1), + }, + EventName.laneChange: { ET.WARNING: Alert( "Changing Lane", diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 40e6c38002..bfe8597df6 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -114,6 +114,9 @@ class PathPlanner(): ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) + blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or + (sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left)) + lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions @@ -126,7 +129,7 @@ class PathPlanner(): elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off - elif torque_applied: + elif torque_applied and not blindspot_detected: self.lane_change_state = LaneChangeState.laneChangeStarting # starting diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 31fdb6ce07..de32dae9b2 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -12861083f7fdcfd2876d7f8102f1853a3248e2a7 \ No newline at end of file +f0ff304da1765fd9cfd36d8c730f280315df91bf \ No newline at end of file