From 2a383c65b88c1b09e99c60ac6590597424d179a8 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 6 Aug 2024 17:26:37 -0400 Subject: [PATCH 1/6] a tad less aggressive --- selfdrive/controls/lib/longitudinal_planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 976944228e..852a6bd1bf 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -82,7 +82,7 @@ class LongitudinalPlanner: self.dt = dt self.a_desired = init_a - v_ego_sec = 0.5 if CP.carName == "hyundai" else 2.0 + v_ego_sec = 0.6 if CP.carName == "hyundai" else 2.0 self.v_desired_filter = FirstOrderFilter(init_v, v_ego_sec, self.dt) self.v_model_error = 0.0 From c6fa48d166f55c962ba2e71319c08045ade1d5c7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 6 Aug 2024 18:54:46 -0400 Subject: [PATCH 2/6] no starting state for non-ICE --- selfdrive/car/hyundai/interface.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 35ad043a69..d667c2a639 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -102,15 +102,15 @@ class CarInterface(CarInterfaceBase): ret.pcmCruise = not ret.openpilotLongitudinalControl ret.stoppingControl = True - ret.startingState = True ret.vEgoStarting = 0.5 + ret.startAccel = 1.5 ret.stopAccel = -1.0 ret.longitudinalActuatorDelay = 0.5 if ret.flags & (HyundaiFlags.HYBRID | HyundaiFlags.EV): - ret.startAccel = 1.0 + ret.startingState = False else: - ret.startAccel = 1.5 + ret.startingState = True if DBC[ret.carFingerprint]["radar"] is None: if ret.spFlags & (HyundaiFlagsSP.SP_ENHANCED_SCC | HyundaiFlagsSP.SP_CAMERA_SCC_LEAD): From 3c168d0975356c0557b838cd3db99a04b652fc79 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 6 Aug 2024 20:22:10 -0400 Subject: [PATCH 3/6] send it --- cereal/custom.capnp | 1 + common/params.cc | 1 + .../lib/longitudinal_mpc_lib/long_mpc.py | 8 +++++++- .../controls/lib/longitudinal_planner.py | 20 ++++++++++++++++--- selfdrive/controls/plannerd.py | 2 +- .../offroad/settings/sunnypilot_settings.cc | 8 +++++++- 6 files changed, 34 insertions(+), 6 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 8825cf6f07..8bcf50f34e 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -16,6 +16,7 @@ enum LongitudinalPersonalitySP { moderate @1; standard @2; relaxed @3; + overtake @4; } enum AccelerationPersonality { diff --git a/common/params.cc b/common/params.cc index 1f71c36df1..92c2742921 100644 --- a/common/params.cc +++ b/common/params.cc @@ -291,6 +291,7 @@ std::unordered_map keys = { {"OsmLocationUrl", PERSISTENT}, {"OsmWayTest", PERSISTENT}, {"OsmDownloadedDate", PERSISTENT}, + {"OvertakingAccelerationAssist", PERSISTENT}, {"PathOffset", PERSISTENT | BACKUP}, {"PauseLateralSpeed", PERSISTENT | BACKUP}, {"QuietDrive", PERSISTENT | BACKUP}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index a2df527a55..0c67119d14 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -66,6 +66,8 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): return 0.8 elif personality==custom.LongitudinalPersonalitySP.aggressive: return 0.6 + elif personality==custom.LongitudinalPersonalitySP.overtake: + return 0.25 else: raise NotImplementedError("Longitudinal personality not supported") @@ -79,6 +81,8 @@ def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard): return 1.25 elif personality==custom.LongitudinalPersonalitySP.aggressive: return 1.0 + elif personality==custom.LongitudinalPersonalitySP.overtake: + return 0.5 else: raise NotImplementedError("Longitudinal personality not supported") @@ -358,9 +362,11 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard, dynamic_personality=False): + def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard, + dynamic_personality=False, overtaking_acceleration_assist=False): v_ego = self.x0[1] t_follow = get_dynamic_personality(v_ego, personality) if dynamic_personality else get_T_FOLLOW(personality) + t_follow = get_T_FOLLOW(custom.LongitudinalPersonalitySP.overtake) if overtaking_acceleration_assist else t_follow self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 852a6bd1bf..9c5b770433 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -3,7 +3,7 @@ import math import numpy as np from openpilot.common.numpy_fast import clip, interp from openpilot.common.params import Params -from cereal import car +from cereal import car, custom import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV @@ -36,6 +36,8 @@ _A_TOTAL_MAX_BP = [20., 40.] EventName = car.CarEvent.EventName +LaneChangeState = log.LaneChangeState +LaneChangeDirection = log.LaneChangeDirection def get_max_accel(v_ego): @@ -102,6 +104,8 @@ class LongitudinalPlanner: self.dynamic_experimental_controller = DynamicExperimentalController() self.accel_controller = AccelController() + self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist") + def read_param(self): try: self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl")) @@ -109,6 +113,8 @@ class LongitudinalPlanner: self.dynamic_experimental_controller = DynamicExperimentalController() self.accel_controller = AccelController() + self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist") + @staticmethod def parse_model(model_msg, model_error): if (len(model_msg.position.x) == ModelConstants.IDX_N and @@ -168,6 +174,13 @@ class LongitudinalPlanner: accel_limits = [ACCEL_MIN, ACCEL_MAX] accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] + lat_plan = sm['lateralPlanDEPRECATED'] + dm_state = sm['driverMonitoringState'] + overtaking_accel_allowed = ((lat_plan.laneChangeDirection == LaneChangeDirection.right and dm_state.isRHD) or + (lat_plan.laneChangeDirection == LaneChangeDirection.left and not dm_state.isRHD)) and \ + (lat_plan.laneChangeState in (LaneChangeState.preLaneChange, LaneChangeState.laneChangeStarting)) + overtaking_accel_engaged = self.overtaking_accel and overtaking_accel_allowed and v_ego > 40 * CV.MPH_TO_MS + if reset_state: self.v_desired_filter.x = v_ego # Clip aEgo to cruise limits to prevent large accelerations when becoming active @@ -190,11 +203,12 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint, personality=sm['controlsStateSP'].personality) + self.mpc.set_weights(prev_accel_constraint, personality=custom.LongitudinalPersonalitySP.overtake if overtaking_accel_engaged else sm['controlsStateSP'].personality) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsStateSP'].personality, dynamic_personality=sm['controlsStateSP'].dynamicPersonality) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsStateSP'].personality, + dynamic_personality=sm['controlsStateSP'].dynamicPersonality, overtaking_acceleration_assist=overtaking_accel_engaged) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 6c51b38b13..daef841c19 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -32,7 +32,7 @@ def plannerd_thread(): pm = messaging.PubMaster(['longitudinalPlan', 'longitudinalPlanSP'] + lateral_planner_svs) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'longitudinalPlan', 'navInstruction', 'longitudinalPlanSP', - 'liveMapDataSP', 'e2eLongStateSP', 'controlsStateSP'] + lateral_planner_svs, + 'liveMapDataSP', 'e2eLongStateSP', 'controlsStateSP', 'driverMonitoringState'] + lateral_planner_svs, poll='modelV2', ignore_avg_freq=['radarState']) while True: diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.cc index c363e78844..2bbbbc8872 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.cc @@ -49,6 +49,12 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) { tr("While in Auto Lane, switch to Laneless for current/future curves."), "../assets/offroad/icon_blank.png", }, + { + "OvertakingAccelerationAssist", + tr("Overtaking Acceleration Assist"), + tr("Overtaking Acceleration Assist will operate when the turn signal indicator is turned on to the left (left-hand drive) or turned on to the right (right-hand drive) while Smart Cruise Control is operating."), + "../assets/offroad/icon_blank.png", + }, { "EnableSlc", tr("Speed Limit Control (SLC)"), @@ -298,7 +304,7 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) { list->addItem(dlp_settings); } - if (param == "VisionCurveLaneless") { + if (param == "OvertakingAccelerationAssist") { list->addItem(laneChangeSettingsLayout); list->addItem(horizontal_line()); From 1eafceb321025d5a93364bf01c383b597ec53ba7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 6 Aug 2024 20:46:06 -0400 Subject: [PATCH 4/6] fix --- selfdrive/controls/lib/longitudinal_planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 9c5b770433..5536b0c1c8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -3,7 +3,7 @@ import math import numpy as np from openpilot.common.numpy_fast import clip, interp from openpilot.common.params import Params -from cereal import car, custom +from cereal import car, log, custom import cereal.messaging as messaging from openpilot.common.conversions import Conversions as CV From fa4ae1367f236252469b528e9d18a773569ca15a Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 6 Aug 2024 22:54:13 -0400 Subject: [PATCH 5/6] back to stock --- selfdrive/car/hyundai/interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index d667c2a639..43b9e28664 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -103,7 +103,7 @@ class CarInterface(CarInterfaceBase): ret.stoppingControl = True ret.vEgoStarting = 0.5 - ret.startAccel = 1.5 + ret.startAccel = 1.0 ret.stopAccel = -1.0 ret.longitudinalActuatorDelay = 0.5 From bacb49d8ee5043053288b95f0875827bc14c65b0 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 6 Aug 2024 23:42:31 -0400 Subject: [PATCH 6/6] fix --- cereal/custom.capnp | 1 + selfdrive/controls/controlsd.py | 17 +++++++++++++++-- .../lib/longitudinal_mpc_lib/long_mpc.py | 2 +- selfdrive/controls/lib/longitudinal_planner.py | 14 +------------- .../qt/offroad/settings/sunnypilot_settings.cc | 1 + 5 files changed, 19 insertions(+), 16 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 8bcf50f34e..0ef005b07f 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -40,6 +40,7 @@ struct ControlsStateSP @0x81c2f05a394cf4af { personality @8 :LongitudinalPersonalitySP; dynamicPersonality @9 :Bool; accelPersonality @10 :AccelerationPersonality; + overtakingAccelerationAssist @11 :Bool; lateralControlState :union { indiState @1 :LateralINDIState; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6c5cf6ee5a..7b5d90cbbc 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -185,6 +185,7 @@ class Controls: self.custom_model_metadata.capabilities & ModelCapabilities.LateralPlannerSolution self.dynamic_personality = self.params.get_bool("DynamicPersonality") + self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist") self.accel_personality = self.read_accel_personality_param() @@ -811,11 +812,21 @@ class Controls: # Curvature & Steering angle lp = self.sm['liveParameters'] - lp_mono_time_svs = 'lateralPlanDEPRECATED' if self.model_use_lateral_planner else 'modelV2' + dh = 'lateralPlanDEPRECATED' if self.model_use_lateral_planner else 'modelV2' steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) + lc_svs = self.sm[dh] + long_plan = self.sm['longitudinalPlan'].hasLead + dm_state = self.sm['driverMonitoringState'] + overtaking_accel_allowed = ((lc_svs.laneChangeDirection == LaneChangeDirection.right and dm_state.isRHD) or + (lc_svs.laneChangeDirection == LaneChangeDirection.left and not dm_state.isRHD)) and \ + (lc_svs.laneChangeState in (LaneChangeState.preLaneChange, LaneChangeState.laneChangeStarting)) + 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) + # controlsState dat = messaging.new_message('controlsState') dat.valid = CS.canValid @@ -830,7 +841,7 @@ class Controls: controlsState.alertSound = current_alert.audible_alert controlsState.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] - controlsState.lateralPlanMonoTime = self.sm.logMonoTime[lp_mono_time_svs] + controlsState.lateralPlanMonoTime = self.sm.logMonoTime[dh] controlsState.enabled = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.enabled or CS.cruiseState.enabled) and CS.gearShifter not in [GearShifter.park, GearShifter.reverse] controlsState.active = not (CS.brakePressed and (not self.CS_prev.brakePressed or not CS.standstill)) and (self.active or CS.cruiseState.enabled) controlsState.curvature = curvature @@ -868,6 +879,7 @@ class Controls: controlsStateSP.personality = self.personality controlsStateSP.dynamicPersonality = self.dynamic_personality controlsStateSP.accelPersonality = self.accel_personality + controlsStateSP.overtakingAccelerationAssist = overtaking_accel_engaged if self.enable_nnff and lat_tuning == 'torque': controlsStateSP.lateralControlState.torqueState = self.LaC.pid_long_sp @@ -934,6 +946,7 @@ class Controls: self.reverse_acc_change = self.params.get_bool("ReverseAccChange") self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl") + self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist") if self.sm.frame % int(2.5 / DT_CTRL) == 0: self.live_torque = self.params.get_bool("LiveTorque") diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 0c67119d14..cfc1f6cf09 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -67,7 +67,7 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): elif personality==custom.LongitudinalPersonalitySP.aggressive: return 0.6 elif personality==custom.LongitudinalPersonalitySP.overtake: - return 0.25 + return 0.1 else: raise NotImplementedError("Longitudinal personality not supported") diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 5536b0c1c8..f17f8df8b1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -36,8 +36,6 @@ _A_TOTAL_MAX_BP = [20., 40.] EventName = car.CarEvent.EventName -LaneChangeState = log.LaneChangeState -LaneChangeDirection = log.LaneChangeDirection def get_max_accel(v_ego): @@ -104,8 +102,6 @@ class LongitudinalPlanner: self.dynamic_experimental_controller = DynamicExperimentalController() self.accel_controller = AccelController() - self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist") - def read_param(self): try: self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl")) @@ -113,8 +109,6 @@ class LongitudinalPlanner: self.dynamic_experimental_controller = DynamicExperimentalController() self.accel_controller = AccelController() - self.overtaking_accel = self.params.get_bool("OvertakingAccelerationAssist") - @staticmethod def parse_model(model_msg, model_error): if (len(model_msg.position.x) == ModelConstants.IDX_N and @@ -174,13 +168,6 @@ class LongitudinalPlanner: accel_limits = [ACCEL_MIN, ACCEL_MAX] accel_limits_turns = [ACCEL_MIN, ACCEL_MAX] - lat_plan = sm['lateralPlanDEPRECATED'] - dm_state = sm['driverMonitoringState'] - overtaking_accel_allowed = ((lat_plan.laneChangeDirection == LaneChangeDirection.right and dm_state.isRHD) or - (lat_plan.laneChangeDirection == LaneChangeDirection.left and not dm_state.isRHD)) and \ - (lat_plan.laneChangeState in (LaneChangeState.preLaneChange, LaneChangeState.laneChangeStarting)) - overtaking_accel_engaged = self.overtaking_accel and overtaking_accel_allowed and v_ego > 40 * CV.MPH_TO_MS - if reset_state: self.v_desired_filter.x = v_ego # Clip aEgo to cruise limits to prevent large accelerations when becoming active @@ -203,6 +190,7 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) + overtaking_accel_engaged = sm['controlsStateSP'].overtakingAccelerationAssist self.mpc.set_weights(prev_accel_constraint, personality=custom.LongitudinalPersonalitySP.overtake if overtaking_accel_engaged else sm['controlsStateSP'].personality) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.cc index 2bbbbc8872..17be24764f 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/sunnypilot_settings.cc @@ -372,6 +372,7 @@ SunnypilotPanel::SunnypilotPanel(QWidget *parent) : QFrame(parent) { toggles["EnableMads"]->setConfirmation(true, false); toggles["EndToEndLongAlertLight"]->setConfirmation(true, false); toggles["CustomOffsets"]->showDescription(); + toggles["OvertakingAccelerationAssist"]->setConfirmation(true, false); connect(toggles["EnableMads"], &ToggleControlSP::toggleFlipped, mads_settings, &MadsSettings::updateToggles); connect(toggles["EnableMads"], &ToggleControlSP::toggleFlipped, [=](bool state) {