From a8d1f2318e488c9e95716ea60d5fef5d2afc4354 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 15 Sep 2026 15:00:02 -0500 Subject: [PATCH] whoopity scoop --- .../opendbc/car/toyota/carcontroller.py | 20 +++--- .../opendbc/car/toyota/tests/test_toyota.py | 15 +++-- opendbc_repo/opendbc/safety/modes/toyota.h | 4 +- .../opendbc/safety/tests/test_toyota.py | 2 +- selfdrive/controls/lib/desire_helper.py | 11 +++- .../controls/tests/test_navigation_desires.py | 63 ++++++++++++++++++- .../controls/tests/test_starpilot_vcruise.py | 19 ++++++ starpilot/controls/lib/starpilot_vcruise.py | 2 +- 8 files changed, 107 insertions(+), 29 deletions(-) diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index da4eba6054..1e6dcfab58 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -46,7 +46,7 @@ TOYOTA_AUTO_HOLD_ACTIVATION_FRAMES = 100 # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long MAX_STEER_RATE = 100 # deg/s -MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut +MAX_STEER_RATE_FRAMES = 17 # tx control frames needed before torque can be cut # EPS allows user torque above threshold for 50 frames before permanently faulting MAX_USER_TORQUE = 500 @@ -77,13 +77,12 @@ def should_bypass_toyota_long_pid(CP, starpilot_toggles=None) -> bool: ) or highlander_sdsu) -def apply_toyota_corolla_steer_rate_guard(car_fingerprint, steering_rate_deg: float, lat_active: bool, - apply_torque: int, apply_steer_req: bool) -> tuple[int, bool]: - if (car_fingerprint == CAR.TOYOTA_COROLLA_TSS2 and lat_active and - abs(steering_rate_deg) >= MAX_STEER_RATE): - return 0, True +def get_toyota_lat_active(car_fingerprint, requested_active: bool, steering_torque: float, + steering_pressed: bool) -> bool: + if not requested_active or abs(steering_torque) >= MAX_USER_TORQUE: + return False - return apply_torque, apply_steer_req + return not (car_fingerprint == CAR.TOYOTA_COROLLA_TSS2 and steering_pressed) def supports_toyota_auto_hold(CP, auto_hold_enabled: bool) -> bool: @@ -344,7 +343,8 @@ class CarController(CarControllerBase): stopping = actuators.longControlState == LongCtrlState.stopping hud_control = CC.hudControl pcm_cancel_cmd = CC.cruiseControl.cancel - lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE + lat_active = get_toyota_lat_active(self.CP.carFingerprint, CC.latActive, + CS.out.steeringTorque, CS.out.steeringPressed) if len(CC.orientationNED) == 3: self.pitch.update(CC.orientationNED[1]) @@ -375,10 +375,6 @@ class CarController(CarControllerBase): self.steer_rate_counter, MAX_STEER_RATE_FRAMES, ) - apply_torque, apply_steer_req = apply_toyota_corolla_steer_rate_guard( - self.CP.carFingerprint, CS.out.steeringRateDeg, lat_active, apply_torque, apply_steer_req, - ) - if not lat_active: apply_torque = 0 diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 064f7930d0..ac4f25b3ff 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -11,7 +11,7 @@ from opendbc.car.toyota import toyotacan from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_feedforward, get_long_tune, get_prius_feedforward, \ get_prius_positive_feedforward_scale, \ get_rav4_interceptor_pedal_scale, \ - apply_toyota_corolla_steer_rate_guard, \ + get_toyota_lat_active, \ limit_interceptor_pcm_accel, \ limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \ limit_prius_stopping_accel, should_bypass_toyota_long_pid, supports_toyota_auto_hold, \ @@ -735,15 +735,14 @@ class TestToyotaFingerprint: class TestToyotaCarController: - def test_corolla_tss2_cuts_torque_but_keeps_lka_request_at_high_steer_rate(self): - assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_COROLLA_TSS2, 100.0, True, 250, True) == (0, True) - assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_COROLLA_TSS2, 120.0, True, 0, False) == (0, True) + def test_corolla_tss2_hands_off_immediately_when_driver_is_steering(self): + assert not get_toyota_lat_active(CAR.TOYOTA_COROLLA_TSS2, True, 117, True) - def test_corolla_tss2_steer_rate_guard_leaves_normal_rate_unchanged(self): - assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_COROLLA_TSS2, 99.9, True, 250, True) == (250, True) + def test_corolla_tss2_stays_active_without_driver_input(self): + assert get_toyota_lat_active(CAR.TOYOTA_COROLLA_TSS2, True, 99, False) - def test_corolla_tss2_steer_rate_guard_is_corolla_only(self): - assert apply_toyota_corolla_steer_rate_guard(CAR.TOYOTA_RAV4_TSS2, 120.0, True, 250, True) == (250, True) + def test_toyota_driver_handoff_behavior_is_corolla_only(self): + assert get_toyota_lat_active(CAR.TOYOTA_RAV4_TSS2, True, 117, True) @staticmethod def _make_controller(*, standstill_req=False, last_standstill=False): diff --git a/opendbc_repo/opendbc/safety/modes/toyota.h b/opendbc_repo/opendbc/safety/modes/toyota.h index ab0705b88b..cd6d293e97 100644 --- a/opendbc_repo/opendbc/safety/modes/toyota.h +++ b/opendbc_repo/opendbc/safety/modes/toyota.h @@ -235,9 +235,9 @@ static bool toyota_tx_hook(const CANPacket_t *msg) { // the EPS faults when the steering angle rate is above a certain threshold for too long. to prevent this, // we allow setting STEER_REQUEST bit to 0 while maintaining the requested torque value for a single frame - .min_valid_request_frames = 18, + .min_valid_request_frames = 17, .max_invalid_request_frames = 1, - .min_valid_request_rt_interval = 171000, // 171ms; a ~10% buffer on cutting every 19 frames + .min_valid_request_rt_interval = 162000, // 162ms; a ~10% buffer on cutting every 18 frames .has_steer_req_tolerance = true, }; diff --git a/opendbc_repo/opendbc/safety/tests/test_toyota.py b/opendbc_repo/opendbc/safety/tests/test_toyota.py index 55f75a66ed..e25a7569cd 100644 --- a/opendbc_repo/opendbc/safety/tests/test_toyota.py +++ b/opendbc_repo/opendbc/safety/tests/test_toyota.py @@ -254,7 +254,7 @@ class TestToyotaSafetyTorque(TestToyotaSafetyBase, common.MotorTorqueSteeringSaf TORQUE_MEAS_TOLERANCE = 1 # toyota safety adds one to be conservative for rounding # Safety around steering req bit - MIN_VALID_STEERING_FRAMES = 18 + MIN_VALID_STEERING_FRAMES = 17 MAX_INVALID_STEERING_FRAMES = 1 def setUp(self): diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 5f0d24bd1c..84ef56fab4 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -13,7 +13,7 @@ LaneChangeDirection = log.LaneChangeDirection LANE_CHANGE_SPEED_MIN = 20 * CV.MPH_TO_MS LANE_CHANGE_TIME_MAX = 10. NAV_TURN_DISTANCE_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0] -NAV_TURN_DISTANCE_BREAKPOINTS = [20.0, 25.0, 30.0] +NAV_TURN_DISTANCE_BREAKPOINTS = [20.0, 35.0, 55.0] # A driver normally signals an intersection before slowing below the lane-change # speed threshold. Use the route to classify that early signal so it does not # start a lane change while approaching the matching turn. @@ -122,7 +122,7 @@ class DesireHelper: except (TypeError, ValueError): return False - return distance <= float(np.interp(carstate.vEgo, NAV_TURN_DISTANCE_SPEED_BREAKPOINTS, NAV_TURN_DISTANCE_BREAKPOINTS)) + return 0.0 <= distance <= float(np.interp(carstate.vEgo, NAV_TURN_DISTANCE_SPEED_BREAKPOINTS, NAV_TURN_DISTANCE_BREAKPOINTS)) @staticmethod def _nav_turn_signal_matches(carstate, nav_instruction_state): @@ -242,6 +242,9 @@ class DesireHelper: if modifier == "": return log.Desire.none + if modifier in ("left", "sharpLeft", "right", "sharpRight") and str(self._nav_instruction_state.get("maneuverType", "")).strip().lower() != "turn": + return log.Desire.none + if modifier == "slightLeft": if not self.nav_lane_positioning_allowed: return log.Desire.none @@ -259,11 +262,15 @@ class DesireHelper: if desired_lane_width >= starpilot_toggles.lane_detection_width and self._nav_torque_applied(carstate, lane_change_direction): return log.Desire.keepRight elif modifier in ("left", "sharpLeft"): + if self.turn_stop_hold: + return log.Desire.none turn_allowed = carstate.leftBlinker and not carstate.rightBlinker and not carstate.leftBlindspot turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance): return log.Desire.turnLeft elif modifier in ("right", "sharpRight"): + if self.turn_stop_hold: + return log.Desire.none turn_allowed = carstate.rightBlinker and not carstate.leftBlinker and not carstate.rightBlindspot turn_allowed &= carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill if turn_allowed and self._nav_turn_is_imminent(carstate, maneuver_distance): diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py index a57d5b4b7a..c3c6121be1 100644 --- a/selfdrive/controls/tests/test_navigation_desires.py +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -68,7 +68,7 @@ def test_nav_desires_turn_right_below_lane_change_speed(): helper = DesireHelper() helper.nav_desires_allowed = True helper._update_nav_params = lambda: None - helper._nav_instruction_state = {"valid": True, "maneuverModifier": "right", "maneuverDistance": 10.0} + helper._nav_instruction_state = {"valid": True, "maneuverType": "turn", "maneuverModifier": "right", "maneuverDistance": 10.0} helper.update( make_car_state(vEgo=5.0, rightBlinker=True), @@ -81,12 +81,69 @@ def test_nav_desires_turn_right_below_lane_change_speed(): assert helper.desire == log.Desire.turnRight +def test_nav_desires_turn_preview_starts_before_last_second(): + helper = DesireHelper() + helper._update_nav_params = lambda: None + helper._nav_instruction_state = {"valid": True, "maneuverType": "turn", "maneuverModifier": "right", "maneuverDistance": 50.0} + + helper.update( + make_car_state(vEgo=10.5, rightBlinker=True), + True, + 0.0, + make_plan(), + make_toggles(minimum_lane_change_speed=11.1), + ) + + assert helper.desire == log.Desire.turnRight + assert helper.lane_change_state == LaneChangeState.off + + +def test_nav_desires_turn_preview_is_bounded_and_requires_matching_signal(): + for distance, blinker, speed, maneuver_type in ( + (65.0, True, 10.5, "turn"), + (-1.0, True, 10.5, "turn"), + (50.0, False, 10.5, "turn"), + (50.0, True, 11.2, "turn"), + (50.0, True, 10.5, "arrive"), + ): + helper = DesireHelper() + helper._update_nav_params = lambda: None + helper._nav_instruction_state = {"valid": True, "maneuverType": maneuver_type, "maneuverModifier": "right", "maneuverDistance": distance} + + helper.update( + make_car_state(vEgo=speed, rightBlinker=blinker), + True, + 0.0, + make_plan(), + make_toggles(minimum_lane_change_speed=11.1), + ) + + assert helper.desire == log.Desire.none + + +def test_nav_desires_turn_preview_respects_stop_hold(): + helper = DesireHelper() + helper._update_nav_params = lambda: None + helper._nav_instruction_state = {"valid": True, "maneuverType": "turn", "maneuverModifier": "right", "maneuverDistance": 20.0} + + helper.update( + make_car_state(vEgo=5.0, rightBlinker=True), + True, + 0.0, + make_plan(redLight=True), + make_toggles(minimum_lane_change_speed=11.1), + ) + + assert helper.turn_stop_hold + assert helper.desire == log.Desire.none + + def test_nav_desires_turn_requires_matching_blinker(): for modifier, opposite_blinker in (("left", "rightBlinker"), ("right", "leftBlinker")): helper = DesireHelper() helper.nav_desires_allowed = True helper._update_nav_params = lambda: None - helper._nav_instruction_state = {"valid": True, "maneuverModifier": modifier, "maneuverDistance": 10.0} + helper._nav_instruction_state = {"valid": True, "maneuverType": "turn", "maneuverModifier": modifier, "maneuverDistance": 10.0} helper.update( make_car_state(vEgo=5.0, **{opposite_blinker: True}), @@ -103,7 +160,7 @@ def test_nav_desires_turn_right_waits_until_turn_is_close(): helper = DesireHelper() helper.nav_desires_allowed = True helper._update_nav_params = lambda: None - helper._nav_instruction_state = {"valid": True, "maneuverModifier": "right", "maneuverDistance": 300.0} + helper._nav_instruction_state = {"valid": True, "maneuverType": "turn", "maneuverModifier": "right", "maneuverDistance": 300.0} helper.update( make_car_state(vEgo=5.0), diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 3c836224f8..3e69d620a2 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -1311,6 +1311,25 @@ def test_nav_turn_speed_control_begins_before_reported_intersection_approach(): assert result == pytest.approx(vcruise.nav_turn_target) +def test_nav_turn_speed_control_starts_earlier_at_reported_route_cruise(): + _, vcruise = make_vcruise(nav_state={ + "valid": True, + "maneuverType": "turn", + "maneuverModifier": "right", + "maneuverDistance": 100.0, + "nextManeuverType": "", + "nextManeuverModifier": "", + "nextManeuverDistance": 0.0, + }) + + toggles = make_toggles() + toggles.nav_longitudinal_allowed = True + result = update_vcruise(vcruise, make_sm(standstill=False), toggles, now=0.0, v_ego=11.94, v_cruise=11.94) + + assert result < 11.94 + assert result == pytest.approx(vcruise.nav_turn_target) + + def test_nav_turn_speed_control_ignores_distant_turn(): _, vcruise = make_vcruise(nav_state={ "valid": True, diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 5f3468eed2..7b3a39d52e 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -38,7 +38,7 @@ SLC_LEAD_DROP_RELAXATION_DECEL_V = [0.7, 0.9, 1.15, 1.35] # This is an approach envelope, not a request for harder braking. A gentler # deceleration value lowers the target farther from the turn and gives the MPC # more time to settle before the intersection. -NAV_TURN_COMFORT_DECEL = 0.85 +NAV_TURN_COMFORT_DECEL = 0.45 NAV_TURN_DISTANCE_BUFFER = 8.0 NAV_TURN_MIN_TARGET_DELTA = 0.25 NAV_TURN_TARGET_SPEEDS = {