diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 204f41853..0714ae012 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -47,22 +47,23 @@ UNWIND_D_DES_THRESHOLD = -1.0 UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3 MIN_LATERAL_CONTROL_SPEED = 0.3 CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30 +CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.10 CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED = 12.0 CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE = 0.10 CIVIC_BOSCH_MODIFIED_B_FF_ONSET = 0.18 CIVIC_BOSCH_MODIFIED_B_FF_ONSET_WIDTH = 0.07 CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF = 1.00 CIVIC_BOSCH_MODIFIED_B_FF_CUTOFF_WIDTH = 0.30 -CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_LEFT = 0.11 -CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_RIGHT = 0.08 -CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT = 0.08 -CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT = 0.10 -CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT = 0.22 -CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT = 0.24 -CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.04 -CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.05 -CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.15 -CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.18 +CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_LEFT = 0.10 +CIVIC_BOSCH_MODIFIED_B_FF_REDUCTION_RIGHT = 0.16 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_LEFT = 0.02 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_BOOST_RIGHT = 0.04 +CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT = 0.28 +CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT = 0.34 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.01 +CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.02 +CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.18 +CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.24 BOLT_2022_2023_CARS = ( GM_CAR.CHEVROLET_BOLT_ACC_2022_2023, @@ -1063,6 +1064,8 @@ 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(): + 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)) ki_scale = getattr(self.torque_params, "ki", getattr(self.torque_params, "kiDEPRECATED", 1.0)) @@ -1076,6 +1079,8 @@ 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(): + latAccelFactor *= CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT self.torque_params.latAccelFactor = latAccelFactor self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 2f08550fb..0d103132d 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -414,6 +414,20 @@ class TestLatControl: assert captured["threshold"] == pytest.approx(0.3) + def test_modified_civic_b_torque_path_scales_lat_accel_factor(self, monkeypatch): + CarInterface = interfaces[HONDA.HONDA_CIVIC_BOSCH] + CP = CarInterface.get_non_essential_params(HONDA.HONDA_CIVIC_BOSCH) + CP.flags |= int(HondaFlags.EPS_MODIFIED) + CP.lateralTuning.init("torque") + 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) + + assert controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.10) + def test_modified_civic_b_torque_ff_scale_curve(self): steady_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.0, 12.0) steady_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.0, 12.0)