From 345747a042b3fd5ad0896bab72b037c8a336726b Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 30 Apr 2026 23:31:59 -0500 Subject: [PATCH] more stuff --- .../opendbc/car/honda/carcontroller.py | 87 ++++++++++++++++++- .../opendbc/car/honda/tests/test_honda.py | 25 +++++- selfdrive/controls/lib/longcontrol.py | 12 +-- selfdrive/controls/tests/test_longcontrol.py | 54 +++++++++++- selfdrive/ui/lib/starpilot_state.py | 5 +- starpilot/common/starpilot_variables.py | 5 +- starpilot/controls/starpilot_card.py | 2 +- .../controls/tests/test_starpilot_card.py | 74 ++++++++++++++++ 8 files changed, 252 insertions(+), 12 deletions(-) create mode 100644 starpilot/controls/tests/test_starpilot_card.py diff --git a/opendbc_repo/opendbc/car/honda/carcontroller.py b/opendbc_repo/opendbc/car/honda/carcontroller.py index e85aa6f7e..57bd18879 100644 --- a/opendbc_repo/opendbc/car/honda/carcontroller.py +++ b/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)) diff --git a/opendbc_repo/opendbc/car/honda/tests/test_honda.py b/opendbc_repo/opendbc/car/honda/tests/test_honda.py index ad6d6a7dd..ec91081bd 100644 --- a/opendbc_repo/opendbc/car/honda/tests/test_honda.py +++ b/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 diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index cb3604bac..7a2abf36a 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/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 diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index fb56c9989..1013964e7 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/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" diff --git a/selfdrive/ui/lib/starpilot_state.py b/selfdrive/ui/lib/starpilot_state.py index 23640cfc4..8a58806df 100644 --- a/selfdrive/ui/lib/starpilot_state.py +++ b/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)) diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 71bb08993..f9f7a1bb1 100644 --- a/starpilot/common/starpilot_variables.py +++ b/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 diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 268830efa..f24668909 100644 --- a/starpilot/controls/starpilot_card.py +++ b/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 diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py new file mode 100644 index 000000000..124040adb --- /dev/null +++ b/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