From 019f70c0860e9be805abb0b3c79197c34fbd77ec Mon Sep 17 00:00:00 2001 From: dragonpilot Date: Sun, 19 Jan 2020 20:29:24 +1000 Subject: [PATCH] simplify alc logic --- selfdrive/controls/lib/pathplanner.py | 74 +++++++++++++-------------- 1 file changed, 35 insertions(+), 39 deletions(-) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index d4e550a41..605fd0b7a 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -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