more stuff

opendbc_repo/opendbc/car/honda/carcontroller.py

@@ -23,6 +23,50 @@ def get_civic_bosch_modified_steer_can_max(base_steer_can_max: int, CP) -> int:
   return base_steer_can_max
 
 
+def get_civic_bosch_modified_torque_lpf_tau(torque_cmd: float, prev_torque_cmd: float, v_ego: float) -> float:
+  torque_delta = abs(float(torque_cmd) - float(prev_torque_cmd))
+  sign_change = (float(torque_cmd) * float(prev_torque_cmd)) < 0.0
+  highway = v_ego > (50.0 * 0.44704)
+
+  if highway:
+    if sign_change and torque_delta > 0.15:
+      return 0.09
+    return 0.11
+
+  if torque_delta > 0.50:
+    return 0.10
+  elif torque_delta > 0.20:
+    return 0.11
+  elif torque_delta > 0.05:
+    return 0.12
+  else:
+    return 0.15
+
+
+def get_civic_bosch_modified_steering_pressed(raw_pressed: bool, steering_torque: float, torque_cmd: float,
+                                              filter_s: float, was_pressed: bool) -> tuple[float, bool]:
+  torque_product = steering_torque * torque_cmd
+  torque_cmd_abs = abs(torque_cmd)
+
+  if raw_pressed:
+    if was_pressed:
+      trigger_s = 0.05
+    elif torque_cmd_abs < 0.10:
+      trigger_s = 0.12
+    elif torque_product < 0.0:
+      trigger_s = 0.12
+    else:
+      trigger_s = 0.32
+
+    filter_s = min(1.0, filter_s + DT_CTRL)
+    steering_pressed = filter_s >= trigger_s
+  else:
+    filter_s = max(0.0, filter_s - 4.0 * DT_CTRL)
+    steering_pressed = filter_s > 0.08 and was_pressed
+
+  return filter_s, steering_pressed
+
+
 def compute_gb_honda_bosch(accel, speed):
   # TODO returns 0s, is unused
   return 0.0, 0.0
@@ -120,6 +164,24 @@ class CarController(CarControllerBase):
     self.gas = 0.0
     self.brake = 0.0
     self.last_torque = 0.0
+    self.torque_lpf = 0.0
+    self.prev_torque_cmd = 0.0
+    self.steering_pressed_filter_s = 0.0
+    self.steering_pressed_robust_prev = False
+
+  def _modified_civic_active(self) -> bool:
+    return self.CP.carFingerprint == CAR.HONDA_CIVIC_BOSCH and self.CP.dashcamOnly and civic_bosch_modified_lateral_testing_ground_active()
+
+  def _filtered_steering_pressed(self, CS, torque_cmd: float) -> bool:
+    self.steering_pressed_filter_s, steering_pressed = get_civic_bosch_modified_steering_pressed(
+      bool(CS.out.steeringPressed),
+      float(getattr(CS.out, "steeringTorque", 0.0)),
+      float(torque_cmd),
+      self.steering_pressed_filter_s,
+      self.steering_pressed_robust_prev,
+    )
+    self.steering_pressed_robust_prev = steering_pressed
+    return steering_pressed
 
   def update(self, CC, CS, now_nanos, starpilot_toggles):
     actuators = CC.actuators
@@ -134,8 +196,29 @@ class CarController(CarControllerBase):
       accel = 0.0
       gas, brake = 0.0, 0.0
 
+    torque_cmd = float(actuators.torque)
+    filtered_steering_pressed = bool(CS.out.steeringPressed)
+    if self._modified_civic_active():
+      if CC.latActive:
+        filtered_steering_pressed = self._filtered_steering_pressed(CS, torque_cmd)
+        if filtered_steering_pressed:
+          self.torque_lpf = 0.0
+          self.prev_torque_cmd = 0.0
+          torque_cmd = 0.0
+        else:
+          tau = get_civic_bosch_modified_torque_lpf_tau(torque_cmd, self.prev_torque_cmd, CS.out.vEgo)
+          alpha = DT_CTRL / (tau + DT_CTRL)
+          self.torque_lpf = alpha * torque_cmd + ((1.0 - alpha) * self.torque_lpf)
+          self.prev_torque_cmd = torque_cmd
+          torque_cmd = self.torque_lpf
+      else:
+        self.torque_lpf = 0.0
+        self.prev_torque_cmd = 0.0
+        self.steering_pressed_filter_s = 0.0
+        self.steering_pressed_robust_prev = False
+
     # *** rate limit steer ***
-    limited_torque = rate_limit(actuators.torque, self.last_torque, -self.params.STEER_DELTA_DOWN * DT_CTRL,
+    limited_torque = rate_limit(torque_cmd, self.last_torque, -self.params.STEER_DELTA_DOWN * DT_CTRL,
                                 self.params.STEER_DELTA_UP * DT_CTRL)
     self.last_torque = limited_torque
 
@@ -241,7 +324,7 @@ class CarController(CarControllerBase):
                                                  hud_control, hud_v_cruise, CS.is_metric, CS.acc_hud))
 
       steering_available = CS.out.cruiseState.available and CS.out.vEgo > self.CP.minSteerSpeed
-      reduced_steering = CS.out.steeringPressed
+      reduced_steering = filtered_steering_pressed
       can_sends.extend(hondacan.create_lkas_hud(self.packer, self.CAN.lkas, self.CP, hud_control, CC.latActive,
                                                 steering_available, reduced_steering, alert_steer_required, CS.lkas_hud))
 

opendbc_repo/opendbc/car/honda/tests/test_honda.py

@@ -1,7 +1,12 @@
 import re
 
 from opendbc.car.structs import CarParams
-from opendbc.car.honda.carcontroller import CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX, get_civic_bosch_modified_steer_can_max
+from opendbc.car.honda.carcontroller import (
+  CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX,
+  get_civic_bosch_modified_steer_can_max,
+  get_civic_bosch_modified_steering_pressed,
+  get_civic_bosch_modified_torque_lpf_tau,
+)
 from opendbc.car.honda.fingerprints import FW_VERSIONS
 from opendbc.car.honda.values import CAR, HONDA_BOSCH, HONDA_BOSCH_TJA_CONTROL, HondaFlags
 
@@ -30,3 +35,21 @@ class TestHondaFingerprint:
 
     CP.dashcamOnly = False
     assert get_civic_bosch_modified_steer_can_max(4096, CP) == 4096
+
+  def test_modified_civic_torque_lpf_tau_reacts_to_sign_change(self):
+    assert get_civic_bosch_modified_torque_lpf_tau(0.7, -0.1, 25.0) == 0.09
+    assert get_civic_bosch_modified_torque_lpf_tau(0.02, 0.01, 12.0) == 0.15
+    assert get_civic_bosch_modified_torque_lpf_tau(0.30, 0.0, 12.0) == 0.11
+
+  def test_modified_civic_steering_pressed_filter_rejects_short_same_direction_spikes(self):
+    filter_s, pressed = get_civic_bosch_modified_steering_pressed(True, 1500.0, 0.8, 0.01, False)
+    assert not pressed
+    assert filter_s > 0.01
+
+    filter_s = 0.31
+    filter_s, pressed = get_civic_bosch_modified_steering_pressed(True, 1500.0, 0.8, filter_s, False)
+    assert pressed
+
+  def test_modified_civic_steering_pressed_filter_allows_opposing_driver_torque_quickly(self):
+    filter_s, pressed = get_civic_bosch_modified_steering_pressed(True, -1500.0, 0.8, 0.11, False)
+    assert pressed

selfdrive/controls/lib/longcontrol.py

@@ -32,10 +32,12 @@ def long_control_state_trans(CP, active, long_control_state, v_ego,
   # Ignore cruise standstill if car has a gas interceptor
   cruise_standstill = cruise_standstill and not CP.enableGasInterceptorDEPRECATED
   stopping_condition = should_stop
-  starting_condition = (not should_stop and
-                        not cruise_standstill and
-                        not brake_pressed)
-  stopping_release_condition = starting_condition and allow_stopping_release
+  release_condition = not should_stop and not brake_pressed
+  starting_condition = release_condition and not cruise_standstill
+  # Some stock ACC platforms keep standstill latched until they see positive drive torque.
+  # Once the planner has sustained a release request, allow LongControl to leave stopping
+  # even if the standstill bit has not dropped yet.
+  stopping_release_condition = release_condition and allow_stopping_release
   started_condition = v_ego > starpilot_toggles.vEgoStarting
 
   if not active:
@@ -166,7 +168,7 @@ class LongControl:
       self.stop_release_counter = 0
       return True
 
-    if should_stop or CS.brakePressed or CS.cruiseState.standstill:
+    if should_stop or CS.brakePressed:
       self.stop_release_counter = 0
       return False
 

selfdrive/controls/tests/test_longcontrol.py

@@ -34,7 +34,8 @@ class TestLongControlStateTransition:
                              should_stop=False, brake_pressed=True, cruise_standstill=False, starpilot_toggles=toggles)
     assert next_state == LongCtrlState.stopping
     next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
-                             should_stop=False, brake_pressed=False, cruise_standstill=True, starpilot_toggles=toggles)
+                             should_stop=False, brake_pressed=False, cruise_standstill=True, starpilot_toggles=toggles,
+                             allow_stopping_release=False)
     assert next_state == LongCtrlState.stopping
     next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
                              should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles)
@@ -89,6 +90,18 @@ def test_stopping_release_hysteresis_blocks_immediate_launch():
   assert next_state == LongCtrlState.stopping
 
 
+def test_stopping_release_allows_launch_while_cruise_standstill_latched():
+  CP = car.CarParams.new_message()
+  toggles = make_toggles(vEgoStarting=0.5)
+  active = True
+  current_state = LongCtrlState.stopping
+
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.0,
+                             should_stop=False, brake_pressed=False, cruise_standstill=True, starpilot_toggles=toggles,
+                             allow_stopping_release=True)
+  assert next_state == LongCtrlState.pid
+
+
 def test_starting_accel_unchanged_when_custom_profile_disabled():
   CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
   CP.longitudinalTuning.kpBP = [0.0]
@@ -174,6 +187,45 @@ def test_update_requires_sustained_positive_target_to_leave_stopping():
   assert lc.long_control_state == LongCtrlState.starting
 
 
+def test_update_releases_stopping_with_cruise_standstill_latched():
+  CP = car.CarParams.new_message(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.stopping
+  lc.last_output_accel = -2.003
+  CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
+  CS.cruiseState.standstill = True
+
+  release_frames = int(round(longcontrol.STOPPING_RELEASE_HYSTERESIS / longcontrol.DT_CTRL))
+  for _ in range(release_frames - 1):
+    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(startAccel=1.5),
+    )
+    assert lc.long_control_state == LongCtrlState.stopping
+    assert output_accel <= 0.0
+
+  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(startAccel=1.5),
+  )
+
+  assert lc.long_control_state == LongCtrlState.pid
+  assert output_accel > 0.0
+
+
 def test_volt_testing_ground_handoff_freezes_integrator(monkeypatch):
   CP = car.CarParams.new_message()
   CP.brand = "gm"

selfdrive/ui/lib/starpilot_state.py

@@ -162,7 +162,10 @@ class StarPilotState:
             
             cp_flags = self._safe_get(CP, "flags", 0)
             self.car_state.hasModeStarButtons = car_make == "hyundai" and bool(cp_flags & HyundaiFlags.CANFD)
-            self.car_state.lkasAllowedForAOL = car_make == "hyundai" and bool(cp_flags & (HyundaiFlags.CANFD | HyundaiFlags.HAS_LDA_BUTTON))
+            self.car_state.lkasAllowedForAOL = (
+                (car_make == "hyundai" and bool(cp_flags & (HyundaiFlags.CANFD | HyundaiFlags.HAS_LDA_BUTTON))) or
+                car_make == "honda"
+            )
             self.car_state.longitudinalActuatorDelay = float(self._safe_get(CP, "longitudinalActuatorDelay", self.car_state.longitudinalActuatorDelay))
             self.car_state.startAccel = float(self._safe_get(CP, "startAccel", self.car_state.startAccel))
             self.car_state.steerActuatorDelay = float(self._safe_get(CP, "steerActuatorDelay", self.car_state.steerActuatorDelay))

starpilot/common/starpilot_variables.py

@@ -501,7 +501,10 @@ class StarPilotVariables:
     latAccelFactor = CP.lateralTuning.torque.latAccelFactor
     if not math.isfinite(latAccelFactor):
       latAccelFactor = 0.0
-    toggle.lkas_allowed_for_aol = toggle.car_make == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD or CP.flags & HyundaiFlags.HAS_LDA_BUTTON)
+    toggle.lkas_allowed_for_aol = (
+      (toggle.car_make == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD or CP.flags & HyundaiFlags.HAS_LDA_BUTTON)) or
+      toggle.car_make == "honda"
+    )
     longitudinalActuatorDelay = CP.longitudinalActuatorDelay
     toggle.openpilot_longitudinal = CP.openpilotLongitudinalControl and not toggle.disable_openpilot_long
     pcm_cruise = CP.pcmCruise

starpilot/controls/starpilot_card.py

@@ -80,7 +80,7 @@ class StarPilotCard:
   def update(self, carState, starpilotCarState, sm, starpilot_toggles):
     self.switchback_mode_enabled = self.params_memory.get_bool("SwitchbackModeEnabled")
 
-    if self.CP.brand == "hyundai":
+    if self.CP.brand == "hyundai" or starpilot_toggles.lkas_allowed_for_aol:
       for be in carState.buttonEvents:
         if be.type == ButtonType.lkas and be.pressed and starpilot_toggles.always_on_lateral_lkas:
           self.always_on_lateral_allowed = not self.always_on_lateral_allowed

starpilot/controls/tests/test_starpilot_card.py

@@ -0,0 +1,74 @@
+from types import SimpleNamespace
+
+from openpilot.starpilot.controls import starpilot_card as spc
+
+
+class FakeParams:
+  def __init__(self, *args, **kwargs):
+    self._store = {}
+
+  def get_bool(self, key):
+    return bool(self._store.get(key, False))
+
+  def put_bool(self, key, value):
+    self._store[key] = bool(value)
+
+  def get_int(self, key, default=0):
+    return int(self._store.get(key, default))
+
+  def put_int(self, key, value):
+    self._store[key] = int(value)
+
+  def put_bool_nonblocking(self, key, value):
+    self.put_bool(key, value)
+
+
+class FakeSM(dict):
+  def __init__(self, *args, updated=None, **kwargs):
+    super().__init__(*args, **kwargs)
+    self.updated = updated or {}
+
+
+def test_honda_lkas_button_can_toggle_always_on_lateral(monkeypatch, tmp_path):
+  monkeypatch.setattr(spc, "Params", FakeParams)
+  monkeypatch.setattr(spc, "is_FrogsGoMoo", lambda: False)
+  monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path)
+
+  card = spc.StarPilotCard(SimpleNamespace(brand="honda"), SimpleNamespace(alternativeExperience=0))
+
+  car_state = SimpleNamespace(
+    buttonEvents=[SimpleNamespace(type=spc.ButtonType.lkas, pressed=True)],
+    cruiseState=SimpleNamespace(available=False),
+    gearShifter=spc.GearShifter.drive,
+    brakePressed=False,
+    gasPressed=False,
+    standstill=False,
+    vEgo=15.0,
+  )
+  starpilot_car_state = SimpleNamespace(distancePressed=False)
+  sm = FakeSM({
+    "carControl": SimpleNamespace(longActive=False),
+    "selfdriveState": SimpleNamespace(active=False, alertType=[], experimentalMode=False),
+    "starpilotSelfdriveState": SimpleNamespace(alertType=[]),
+    "starpilotPlan": SimpleNamespace(lateralCheck=True),
+    "liveCalibration": SimpleNamespace(calPerc=100),
+  }, updated={"starpilotPlan": False})
+  toggles = SimpleNamespace(
+    always_on_lateral_lkas=True,
+    always_on_lateral_main=False,
+    always_on_lateral_pause_speed=0.0,
+    bookmark_via_lkas=False,
+    conditional_experimental_mode=False,
+    experimental_mode_via_lkas=False,
+    force_coast_via_lkas=False,
+    lkas_allowed_for_aol=True,
+    pause_lateral_via_lkas=False,
+    pause_longitudinal_via_lkas=False,
+    speed_limit_controller=False,
+    switchback_mode_via_lkas=False,
+    traffic_mode_via_lkas=False,
+  )
+
+  ret = card.update(car_state, starpilot_car_state, sm, toggles)
+
+  assert ret.alwaysOnLateralAllowed is True