From d2cd3fd0edf80ba229792c894c09300dfe5b1bb2 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 2 Oct 2026 21:03:49 -0500 Subject: [PATCH] Quintessence: A Quest --- .../opendbc/car/volvo/carcontroller.py | 6 +- .../car/volvo/tests/test_controller.py | 32 +++++++--- opendbc_repo/opendbc/car/volvo/volvocan.py | 5 +- opendbc_repo/opendbc/safety/modes/volvo.h | 44 ++++++++++++- .../opendbc/safety/tests/test_volvo.py | 62 +++++++++++++++++++ selfdrive/controls/lib/latcontrol_pid.py | 6 ++ .../controls/lib/latcontrol_vehicle_tunes.py | 9 +++ .../controls/tests/test_honda_crv_pid_gain.py | 61 ++++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 35 +++++++++++ starpilot/car/ford/lateral.py | 2 +- starpilot/car/ford/tests/test_lateral.py | 56 +++++++++++++++-- 11 files changed, 298 insertions(+), 20 deletions(-) create mode 100644 selfdrive/controls/tests/test_honda_crv_pid_gain.py diff --git a/opendbc_repo/opendbc/car/volvo/carcontroller.py b/opendbc_repo/opendbc/car/volvo/carcontroller.py index 16ce08ea04..2e4e92b4f4 100644 --- a/opendbc_repo/opendbc/car/volvo/carcontroller.py +++ b/opendbc_repo/opendbc/car/volvo/carcontroller.py @@ -248,12 +248,12 @@ class CarController(CarControllerBase): # LCA_5 (formerly SPEED_1) - 0x67 - 50 Hz # Contains wheel speeds + LCA signals (LCA_TURN_BITS, LCA_5_STEER) if self.frame % 2 == 0: # 50 Hz - # Initialize counter from CarState on first run - if self.lca_5_counter is None: + if not lat_active or self.lca_5_counter is None: self.lca_5_counter = CS.msg_lca_5['COUNTER'] # Increment counter by +4, wrap at 15 (0xF never used) - self.lca_5_counter = (self.lca_5_counter + 4) % 15 + if lat_active: + self.lca_5_counter = (self.lca_5_counter + 4) % 15 can_sends.append(create_lca_5_message(self.packer, lat_active, apply_angle, CS.msg_lca_5, self.lca_5_counter)) diff --git a/opendbc_repo/opendbc/car/volvo/tests/test_controller.py b/opendbc_repo/opendbc/car/volvo/tests/test_controller.py index 7ad1846816..cb9bda92d4 100644 --- a/opendbc_repo/opendbc/car/volvo/tests/test_controller.py +++ b/opendbc_repo/opendbc/car/volvo/tests/test_controller.py @@ -3,6 +3,7 @@ from types import SimpleNamespace import pytest +from opendbc.can.parser import CANParser from opendbc.car.volvo.carcontroller import CarController from opendbc.car.volvo.helpers import checksum_lca_5_message from opendbc.car.volvo.interface import CarInterface @@ -56,21 +57,36 @@ def test_controller_emits_valid_eight_byte_messages_and_lca5_checksum(): assert data[2] == checksum_lca_5_message(data[0], data[1], data[3], data[4], data[5]) -def test_controller_relays_stock_lca5_angle_when_inactive(): - cp = CarInterface.get_non_essential_params("VOLVO_XC40_RECHARGE") +@pytest.mark.parametrize("fingerprint", [CAR.POLESTAR_2, CAR.VOLVO_XC40_RECHARGE]) +def test_controller_relays_complete_stock_lca5_when_inactive(fingerprint): + cp = CarInterface.get_non_essential_params(fingerprint) controller = CarController(DBC[cp.carFingerprint], cp) + parser = CANParser("volvo_mid_1", [("LCA_5", 50)], 0) + stock_data = bytes.fromhex("88c04cef1190ba00") + parser.update([0, [(0x67, stock_data, 0)]]) cs = _state() - cs.msg_lca_5["LCA_5_STEER"] = 12.0 + cs.msg_lca_5 = parser.vl["LCA_5"] cc = SimpleNamespace(latActive=False, actuators=_Actuators()) + safety = libsafety_py.libsafety + config = cp.safetyConfigs[0] + assert safety.set_safety_hooks(config.safetyModel.raw, config.safetyParam) == 0 + safety.init_tests() + safety.set_controls_allowed(False) + safety.safety_rx_hook(libsafety_py.make_CANPacket(0x67, 0, stock_data)) _, can_sends = controller.update(cc, cs, 0, None) lca5 = next(msg for msg in can_sends if msg[0] == 0x67) + assert lca5[1] == stock_data + assert safety.safety_tx_hook(libsafety_py.make_CANPacket(lca5[0], lca5[2], lca5[1])) - # The inactive path must not manufacture a new angle command. - raw = ((lca5[1][6] & 0x7F) << 8) | lca5[1][7] - if raw & (1 << 14): - raw -= 1 << 15 - assert abs(raw * 0.05596 - 12.0) < 0.1 + cs.msg_lca_5["COUNTER"] = 7 + controller.update(cc, cs, 0, None) + controller.update(cc, cs, 0, None) + controller.update(cc, cs, 0, None) + cc.latActive = True + _, can_sends = controller.update(cc, cs, 0, None) + active_lca5 = next(msg for msg in can_sends if msg[0] == 0x67) + assert active_lca5[1][3] >> 4 == 11 @pytest.mark.parametrize("fingerprint", [CAR.POLESTAR_2, CAR.VOLVO_XC40_RECHARGE]) diff --git a/opendbc_repo/opendbc/car/volvo/volvocan.py b/opendbc_repo/opendbc/car/volvo/volvocan.py index 963dc9c632..c91e5475dd 100644 --- a/opendbc_repo/opendbc/car/volvo/volvocan.py +++ b/opendbc_repo/opendbc/car/volvo/volvocan.py @@ -250,6 +250,9 @@ def create_lca_5_message(packer, lat_active: bool, target_angle_deg: float, msg_ CAN message for LCA_5 on bus 2 """ + if not lat_active: + return packer.make_can_msg('LCA_5', 2, msg_lca_5) + # DBC defines LCA_5_STEER as 15-bit signed with scale 0.05596 deg/count # Packer handles the encoding automatically - just pass the angle in degrees @@ -261,7 +264,7 @@ def create_lca_5_message(packer, lat_active: bool, target_angle_deg: float, msg_ 'WHEEL_SPEED_2': msg_lca_5['WHEEL_SPEED_2'], 'NEW_SIGNAL_5': msg_lca_5['NEW_SIGNAL_5'], 'NEW_SIGNAL_2': msg_lca_5['NEW_SIGNAL_2'], - 'LCA_5_STEER': target_angle_deg if lat_active else msg_lca_5['LCA_5_STEER'], + 'LCA_5_STEER': target_angle_deg, 'COUNTER': counter, } diff --git a/opendbc_repo/opendbc/safety/modes/volvo.h b/opendbc_repo/opendbc/safety/modes/volvo.h index f54906200c..780b343556 100644 --- a/opendbc_repo/opendbc/safety/modes/volvo.h +++ b/opendbc_repo/opendbc/safety/modes/volvo.h @@ -71,6 +71,27 @@ static uint16_t volvo_ecm_1_addr; static uint16_t volvo_bus1_cruise_control_addr; static bool volvo_c1; +#define VOLVO_STOCK_LCA5_FRAMES 4U +#define VOLVO_STOCK_LCA5_MAX_AGE_US 100000U +static uint8_t volvo_stock_lca5_data[VOLVO_STOCK_LCA5_FRAMES][8]; +static uint32_t volvo_stock_lca5_ts[VOLVO_STOCK_LCA5_FRAMES]; +static bool volvo_stock_lca5_valid[VOLVO_STOCK_LCA5_FRAMES]; +static uint8_t volvo_stock_lca5_index; + +static bool volvo_lca5_stock_relay(const CANPacket_t *msg) { + const uint32_t now = microsecond_timer_get(); + bool matches_stock = false; + for (uint8_t i = 0U; i < VOLVO_STOCK_LCA5_FRAMES; i++) { + bool matches = volvo_stock_lca5_valid[i] && + (safety_get_ts_elapsed(now, volvo_stock_lca5_ts[i]) <= VOLVO_STOCK_LCA5_MAX_AGE_US); + for (uint8_t byte = 0U; byte < 8U; byte++) { + matches &= msg->data[byte] == volvo_stock_lca5_data[i][byte]; + } + matches_stock |= matches; + } + return matches_stock; +} + static int volvo_be_15(const CANPacket_t *msg, uint8_t byte) { return (int)(((uint16_t)(msg->data[byte] & 0x7FU) << 8U) | msg->data[byte + 1U]); } @@ -159,6 +180,15 @@ static void volvo_rx_hook(const CANPacket_t *msg) { // Main bus (bus 0) messages if (msg->bus == VOLVO_MAIN_BUS) { + if (msg->addr == VOLVO_LCA_5) { + for (uint8_t byte = 0U; byte < 8U; byte++) { + volvo_stock_lca5_data[volvo_stock_lca5_index][byte] = msg->data[byte]; + } + volvo_stock_lca5_ts[volvo_stock_lca5_index] = microsecond_timer_get(); + volvo_stock_lca5_valid[volvo_stock_lca5_index] = true; + volvo_stock_lca5_index = (volvo_stock_lca5_index + 1U) % VOLVO_STOCK_LCA5_FRAMES; + } + // Update brake pedal and cruise state from BCM2 if (msg->addr == VOLVO_LCA_2) { // DBC: SG_ BRAKE_PEDAL_PRESSED_A : 47|1@0+ (-1,1) - inverted in DBC, so we invert raw bit @@ -263,9 +293,13 @@ static bool volvo_tx_hook(const CANPacket_t *msg) { // LCA frame also contains an angle-shaped field, but the imported controller // deliberately leaves that field at the observed vehicle value. if (msg->addr == VOLVO_LCA_5) { - const int desired_angle = volvo_lca_5_angle(msg); - tx &= SAFETY_ABS(desired_angle) <= VOLVO_MAX_ANGLE_CAN; - tx &= !steer_angle_cmd_checks(desired_angle, controls_allowed, VOLVO_ANGLE_STEERING_LIMITS); + if (!controls_allowed && volvo_lca5_stock_relay(msg)) { + desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -VOLVO_MAX_ANGLE_CAN, VOLVO_MAX_ANGLE_CAN); + } else { + const int desired_angle = volvo_lca_5_angle(msg); + tx &= SAFETY_ABS(desired_angle) <= VOLVO_MAX_ANGLE_CAN; + tx &= !steer_angle_cmd_checks(desired_angle, controls_allowed, VOLVO_ANGLE_STEERING_LIMITS); + } } // Keep the two torque-authority arms and the companion LCA angle bounded even @@ -357,6 +391,10 @@ static bool volvo_tx_hook(const CANPacket_t *msg) { static safety_config volvo_init(uint16_t param) { bool spa = GET_FLAG(param, VOLVO_FLAG_SPA); volvo_c1 = GET_FLAG(param, VOLVO_FLAG_C1); + volvo_stock_lca5_index = 0U; + for (uint8_t i = 0U; i < VOLVO_STOCK_LCA5_FRAMES; i++) { + volvo_stock_lca5_valid[i] = false; + } if (volvo_c1) { static const CanMsg VOLVO_C1_TX_MSGS[] = { diff --git a/opendbc_repo/opendbc/safety/tests/test_volvo.py b/opendbc_repo/opendbc/safety/tests/test_volvo.py index cabbadbb2a..f4854eb457 100644 --- a/opendbc_repo/opendbc/safety/tests/test_volvo.py +++ b/opendbc_repo/opendbc/safety/tests/test_volvo.py @@ -177,6 +177,68 @@ class TestVolvoSafetyBase(common.CarSafetyTest): self.assertTrue(self._tx(self._angle_cmd_msg(10))) self.assertFalse(self._tx(self._angle_cmd_msg(20))) + STOCK_LCA5 = bytes.fromhex("88c04cef1190ba00") + + def _stock_lca5(self, data=None, bus=VOLVO_PARTY_BUS): + return libsafety_py.make_CANPacket(VOLVO_LCA_5, bus, self.STOCK_LCA5 if data is None else data) + + def test_inactive_lca5_stock_relay_requires_unchanged_received_frame(self): + self._reset_angle_measurement(10) + self.safety.set_timer(100000) + self.assertFalse(self._tx(self._stock_lca5())) + self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS)) + self.assertTrue(self._tx(self._stock_lca5())) + self.assertEqual(self.safety.get_desired_angle_last(), round(10 / 0.05596)) + + for byte in range(8): + altered = bytearray(self.STOCK_LCA5) + altered[byte] ^= 1 + self.assertFalse(self._tx(self._stock_lca5(bytes(altered))), f"altered {byte=}") + self.assertFalse(self._tx(self._stock_lca5(bus=VOLVO_MAIN_BUS))) + self.assertFalse(self._tx(self._stock_lca5(bus=VOLVO_PT_BUS))) + + def test_inactive_lca5_stock_relay_wrong_rx_bus_and_length(self): + self._rx(self._stock_lca5(bus=VOLVO_PT_BUS)) + self._rx(self._stock_lca5(self.STOCK_LCA5[:7], bus=VOLVO_MAIN_BUS)) + self.assertFalse(self._tx(self._stock_lca5())) + + def test_inactive_lca5_stock_relay_expires_across_timer_wrap(self): + start = 0xFFFF0000 + self.safety.set_timer(start) + self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS)) + self.safety.set_timer((start + 100000) & 0xFFFFFFFF) + self.assertTrue(self._tx(self._stock_lca5())) + self.safety.set_timer((start + 100001) & 0xFFFFFFFF) + self.assertFalse(self._tx(self._stock_lca5())) + + def test_inactive_lca5_stock_relay_history_is_bounded(self): + frames = [] + for i in range(5): + data = bytearray(self.STOCK_LCA5) + data[3] = (data[3] + i) & 0xFF + frames.append(bytes(data)) + self.safety.set_timer(i * 20000) + self._rx(self._stock_lca5(frames[-1], bus=VOLVO_MAIN_BUS)) + self.assertFalse(self._tx(self._stock_lca5(frames[0]))) + for data in frames[1:]: + self.assertTrue(self._tx(self._stock_lca5(data))) + + def test_inactive_lca5_stock_relay_cleared_on_safety_init(self): + self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS)) + self.assertTrue(self._tx(self._stock_lca5())) + self.safety.set_safety_hooks(SAFETY_VOLVO, self.SAFETY_PARAM) + self.assertFalse(self._tx(self._stock_lca5())) + + def test_stock_lca5_placeholder_does_not_bypass_active_angle_limits(self): + self._reset_angle_measurement(10) + self._reset_speed_measurement(50) + self._rx(self._stock_lca5(bus=VOLVO_MAIN_BUS)) + self.assertTrue(self._tx(self._stock_lca5())) + self.safety.set_controls_allowed(True) + self.assertFalse(self._tx(self._stock_lca5())) + self.assertTrue(self._tx(self._angle_cmd_msg(10.4))) + self.assertFalse(self._tx(self._angle_cmd_msg(11))) + def test_angle_tx_rate_matches_controller_cadence(self): """LCA_5 is 50 Hz, so each frame may contain two 100 Hz controller steps.""" self._reset_speed_measurement(50) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index c5141a3018..c9b651214c 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -8,6 +8,7 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( RAV4_TSS2_CARS, SUBARU_IMPREZA_CARS, + get_honda_crv_5g_pid_kp_scale, get_honda_crv_5g_pid_output, get_rav4_tss2_pid_output, get_subaru_impreza_pid_output_scale, @@ -105,6 +106,7 @@ class LatControlPID(LatControl): self.honda_lateral_pid_ki_scale = 1.0 self.is_civic_bosch_modified = CP.carFingerprint == HONDA.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED) self.is_honda_crv_5g = CP.carFingerprint == HONDA.HONDA_CRV_5G + self.is_honda_crv_5g_stock_eps = self.is_honda_crv_5g and not bool(CP.flags & HondaFlags.EPS_MODIFIED) self.is_subaru_impreza = CP.carFingerprint in SUBARU_IMPREZA_CARS self.is_rav4_tss2 = CP.carFingerprint in RAV4_TSS2_CARS self.prev_angle_steers_des_no_offset = 0.0 @@ -163,6 +165,10 @@ class LatControlPID(LatControl): freeze_integrator = steer_limited_by_safety or steering_pressed or CS.vEgo < 5 + if self.is_honda_crv_5g_stock_eps: + kp_scale = self.honda_lateral_pid_kp_scale * get_honda_crv_5g_pid_kp_scale(angle_steers_des_no_offset, CS.vEgo) + self.pid._k_p = [self.base_kp_bp, scale_lateral_pid_gain_values(self.base_kp_v, kp_scale)] + output_torque = self.pid.update(error, feedforward=ff, speed=CS.vEgo, diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 32476d3cb3..3427b5c1d4 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -1261,6 +1261,9 @@ HONDA_CRV_5G_PID_CENTER_ANGLE = 14.0 HONDA_CRV_5G_PID_CENTER_ANGLE_WIDTH = 3.0 HONDA_CRV_5G_PID_OUTPUT_SCALE_MIN = 0.62 HONDA_CRV_5G_PID_OUTPUT_ALPHA_MIN = 0.28 +HONDA_CRV_5G_PID_CENTER_KP_SCALE_MIN = 0.50 +HONDA_CRV_5G_PID_CENTER_KP_SPEED_BP = [11.0 * CV.MPH_TO_MS, 18.0 * CV.MPH_TO_MS] +HONDA_CRV_5G_PID_CENTER_KP_ANGLE_BP = [6.0, 18.0] RAV4_TSS2_CENTER_FRICTION_THRESHOLD_GAIN = 0.14 RAV4_TSS2_CENTER_FRICTION_LAT = 0.30 @@ -1905,6 +1908,12 @@ def get_rav4_tss2_pid_output(output_torque: float, prev_output_torque: float, return float(prev_output_torque + output_alpha * (limited_output - prev_output_torque)) +def get_honda_crv_5g_pid_kp_scale(desired_angle_deg: float, v_ego: float) -> float: + speed_weight = np.interp(max(v_ego, 0.0), HONDA_CRV_5G_PID_CENTER_KP_SPEED_BP, [1.0, 0.0]) + center_weight = np.interp(abs(desired_angle_deg), HONDA_CRV_5G_PID_CENTER_KP_ANGLE_BP, [1.0, 0.0]) + return float(1.0 - (1.0 - HONDA_CRV_5G_PID_CENTER_KP_SCALE_MIN) * speed_weight * center_weight) + + def get_honda_crv_5g_pid_output(output_torque: float, prev_output_torque: float, desired_angle_deg: float, v_ego: float) -> float: """Damp low-speed CR-V 5G center reversals without blunting real turns.""" diff --git a/selfdrive/controls/tests/test_honda_crv_pid_gain.py b/selfdrive/controls/tests/test_honda_crv_pid_gain.py new file mode 100644 index 0000000000..f9448f3622 --- /dev/null +++ b/selfdrive/controls/tests/test_honda_crv_pid_gain.py @@ -0,0 +1,61 @@ +import pytest + +from openpilot.common.constants import CV +from openpilot.common.pid import PIDController +from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import get_honda_crv_5g_pid_kp_scale + + +@pytest.mark.parametrize('mph', [0.0, 3.0, 6.0, 8.0, 11.0]) +@pytest.mark.parametrize('angle', [-6.0, -3.0, 0.0, 3.0, 6.0]) +def test_low_speed_center_gain(mph, angle): + assert get_honda_crv_5g_pid_kp_scale(angle, mph * CV.MPH_TO_MS) == 0.5 + + +@pytest.mark.parametrize('mph', [18.0, 25.0, 45.0, 70.0]) +@pytest.mark.parametrize('angle', [-30.0, -6.0, 0.0, 6.0, 30.0]) +def test_normal_speed_gain_unchanged(mph, angle): + assert get_honda_crv_5g_pid_kp_scale(angle, mph * CV.MPH_TO_MS) == 1.0 + + +@pytest.mark.parametrize('mph', [0.0, 8.0, 14.0]) +@pytest.mark.parametrize('angle', [-90.0, -30.0, -18.0, 18.0, 30.0, 90.0]) +def test_real_turn_gain_unchanged(mph, angle): + assert get_honda_crv_5g_pid_kp_scale(angle, mph * CV.MPH_TO_MS) == 1.0 + + +def test_gain_is_bounded_symmetric_and_monotonic(): + for mph in [0, 8, 11, 12, 14, 16, 18, 45]: + scales = [get_honda_crv_5g_pid_kp_scale(angle, mph * CV.MPH_TO_MS) for angle in [0, 6, 9, 12, 15, 18, 30]] + assert scales == sorted(scales) + assert all(0.5 <= value <= 1.0 for value in scales) + for angle in [0, 6, 9, 12, 15, 18, 30]: + assert get_honda_crv_5g_pid_kp_scale(angle, mph * CV.MPH_TO_MS) == get_honda_crv_5g_pid_kp_scale(-angle, mph * CV.MPH_TO_MS) + scales = [get_honda_crv_5g_pid_kp_scale(0.0, mph * CV.MPH_TO_MS) for mph in [0, 8, 11, 12, 14, 16, 18, 45]] + assert scales == sorted(scales) + + +@pytest.mark.parametrize('mph', [11.0, 18.0]) +def test_speed_boundary_continuity(mph): + left = get_honda_crv_5g_pid_kp_scale(0.0, (mph - 1e-7) * CV.MPH_TO_MS) + right = get_honda_crv_5g_pid_kp_scale(0.0, (mph + 1e-7) * CV.MPH_TO_MS) + assert abs(left - right) < 1e-7 + + +@pytest.mark.parametrize('angle', [-18.0, -6.0, 6.0, 18.0]) +def test_angle_boundary_continuity(angle): + left = get_honda_crv_5g_pid_kp_scale(angle - 1e-7, 8.0 * CV.MPH_TO_MS) + right = get_honda_crv_5g_pid_kp_scale(angle + 1e-7, 8.0 * CV.MPH_TO_MS) + assert abs(left - right) < 1e-7 + + +def test_gain_reduces_feedback_before_saturation_without_changing_feedforward_or_integral(): + base = PIDController(0.64, 0.192, pos_limit=1, neg_limit=-1) + tuned = PIDController(0.64 * get_honda_crv_5g_pid_kp_scale(3.0, 8.0 * CV.MPH_TO_MS), 0.192, pos_limit=1, neg_limit=-1) + base.i = tuned.i = -0.019 + base_output = base.update(2.0, feedforward=0.004, freeze_integrator=True) + tuned_output = tuned.update(2.0, feedforward=0.004, freeze_integrator=True) + assert base_output == 1.0 + assert tuned_output == pytest.approx(0.625) + assert tuned.p == pytest.approx(base.p * 0.5) + assert tuned.i == base.i + assert tuned.f == base.f diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index b10bca3a09..119be8e722 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -2092,6 +2092,41 @@ class TestLatControl: assert lac_log.active assert abs(tuned_output) < abs(base_output) + def test_honda_crv_5g_pid_center_gain_update_path(self): + controller, VM, CS, params, toggles = self._build_pid_controller(HONDA.HONDA_CRV_5G) + CS.vEgo = 8.0 * 0.44704 + CS.steeringAngleDeg = -2.0 + controller.pid.i = 0.03 + toggles.honda_lateral_pid_kp_scale = 1.2 + toggles.honda_lateral_pid_ki_scale = 0.8 + for _ in range(20): + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles) + assert lac_log.p == pytest.approx(controller.base_kp_v[0] * 1.2 * 0.5 * lac_log.angleError) + assert lac_log.i == pytest.approx(0.03) + assert controller.pid._k_i[1] == pytest.approx([value * 0.8 for value in controller.base_ki_v]) + + CS.vEgo = 25.0 * 0.44704 + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles) + assert lac_log.p == pytest.approx(controller.base_kp_v[0] * 1.2 * lac_log.angleError) + + @pytest.mark.parametrize('car_name,eps_modified', [(HONDA.HONDA_CRV_5G, True), (HONDA.HONDA_CIVIC_BOSCH, False), + (TOYOTA.TOYOTA_RAV4_TSS2, False)]) + def test_honda_crv_5g_pid_center_gain_does_not_change_other_paths(self, monkeypatch, car_name, eps_modified): + controller, VM, CS, params, toggles = self._build_pid_controller(car_name) + if eps_modified: + CP = interfaces[car_name].get_non_essential_params(car_name) + CP.flags = int(CP.flags | HondaFlags.EPS_MODIFIED) + controller = LatControlPID(CP.as_reader(), interfaces[car_name](CP, custom.StarPilotCarParams.new_message()), DT_CTRL) + CS.vEgo = 8.0 * 0.44704 + CS.steeringAngleDeg = -2.0 + + def unexpected_gain(*_args): + raise AssertionError('CR-V center gain must not run for this controller') + + monkeypatch.setattr(latcontrol_pid, 'get_honda_crv_5g_pid_kp_scale', unexpected_gain) + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0, False, 0.2, None, None, toggles) + assert lac_log.p == pytest.approx(controller.base_kp_v[0] * lac_log.angleError) + def test_rav4_tss2_torque_center_tune_fades_before_real_turns(self): low_speed_center = get_rav4_tss2_center_output_scale(0.05, 8.0) low_speed_turn = get_rav4_tss2_center_output_scale(1.0, 8.0) diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 5957bfbd4b..27247ef3ff 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -245,7 +245,7 @@ class FordLateralController: if (self.CP.carFingerprint != CAR.FORD_MUSTANG_MACH_E_MK1 or not self.CP.flags & FordFlags.CANFD or steering_pressed or lane_change): return base - speed_weight = float(np.interp(v_ego, [9.0, 10.0, 14.0, 16.0], [0.0, 1.0, 1.0, 0.0])) + speed_weight = float(np.interp(v_ego, [8.0, 9.0, 14.0, 16.0], [0.0, 1.0, 1.0, 0.0])) deficit_weight = 0.0 planned_curve = abs(desired) >= 0.003 or (abs(requested) >= 0.003 and requested * predicted > 0.0) if requested * desired > 0.0 and planned_curve: diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index 515984fc56..d107bd0878 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -97,7 +97,7 @@ def test_mach_e_unwind_lag_ramps_continuously(controller, monkeypatch): (12.0, -0.012, -0.012, 0.004, False, False, 0.006), (12.0, 0.012, 0.012, 0.010, False, False, 0.002), (12.0, 0.012, 0.012, 0.014, False, False, 0.002), - (9.5, 0.012, 0.012, 0.004, False, False, 0.004), + (9.5, 0.012, 0.012, 0.004, False, False, 0.006), (15.0, 0.012, 0.012, 0.004, False, False, 0.004), (16.0, 0.012, 0.012, 0.004, False, False, 0.002), (12.0, 0.012, 0.012, 0.004, True, False, 0.002), @@ -118,7 +118,7 @@ def test_understeer_error_preserves_other_fords(controller): @pytest.mark.parametrize("sign", (-1, 1)) -@pytest.mark.parametrize("speed,expected", ((9.0, 0.002), (9.5, 0.004), (10.0, 0.006), +@pytest.mark.parametrize("speed,expected", ((8.0, 0.002), (8.5, 0.004), (9.0, 0.006), (9.5, 0.006), (10.0, 0.006), (12.0, 0.006), (15.0, 0.004), (16.0, 0.002))) def test_mach_e_planned_curve_error_uses_preview_request_before_action_builds(controller, sign, speed, expected): controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 @@ -167,10 +167,58 @@ def test_mach_e_medium_speed_turn_in_lead_is_not_clipped_by_small_action_curvatu assert result.path_angle == 0.0 +@pytest.mark.parametrize("sign", (-1, 1)) +def test_mach_e_curve_request_does_not_collapse_when_speed_crosses_curvature_clamp(controller, monkeypatch, sign): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: sign * 0.006) + controller.curvature_last = sign * 0.006 + controller.desired_curvature_last = sign * 0.006 + commands = [] + for speed in (8.9, 9.0, 9.01, 9.2, 9.5, 10.0): + result = controller.update(SimpleNamespace(latActive=True), car_state(speed=speed), + SimpleNamespace(curvature=sign * 0.006)) + assert result.active + assert result.path_angle == 0.0 + commands.append(result.curvature) + assert commands == pytest.approx([sign * 0.006] * 6) + + +@pytest.mark.parametrize("sign", (-1, 1)) +def test_mach_e_reversal_request_does_not_collapse_at_curvature_clamp(controller, monkeypatch, sign): + controller.CP.carFingerprint = CAR.FORD_MUSTANG_MACH_E_MK1 + controller.CP.flags = FordFlags.CANFD + monkeypatch.setattr(controller, "_predicted_curvature", lambda *_: -sign * 0.004) + monkeypatch.setattr(controller, "_blend_and_scale", lambda *_: (-sign * 0.005, 1)) + controller.curvature_last = -sign * 0.005 + for speed in (8.9, 9.01, 9.5, 10.0): + result = controller.update(SimpleNamespace(latActive=True), car_state(speed=speed, curvature=sign * 0.001), + SimpleNamespace(curvature=sign * 0.002)) + assert result.curvature == pytest.approx(-sign * 0.005) + assert result.path_angle == 0.0 + + +@pytest.mark.parametrize("fingerprint,flags,driver,lane_change", ( + (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, True, False), + (CAR.FORD_MUSTANG_MACH_E_MK1, FordFlags.CANFD, False, True), + (CAR.FORD_MUSTANG_MACH_E_MK1, 0, False, False), + (CAR.FORD_EDGE_MK2, FordFlags.CANFD, False, False), + (CAR.FORD_EXPLORER_MK6, FordFlags.CANFD, False, False), + (CAR.FORD_F_150_MK14, FordFlags.CANFD, False, False), +)) +def test_curve_clamp_transition_preserves_driver_lane_change_and_other_fords(controller, fingerprint, flags, driver, lane_change): + controller.CP.carFingerprint = fingerprint + controller.CP.flags = flags + for speed in (9.01, 9.5): + assert controller._curvature_error_limit(0.006, 0.006, 0.0, speed, driver, lane_change, 0.006) == 0.002 + + @pytest.mark.parametrize("sign", (-1, 1)) @pytest.mark.parametrize("speed,preview,expected", ( - (9.0, -0.004, 0.002), - (9.5, -0.004, 0.004), + (8.0, -0.004, 0.002), + (8.5, -0.004, 0.004), + (9.0, -0.004, 0.006), + (9.5, -0.004, 0.006), (10.0, -0.004, 0.006), (12.0, -0.004, 0.006), (14.0, -0.004, 0.006),