From e2fd3e109bd051e27b8ac8f62ba889a3f8ba77bd Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 10 Aug 2024 11:17:38 -0400 Subject: [PATCH 1/2] more --- selfdrive/controls/controlsd.py | 9 +++++---- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6d1cc2b531..d58a226223 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -829,11 +829,12 @@ class Controls: self.prev_overtaking_accel_engaged = self.overtaking_accel_engaged ttc = self.sm['radarState'].leadOne.dRel / CS.vEgo if CS.vEgo > 0 else 255 overtaking_accel_engaged = self.overtaking_accel and overtaking_accel_allowed and \ - (CS.vEgo > ((60 * CV.KPH_TO_MS) if self.is_metric else (40 * CV.MPH_TO_MS))) and long_plan.hasLead and \ - long_plan.aTarget > -0.2 and not (CS.leftBlinker and CS.rightBlinker) and (0.75 < ttc < 3.0) - if ttc <= 0.75 and self.prev_overtaking_accel_engaged and overtaking_accel_engaged: + (CS.vEgo > ((60 * CV.KPH_TO_MS) if self.is_metric else (40 * CV.MPH_TO_MS))) and \ + not (CS.leftBlinker and CS.rightBlinker) + if ttc < 0.75 and self.prev_overtaking_accel_engaged and overtaking_accel_engaged: overtaking_accel_engaged = False - if overtaking_accel_engaged and not self.prev_overtaking_accel_engaged: + if overtaking_accel_engaged and not self.prev_overtaking_accel_engaged and \ + long_plan.hasLead and long_plan.aTarget > -0.1 and (0.75 < ttc < 3.0): self.overtaking_accel_engaged = True elif not overtaking_accel_engaged: self.overtaking_accel_engaged = False diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 6fdad993ac..d8be674d3a 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -82,7 +82,7 @@ def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard): elif personality==custom.LongitudinalPersonalitySP.aggressive: return 1.0 elif personality==custom.LongitudinalPersonalitySP.overtake: - return 0.3 + return 0.25 else: raise NotImplementedError("Longitudinal personality not supported") From 150609dc83c8e8b3ec2d8927aeafda52c192e121 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 10 Aug 2024 11:57:35 -0400 Subject: [PATCH 2/2] only trigger once? --- selfdrive/controls/controlsd.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d58a226223..baa4783a3d 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -188,6 +188,9 @@ class Controls: self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist") self.overtaking_accel_engaged = False self.prev_overtaking_accel_engaged = False + self.overtaking_accel_allowed = False + self.prev_overtaking_accel_allowed = False + self.overtaking_accel_blocked = False self.accel_personality = self.read_accel_personality_param() @@ -823,16 +826,21 @@ class Controls: long_plan = self.sm['longitudinalPlan'] dm_state = self.sm['driverMonitoringState'] blinker_svs = lat_plan if self.model_use_lateral_planner else model_v2.meta - overtaking_accel_allowed = ((blinker_svs.laneChangeDirection == LaneChangeDirection.right and dm_state.isRHD) or - (blinker_svs.laneChangeDirection == LaneChangeDirection.left and not dm_state.isRHD)) and \ - (blinker_svs.laneChangeState in (LaneChangeState.preLaneChange, LaneChangeState.laneChangeStarting)) + self.prev_overtaking_accel_allowed = self.overtaking_accel_allowed + if not self.overtaking_accel_allowed and not self.prev_overtaking_accel_allowed: + self.overtaking_accel_blocked = False + self.overtaking_accel_allowed = ((blinker_svs.laneChangeDirection == LaneChangeDirection.right and dm_state.isRHD) or + (blinker_svs.laneChangeDirection == LaneChangeDirection.left and not dm_state.isRHD)) and \ + (blinker_svs.laneChangeState in (LaneChangeState.preLaneChange, LaneChangeState.laneChangeStarting)) and \ + not self.overtaking_accel_blocked self.prev_overtaking_accel_engaged = self.overtaking_accel_engaged ttc = self.sm['radarState'].leadOne.dRel / CS.vEgo if CS.vEgo > 0 else 255 - overtaking_accel_engaged = self.overtaking_accel and overtaking_accel_allowed and \ + overtaking_accel_engaged = self.overtaking_accel and self.overtaking_accel_allowed and \ (CS.vEgo > ((60 * CV.KPH_TO_MS) if self.is_metric else (40 * CV.MPH_TO_MS))) and \ not (CS.leftBlinker and CS.rightBlinker) if ttc < 0.75 and self.prev_overtaking_accel_engaged and overtaking_accel_engaged: overtaking_accel_engaged = False + self.overtaking_accel_blocked = True if overtaking_accel_engaged and not self.prev_overtaking_accel_engaged and \ long_plan.hasLead and long_plan.aTarget > -0.1 and (0.75 < ttc < 3.0): self.overtaking_accel_engaged = True