mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-23 06:52:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

update auto_lc logic, fix incorrect auto_lc alert.

Browse Source
This commit is contained in:
dragonpilot
2019-12-18 11:57:11 +10:00
parent 2862f3006c
commit 695d5746fe
3 changed files with 21 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+1 -1
View File
@@ -46,7 +46,7 @@ def add_lane_change_event(events, path_plan):
event_name = 'preAutoLaneChangeLeft'
events.append(create_event(event_name, [ET.WARNING]))
else:
event_name = 'preLaneChangeLeft'
event_name = 'preLaneChangeRight'
if path_plan.autoLCAllowed:
event_name = 'preAutoLaneChangeRight'
events.append(create_event(event_name, [ET.WARNING]))
selfdrive/controls/lib/alerts.py
+3 -3
View File
@@ -784,20 +784,20 @@ ALERTS = [
"Left Auto Lane Change will engage in 3 seconds",
"Monitor Other Vehicles",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1, alert_rate=0.75),
Alert(
"preAutoLaneChangeRight",
"Right Auto Lane Change will engage in 3 seconds",
"Monitor Other Vehicles",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75),
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1, alert_rate=0.75),
Alert(
"autoLaneChange",
"Changing Lane",
"Monitor Other Vehicles",
AlertStatus.normal, AlertSize.mid,
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .0, .1, .1),
Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning2, .1, .1, .1),
]
selfdrive/controls/lib/pathplanner.py
+17 -13
View File
@@ -126,24 +126,32 @@ class PathPlanner():
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
# dragonpilot auto lc
if self.dragon_assisted_lc_enabled:
# set torque_applied to Ture if auto_lc is enabled and time is up.
if self.dragon_auto_lc_enabled and self.dragon_auto_lc_timer is not None and cur_time > self.dragon_auto_lc_timer:
torque_applied = True
# only allow auto lane change above 40 mph / 65kph
# we allow auto lc when speed is > 40mph / 65kph
if self.dragon_auto_lc_enabled and v_ego >= 40 * CV.MPH_TO_MS:
self.dragon_auto_lc_allowed = True
if self.dragon_auto_lc_timer is None:
# we only set timer when in preLaneChange state, 2 secs delay
if self.lane_change_state == LaneChangeState.preLaneChange:
self.dragon_auto_lc_timer = cur_time + 2.
else:
# if timer is up, we set torque_applied to True to fake user input
if cur_time > self.dragon_auto_lc_timer:
torque_applied = True
else:
# if too slow, we reset all the variables
self.dragon_auto_lc_allowed = False
self.dragon_auto_lc_timer = None
# we reset the timers when torque is applied regardless
if torque_applied:
self.dragon_auto_lc_timer = None
# State transitions
if self.dragon_assisted_lc_enabled and self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker:
self.lane_change_state = LaneChangeState.preLaneChange
# only allow auto lane change above 40 mph / 65kph
if self.dragon_auto_lc_allowed:
self.dragon_auto_lc_timer = cur_time + 3.
# pre
elif self.lane_change_state == LaneChangeState.preLaneChange and not one_blinker:
@@ -158,17 +166,13 @@ class PathPlanner():
# finishing
elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2:
self.lane_change_state = LaneChangeState.preLaneChange
self.dragon_auto_lc_allowed = False
# when finishing, we reset timer to none.
self.dragon_auto_lc_timer = None
# Don't allow starting lane change below 24 mph / 40kph
if (v_ego < 24 * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange):
self.lane_change_state = LaneChangeState.off
# always reset timer if lane change state is off
if self.lane_change_state == LaneChangeState.off:
self.dragon_auto_lc_timer = None
if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]:
self.lane_change_timer = 0.0
else: