Block lane change start on blindspot detection (#1712)

* use BSM to block lane change start

* remove duplicate entry

* add approaching
old-commit-hash: c4a3d7afb0
This commit is contained in:
Adeeb Shihadeh 2020-06-24 17:31:09 -07:00 committed by GitHub
parent 498cd9f47a
commit 55225dcd56
5 changed files with 25 additions and 9 deletions

View File

@ -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)

View File

@ -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)

View File

@ -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",

View File

@ -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

View File

@ -1 +1 @@
12861083f7fdcfd2876d7f8102f1853a3248e2a7
f0ff304da1765fd9cfd36d8c730f280315df91bf