From 65c768cff0561df0f92fd95a50aaa81d92934c7e Mon Sep 17 00:00:00 2001 From: James <91348155+FrogAi@users.noreply.github.com> Date: Mon, 1 Dec 2025 12:00:00 -0700 Subject: [PATCH] Advanced longitudinal tuning --- frogpilot/common/frogpilot_variables.py | 14 ++++++++++++++ .../opendbc/car/volkswagen/carcontroller.py | 2 +- selfdrive/controls/lib/longcontrol.py | 8 ++++---- selfdrive/controls/lib/longitudinal_planner.py | 4 ++-- selfdrive/modeld/modeld.py | 2 +- 5 files changed, 22 insertions(+), 8 deletions(-) diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index cd91db87..b969c91d 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -207,11 +207,17 @@ class FrogPilotVariables: toggle.has_zss = toggle.car_make == "toyota" and bool(FPCP.flags & ToyotaFrogPilotFlags.ZSS.value) is_angle_car = CP.steerControlType == car.CarParams.SteerControlType.angle latAccelFactor = CP.lateralTuning.torque.latAccelFactor + longitudinalActuatorDelay = CP.longitudinalActuatorDelay toggle.openpilot_longitudinal = CP.openpilotLongitudinalControl and not toggle.disable_openpilot_long pcm_cruise = CP.pcmCruise + startAccel = CP.startAccel + stopAccel = CP.stopAccel steerActuatorDelay = CP.steerActuatorDelay steerKp = KP steerRatio = CP.steerRatio + toggle.stoppingDecelRate = CP.stoppingDecelRate + toggle.vEgoStarting = CP.vEgoStarting + toggle.vEgoStopping = CP.vEgoStopping msg_bytes = self.params.get("LiveTorqueParameters") if msg_bytes: @@ -242,6 +248,14 @@ class FrogPilotVariables: toggle.steerRatio = self.get_value("SteerRatio", cast=float, condition=advanced_lateral_tuning, default=steerRatio, min=steerRatio * 0.5, max=steerRatio * 1.5) toggle.use_custom_steerRatio = bool(round(toggle.steerRatio, 2) != round(steerRatio, 2)) + advanced_longitudinal_tuning = toggle.openpilot_longitudinal and self.get_value("AdvancedLongitudinalTune") + toggle.longitudinalActuatorDelay = self.get_value("LongitudinalActuatorDelay", cast=float, condition=advanced_longitudinal_tuning, default=longitudinalActuatorDelay, min=0, max=1) + toggle.startAccel = self.get_value("StartAccel", cast=float, condition=advanced_longitudinal_tuning, default=startAccel, min=0, max=MAX_ACCELERATION) + toggle.stopAccel = self.get_value("StopAccel", cast=float, condition=advanced_longitudinal_tuning, default=stopAccel, min=-MAX_ACCELERATION, max=0) + toggle.stoppingDecelRate = self.get_value("StoppingDecelRate", cast=float, condition=advanced_longitudinal_tuning, default=toggle.stoppingDecelRate, min=0.001, max=1) + toggle.vEgoStarting = self.get_value("VEgoStarting", cast=float, condition=advanced_longitudinal_tuning, default=toggle.vEgoStarting, min=0.01, max=1) + toggle.vEgoStopping = self.get_value("VEgoStopping", cast=float, condition=advanced_longitudinal_tuning, default=toggle.vEgoStopping, min=0.01, max=1) + toggle.alert_volume_controller = self.get_value("AlertVolumeControl") toggle.disengage_volume = self.get_value("DisengageVolume", cast=float, condition=toggle.alert_volume_controller) toggle.engage_volume = self.get_value("EngageVolume", cast=float, condition=toggle.alert_volume_controller) diff --git a/opendbc_repo/opendbc/car/volkswagen/carcontroller.py b/opendbc_repo/opendbc/car/volkswagen/carcontroller.py index 1d834f38..62d7d79d 100644 --- a/opendbc_repo/opendbc/car/volkswagen/carcontroller.py +++ b/opendbc_repo/opendbc/car/volkswagen/carcontroller.py @@ -88,7 +88,7 @@ class CarController(CarControllerBase): acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) accel = float(np.clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0) stopping = actuators.longControlState == LongCtrlState.stopping - starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < self.CP.vEgoStopping) + starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < frogpilot_toggles.vEgoStopping) can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, self.CAN.pt, CS.acc_type, CC.longActive, accel, acc_control, stopping, starting, CS.esp_hold_confirmation)) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 389cca30..8de7c268 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -16,7 +16,7 @@ def long_control_state_trans(CP, active, long_control_state, v_ego, starting_condition = (not should_stop and not cruise_standstill and not brake_pressed) - started_condition = v_ego > CP.vEgoStarting + started_condition = v_ego > frogpilot_toggles.vEgoStarting if not active: long_control_state = LongCtrlState.off @@ -70,13 +70,13 @@ class LongControl: elif self.long_control_state == LongCtrlState.stopping: output_accel = self.last_output_accel - if output_accel > self.CP.stopAccel: + if output_accel > frogpilot_toggles.stopAccel: output_accel = min(output_accel, 0.0) - output_accel -= self.CP.stoppingDecelRate * DT_CTRL + output_accel -= frogpilot_toggles.stoppingDecelRate * DT_CTRL self.reset() elif self.long_control_state == LongCtrlState.starting: - output_accel = self.CP.startAccel + output_accel = frogpilot_toggles.startAccel self.reset() else: # LongCtrlState.pid diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 329b4e1b..4620c9a8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -165,9 +165,9 @@ class LongitudinalPlanner: self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory)) self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0 - action_t = self.CP.longitudinalActuatorDelay + DT_MDL + action_t = frogpilot_toggles.longitudinalActuatorDelay + DT_MDL output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, - action_t=action_t, vEgoStopping=self.CP.vEgoStopping) + action_t=action_t, vEgoStopping=frogpilot_toggles.vEgoStopping) output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 202939c6..79e20108 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -294,7 +294,7 @@ def main(demo=False): # TODO this needs more thought, use .2s extra for now to estimate other delays # TODO Move smooth seconds to action function - long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS + long_delay = frogpilot_toggles.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS prev_action = log.ModelDataV2.Action() DH = DesireHelper()