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

simplify alc logic

Browse Source
This commit is contained in:
dragonpilot
2020-01-19 20:29:24 +10:00
parent a801285d3d
commit 019f70c086
1 changed files with 35 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/pathplanner.py
+35 -39
View File
@@ -92,22 +92,24 @@ class PathPlanner():
cur_time = sec_since_boot()
if cur_time - self.last_ts > 5.:
self.dragon_assisted_lc_enabled = True if self.params.get("DragonEnableAssistedLC", encoding='utf8') == "1" else False
self.dragon_auto_lc_enabled = True if self.params.get("DragonEnableAutoLC", encoding='utf8') == "1" else False
# adjustable assisted lc min speed
self.dragon_assisted_lc_min_mph = float(self.params.get("DragonAssistedLCMinMPH", encoding='utf8'))
if self.dragon_assisted_lc_min_mph < 0:
self.dragon_assisted_lc_min_mph = 0.
# adjustable auto lc min speed
self.dragon_auto_lc_min_mph = float(self.params.get("DragonAutoLCMinMPH", encoding='utf8'))
if self.dragon_auto_lc_min_mph < 0:
self.dragon_auto_lc_min_mph = 0.
# when auto lc is smaller than assisted lc, we set assisted lc to the same speed as auto lc
if self.dragon_auto_lc_min_mph < self.dragon_assisted_lc_min_mph:
self.dragon_assisted_lc_min_mph = self.dragon_auto_lc_min_mph
# adjustable auto lc delay
self.dragon_auto_lc_delay = float(self.params.get("DragonAutoLCDelay", encoding='utf8'))
if self.dragon_auto_lc_delay < 0:
self.dragon_auto_lc_delay = 0.
if self.dragon_assisted_lc_enabled:
self.dragon_auto_lc_enabled = True if self.params.get("DragonEnableAutoLC", encoding='utf8') == "1" else False
# adjustable assisted lc min speed
self.dragon_assisted_lc_min_mph = int(self.params.get("DragonAssistedLCMinMPH", encoding='utf8'))
if self.dragon_assisted_lc_min_mph < 0:
self.dragon_assisted_lc_min_mph = 0
if self.dragon_auto_lc_enabled:
# adjustable auto lc min speed
self.dragon_auto_lc_min_mph = int(self.params.get("DragonAutoLCMinMPH", encoding='utf8'))
if self.dragon_auto_lc_min_mph < 0:
self.dragon_auto_lc_min_mph = 0
# when auto lc is smaller than assisted lc, we set assisted lc to the same speed as auto lc
if self.dragon_auto_lc_min_mph < self.dragon_assisted_lc_min_mph:
self.dragon_assisted_lc_min_mph = self.dragon_auto_lc_min_mph
# adjustable auto lc delay
self.dragon_auto_lc_delay = int(self.params.get("DragonAutoLCDelay", encoding='utf8'))
if self.dragon_auto_lc_delay < 0:
self.dragon_auto_lc_delay = 0
self.last_ts = cur_time
v_ego = sm['carState'].vEgo
@@ -126,13 +128,9 @@ class PathPlanner():
# Lane change logic
lane_change_direction = LaneChangeDirection.none
one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker
below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN
below_lane_change_speed = not self.dragon_assisted_lc_enabled or v_ego < self.dragon_assisted_lc_min_mph * CV.MPH_TO_MS
if not active:
self.lane_change_state = LaneChangeState.off
elif active and self.dragon_auto_lc_enabled and self.lane_change_timer > 13.0:
self.lane_change_state = LaneChangeState.off
elif active and self.lane_change_timer > 10.0:
if not active or self.lane_change_timer > LANE_CHANGE_TIME_MAX:
self.lane_change_state = LaneChangeState.off
else:
if sm['carState'].leftBlinker:
@@ -147,23 +145,21 @@ 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:
# we allow auto lc when speed is > 60mph / 96.5kph
if self.dragon_auto_lc_enabled and v_ego >= self.dragon_auto_lc_min_mph * CV.MPH_TO_MS:
self.dragon_auto_lc_allowed = True
if not below_lane_change_speed and self.dragon_auto_lc_enabled and v_ego >= self.dragon_auto_lc_min_mph * CV.MPH_TO_MS:
# we allow auto lc when speed reached dragon_auto_lc_min_mph
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 + self.dragon_auto_lc_delay
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
if self.dragon_auto_lc_timer is None:
# we only set timer when in preLaneChange state, dragon_auto_lc_delay delay
if self.lane_change_state == LaneChangeState.preLaneChange:
self.dragon_auto_lc_timer = cur_time + self.dragon_auto_lc_delay
elif cur_time > self.dragon_auto_lc_timer:
# if timer is up, we set torque_applied to True to fake user input
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:
@@ -171,7 +167,7 @@ class PathPlanner():
# State transitions
# off
if self.dragon_assisted_lc_enabled and self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker:
if self.dragon_assisted_lc_enabled and self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed:
self.lane_change_state = LaneChangeState.preLaneChange
# pre