Skip to content
Permalink
Browse files

Add lane departure warning on dashboard for Toyota (#605)

* Add lane departure alert in controlsd

* Need init values for LDA

* Add lane departure in interface.py

* Include LDA in CarControler

* Add logic for LDA in toyotacan

* Add speed condition and comments for LDA

* Correct right CS.vEgo

* Correct rPoly spelling

* Add left and rightLaneDepart to HUDControl in car.capnp

* Add left and rightLane_Depart in UI function

* set controlsd priority

* revert

* There must be a line to depart from

* Include changes from @pd0wm

* Remove redundant False allocation

leftLaneDepart and rightLaneDepart as False by default according to @pd0wm

* Modify variable names

right_lane_depart and left_lane_depart to conform with python naming convention

* Modify variable names

right_lane_depart and left_lane_depart to conform with python naming convention

* Wrap lane departure warning in one bool
  • Loading branch information...
arne182 authored and pd0wm committed May 6, 2019
1 parent 340e0f4 commit f5044670fa7665bacab49d438e9631191312b833
@@ -247,6 +247,8 @@ struct CarControl {
audibleAlert @5: AudibleAlert;
rightLaneVisible @6: Bool;
leftLaneVisible @7: Bool;
rightLaneDepart @8: Bool;
leftLaneDepart @9: Bool;

enum VisualAlert {
# these are the choices from the Honda
@@ -125,7 +125,8 @@ def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_
self.packer = CANPacker(dbc_name)

def update(self, sendcan, enabled, CS, frame, actuators,
pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera, left_line, right_line, lead):
pcm_cancel_cmd, hud_alert, audible_alert, forwarding_camera,
left_line, right_line, lead, left_lane_depart, right_lane_depart):

# *** compute control surfaces ***

@@ -246,7 +247,7 @@ def update(self, sendcan, enabled, CS, frame, actuators,
send_ui = False

if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus:
can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line))
can_sends.append(create_ui_command(self.packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart))

if frame % 100 == 0 and ECU.DSU in self.fake_ecus:
can_sends.append(create_fcw_command(self.packer, fcw))
@@ -369,7 +369,8 @@ def apply(self, c):
self.CC.update(self.sendcan, c.enabled, self.CS, self.frame,
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
c.hudControl.audibleAlert, self.forwarding_camera,
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible)
c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)

self.frame += 1
return False
@@ -89,10 +89,11 @@ def create_fcw_command(packer, fcw):
return packer.make_can_msg("ACC_HUD", 0, values)


def create_ui_command(packer, steer, sound1, sound2, left_line, right_line):
def create_ui_command(packer, steer, sound1, sound2, left_line, right_line, left_lane_depart, right_lane_depart):
values = {
"RIGHT_LINE": 1 if right_line else 2,
"LEFT_LINE": 1 if left_line else 2,
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
"BARRIERS" : 3 if left_lane_depart or right_lane_depart else 0,
"SET_ME_X0C": 0x0c,
"SET_ME_X2C": 0x2c,
"SET_ME_X38": 0x38,
@@ -307,6 +307,10 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
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)
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
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.visualAlert = AM.visual_alert
CC.hudControl.audibleAlert = AM.audible_alert

@@ -20,7 +20,10 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_
class PathPlanner(object):
def __init__(self, CP):
self.MP = ModelParser()


self.l_poly = [0., 0., 0., 0.]
self.r_poly = [0., 0., 0., 0.]

self.last_cloudlog_t = 0

context = zmq.Context()

0 comments on commit f504467

Please sign in to comment.
You can’t perform that action at this time.