whoopity scoop

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
 

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):

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,
   };
 

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):

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):

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),

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,

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 = {