diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 55f9ba2b9..ffcf51316 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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])) diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index ad28db57f..fe80435da 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -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), ] diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 68d1e6e2c..a1f6db18d 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -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: