From 7e4818916bf3304237fabecc19b18199ce3dc113 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 4 May 2026 17:48:07 -0500 Subject: [PATCH] Freshly Caught --- .../opendbc/car/honda/tests/test_honda.py | 13 ------ opendbc_repo/opendbc/car/interfaces.py | 7 ++-- selfdrive/controls/lib/latcontrol_torque.py | 42 +++++++++---------- selfdrive/controls/lib/longcontrol.py | 30 +++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 2 - selfdrive/controls/tests/test_longcontrol.py | 27 ++++++++++++ 6 files changed, 81 insertions(+), 40 deletions(-) diff --git a/opendbc_repo/opendbc/car/honda/tests/test_honda.py b/opendbc_repo/opendbc/car/honda/tests/test_honda.py index b8e005314..e1ef77901 100644 --- a/opendbc_repo/opendbc/car/honda/tests/test_honda.py +++ b/opendbc_repo/opendbc/car/honda/tests/test_honda.py @@ -148,19 +148,6 @@ class TestHondaFingerprint: CP = CarInterface.get_params(CAR.HONDA_CIVIC_BOSCH, gen_empty_fingerprint(), car_fw, False, False, False, toggles) assert not CP.dashcamOnly - assert CP.flags & HondaFlags.EPS_MODIFIED - assert list(CP.lateralParams.torqueBP) == [0, 4096] - assert list(CP.lateralParams.torqueV) == [0, 4096] - assert list(CP.lateralTuning.pid.kpV) == pytest.approx([0.8]) - assert list(CP.lateralTuning.pid.kiV) == pytest.approx([0.24]) - - def test_modified_civic_b_testing_ground_forces_torque(self, monkeypatch): - toggles = SimpleNamespace(force_torque_controller=False, nnff=False, nnff_lite=False) - car_fw = [CarParams.CarFw(ecu=CarParams.Ecu.eps, fwVersion=b'39990-TGG,A020\x00\x00', address=0x18DA30F1, subAddress=0)] - monkeypatch.setattr("openpilot.starpilot.common.testing_grounds.is_testing_ground_active", lambda slot_id, variant="B", refresh_interval_s=0.5: slot_id == "8" and variant == "B") - - CP = CarInterface.get_params(CAR.HONDA_CIVIC_BOSCH, gen_empty_fingerprint(), car_fw, False, False, False, toggles) - assert CP.flags & HondaFlags.EPS_MODIFIED assert CP.lateralTuning.which() == "torque" diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index c6110fb02..273e4d5d7 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -183,14 +183,13 @@ class CarInterfaceBase(ABC): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, ret.tireStiffnessFactor) toggles_to_check = ("force_torque_controller", "nnff", "nnff_lite") - modified_civic_b_force_torque = ( + modified_civic_force_torque = ( candidate == HONDA.HONDA_CIVIC_BOSCH and - bool(ret.flags & HondaFlags.EPS_MODIFIED) and - testing_ground.use("8", "B") + bool(ret.flags & HondaFlags.EPS_MODIFIED) ) if ret.steerControlType != structs.CarParams.SteerControlType.angle and ( any(getattr(starpilot_toggles, toggle, False) for toggle in toggles_to_check) or - modified_civic_b_force_torque + modified_civic_force_torque ): CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index a0cc4a2ae..694e14bb7 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -230,21 +230,21 @@ IONIQ_6_FF_CUTOFF = 0.48 IONIQ_6_FF_CUTOFF_WIDTH = 0.12 IONIQ_6_TRANSITION_SPEED = 10.0 IONIQ_6_PHASE_SCALE = 0.10 -IONIQ_6_TURN_IN_BOOST_LEFT = 1.20 -IONIQ_6_TURN_IN_BOOST_RIGHT = 1.28 -IONIQ_6_UNWIND_TAPER_LEFT = 2.02 -IONIQ_6_UNWIND_TAPER_RIGHT = 4.65 +IONIQ_6_TURN_IN_BOOST_LEFT = 1.28 +IONIQ_6_TURN_IN_BOOST_RIGHT = 1.36 +IONIQ_6_UNWIND_TAPER_LEFT = 2.18 +IONIQ_6_UNWIND_TAPER_RIGHT = 5.05 IONIQ_6_FRICTION_MULT = 0.965 IONIQ_6_FRICTION_LAT_RISE = 0.20 IONIQ_6_FRICTION_JERK_RISE = 0.24 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.42 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.58 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 2.25 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 5.35 -IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.22 -IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.36 -IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.95 -IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 4.90 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.46 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.64 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 2.40 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 5.80 +IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.24 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.40 +IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 2.15 +IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 5.30 IONIQ_6_CENTER_TAPER_MAX = 0.056 IONIQ_6_CENTER_TAPER_LAT = 0.22 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02 @@ -267,12 +267,12 @@ IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48 IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.95 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.10 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.12 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.22 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.42 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.70 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.12 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.15 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.26 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.50 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.82 IONIQ_6_OUTPUT_TAPER_SPEED = 8.5 IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5 IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90 @@ -1074,7 +1074,7 @@ class LatControlTorque(LatControl): self.torque_ki_mult = 1.0 if self.is_ioniq_6: self.torque_params.latAccelFactor *= IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT - if self.is_civic_bosch_modified and civic_bosch_modified_lateral_testing_ground_active(): + if self.is_civic_bosch_modified: self.torque_params.latAccelFactor *= CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT if self.is_bolt: kp_scale = getattr(self.torque_params, "kp", getattr(self.torque_params, "kpDEPRECATED", 1.0)) @@ -1089,7 +1089,7 @@ class LatControlTorque(LatControl): def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): if self.is_ioniq_6: latAccelFactor *= IONIQ_6_BASE_LAT_ACCEL_FACTOR_MULT - if self.is_civic_bosch_modified and civic_bosch_modified_lateral_testing_ground_active(): + if self.is_civic_bosch_modified: latAccelFactor *= CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT self.torque_params.latAccelFactor = latAccelFactor self.torque_params.latAccelOffset = latAccelOffset @@ -1194,7 +1194,7 @@ class LatControlTorque(LatControl): ff *= get_volt_plexy_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_volt_plexy_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_volt_plexy_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) - elif self.is_civic_bosch_modified and civic_bosch_modified_lateral_testing_ground_active(): + elif self.is_civic_bosch_modified: ff *= get_civic_bosch_modified_b_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD friction_scale = get_civic_bosch_modified_b_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index b66ebc3af..9831a7feb 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -223,6 +223,34 @@ class LongControl: bleed = interp(v_ego, [0.0, 4.0, 12.0, 25.0], [0.82, 0.86, 0.90, 0.94]) self.pid.i *= bleed + def _trim_positive_overshoot_integrator(self, a_target, error, CS): + if self.pid.i <= 0.0: + return + if a_target >= -0.05 or error >= -0.25: + return + if CS.vEgo <= 8.0 or CS.aEgo <= 0.15: + return + + # If the planner has already crossed into decel but the car is still + # accelerating, bleed stale positive I aggressively so the command can + # cross back through zero instead of carrying throttle for several seconds. + bleed = interp(abs(error), [0.25, 0.75, 1.5], [0.55, 0.25, 0.0]) + self.pid.i *= bleed + + @staticmethod + def _cap_positive_output_on_negative_target(output_accel, a_target, error, CS): + if output_accel <= 0.0: + return output_accel + if a_target >= -0.10 or error >= -0.35: + return output_accel + if CS.vEgo <= 8.0 or CS.aEgo <= 0.15: + return output_accel + + # Once the planner is asking for real decel, don't keep feeding positive + # drive torque while we're still accelerating away from the target. + positive_cap = interp(a_target, [-0.6, -0.1], [0.0, 0.05]) + return min(output_accel, float(positive_cap)) + def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles): """Update longitudinal control. This updates the state machine and runs a PID loop""" self.pid.neg_limit = accel_limits[0] @@ -260,10 +288,12 @@ class LongControl: error = a_target - CS.aEgo self.update_mpc_mode(self.experimental_mode) self._shape_volt_test_tune_integrator(error, CS.vEgo) + self._trim_positive_overshoot_integrator(a_target, error, CS) feedforward = a_target * self.feedforward_gain freeze_integrator = self._get_pedal_long_freeze(a_target, error, CS.vEgo, accel_limits) raw_output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward, freeze_integrator=freeze_integrator) + raw_output_accel = self._cap_positive_output_on_negative_target(raw_output_accel, a_target, error, CS) if self.transitioning and self.prev_mode == 'acc' and self.current_mode == 'blended': diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index dd22d0082..258861cb9 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -411,7 +411,6 @@ class TestLatControl: captured["threshold"] = friction_threshold return 0.0 - monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_lateral_testing_ground_active", lambda: True) monkeypatch.setattr(latcontrol_torque, "get_friction", fake_get_friction) controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, SimpleNamespace()) @@ -425,7 +424,6 @@ class TestLatControl: CP.lateralTuning.torque.latAccelFactor = 3.0 CP.lateralTuning.torque.friction = 0.1 - monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_lateral_testing_ground_active", lambda: True) CI = CarInterface(CP, custom.StarPilotCarParams.new_message()) controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index 1013964e7..de6789be3 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -262,3 +262,30 @@ def test_non_interceptor_volt_testing_ground_handoff_freezes_integrator(monkeypa assert freeze assert lc.integrator_hold_frames > 0 + + +def test_negative_target_unwinds_positive_accel_command_after_sign_flip(): + CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5) + CP.longitudinalTuning.kpBP = [0.0] + CP.longitudinalTuning.kpV = [0.1] + CP.longitudinalTuning.kiBP = [0.0] + CP.longitudinalTuning.kiV = [0.03] + + lc = LongControl(CP) + lc.long_control_state = LongCtrlState.pid + lc.last_output_accel = 1.2 + lc.pid.i = 1.2 + CS = car.CarState.new_message(vEgo=30.0, aEgo=0.9, brakePressed=False, gasPressed=False) + CS.cruiseState.standstill = False + + output_accel = lc.update( + active=True, + CS=CS, + a_target=-0.5, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(), + ) + + assert lc.long_control_state == LongCtrlState.pid + assert output_accel <= 0.01