retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Cleanup Toyota LDW

Willem Melching 2019-05-07 22:27:23 -07:00
parent 184ba93833
commit 1da59216b0
1 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
selfdrive/controls/controlsd.py
View File

@ -11,6 +11,7 @@ import selfdrive.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.car.car_helpers import get_car
from selfdrive.controls.lib.model_parser import CAMERA_OFFSET
from selfdrive.controls.lib.drive_helpers import learn_angle_model_bias, \
get_events, \
create_event, \
@ -305,12 +306,21 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
CC.hudControl.speedVisible = isEnabled(state)
CC.hudControl.lanesVisible = isEnabled(state)
CC.hudControl.leadVisible = plan.hasLead
CC.hudControl.rightLaneVisible = bool(path_plan.pathPlan.rProb > 0.5)
CC.hudControl.leftLaneVisible = bool(path_plan.pathPlan.lProb > 0.5)
right_lane_visible = path_plan.pathPlan.rProb > 0.5
left_lane_visible = path_plan.pathPlan.lProb > 0.5
CC.hudControl.rightLaneVisible = bool(right_lane_visible)
CC.hudControl.leftLaneVisible = bool(left_lane_visible)
blinker = CS.leftBlinker or CS.rightBlinker
ldw_allowed = CS.vEgo > 12.5 and not blinker
if len(list(path_plan.pathPlan.rPoly)) == 4:
CC.hudControl.rightLaneDepart = bool(path_plan.pathPlan.rPoly[3] > -1.11 and not CS.rightBlinker and CS.vEgo > 12.5 and path_plan.pathPlan.rProb > 0.5) # Speed needs to be above 12.5m/s for LDA and only if blinker if off
CC.hudControl.rightLaneDepart = bool(ldw_allowed and path_plan.pathPlan.rPoly[3] > -(1 + CAMERA_OFFSET) and right_lane_visible)
if len(list(path_plan.pathPlan.lPoly)) == 4:
CC.hudControl.leftLaneDepart = bool(path_plan.pathPlan.lPoly[3] < 1.05 and not CS.leftBlinker and CS.vEgo > 12.5 and path_plan.pathPlan.lProb > 0.5) # CAMERA_OFFSET 6cm making it to detect if line is within 15cm of the wheel
CC.hudControl.leftLaneDepart = bool(ldw_allowed and path_plan.pathPlan.lPoly[3] < (1 - CAMERA_OFFSET) and left_lane_visible)
CC.hudControl.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert