diff --git a/opendbc_repo/opendbc/car/honda/carcontroller.py b/opendbc_repo/opendbc/car/honda/carcontroller.py index 609e57db9..3aed25b39 100644 --- a/opendbc_repo/opendbc/car/honda/carcontroller.py +++ b/opendbc_repo/opendbc/car/honda/carcontroller.py @@ -6,23 +6,9 @@ from opendbc.car.honda import hondacan from opendbc.car.honda.values import CAR, CruiseButtons, HONDA_BOSCH, HONDA_BOSCH_CANFD, HONDA_BOSCH_RADARLESS, \ HONDA_BOSCH_TJA_CONTROL, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams, HondaFlags from opendbc.car.interfaces import CarControllerBase -from openpilot.starpilot.common.testing_grounds import testing_ground VisualAlert = structs.CarControl.HUDControl.VisualAlert LongCtrlState = structs.CarControl.Actuators.LongControlState -CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX = 4864 - - -def civic_bosch_modified_lateral_testing_ground_active() -> bool: - return testing_ground.use("8", "B") - - -def get_civic_bosch_modified_steer_can_max(base_steer_can_max: int, CP) -> int: - if CP.carFingerprint == CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED) and civic_bosch_modified_lateral_testing_ground_active(): - return CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX - 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)) torque_cmd_abs = abs(float(torque_cmd)) @@ -192,9 +178,6 @@ class CarController(CarControllerBase): 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 bool(self.CP.flags & HondaFlags.EPS_MODIFIED) and civic_bosch_modified_lateral_testing_ground_active() - def _modified_civic_standard_active(self) -> bool: return self.CP.carFingerprint == CAR.HONDA_CIVIC_BOSCH and bool(self.CP.flags & HondaFlags.EPS_MODIFIED) @@ -261,12 +244,8 @@ class CarController(CarControllerBase): # **** process the car messages **** # steer torque is converted back to CAN reference (positive when steering right) - steer_can_max = get_civic_bosch_modified_steer_can_max(self.params.STEER_MAX, self.CP) - if steer_can_max != self.params.STEER_MAX: - apply_torque = int(np.clip(-limited_torque * steer_can_max, -steer_can_max, steer_can_max)) - else: - apply_torque = int(np.interp(-limited_torque * self.params.STEER_MAX, - self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V)) + apply_torque = int(np.interp(-limited_torque * self.params.STEER_MAX, + self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V)) # Send CAN commands can_sends = [] diff --git a/opendbc_repo/opendbc/car/honda/tests/test_honda.py b/opendbc_repo/opendbc/car/honda/tests/test_honda.py index c174c7baf..0fecc2590 100644 --- a/opendbc_repo/opendbc/car/honda/tests/test_honda.py +++ b/opendbc_repo/opendbc/car/honda/tests/test_honda.py @@ -6,8 +6,6 @@ from opendbc.car.structs import CarParams from opendbc.car import gen_empty_fingerprint from opendbc.car.honda.interface import CarInterface 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, ) @@ -28,17 +26,6 @@ class TestHondaFingerprint: def test_tja_bosch_only(self): assert set(HONDA_BOSCH_TJA_CONTROL).issubset(set(HONDA_BOSCH)), "Nidec car found in TJA control list" - def test_modified_civic_steer_can_max_is_scoped(self, monkeypatch): - CP = CarParams.new_message() - CP.carFingerprint = CAR.HONDA_CIVIC_BOSCH - CP.flags = int(HondaFlags.BOSCH | HondaFlags.EPS_MODIFIED) - - monkeypatch.setattr("opendbc.car.honda.carcontroller.civic_bosch_modified_lateral_testing_ground_active", lambda: True) - assert get_civic_bosch_modified_steer_can_max(4096, CP) == CIVIC_BOSCH_MODIFIED_STEER_CAN_MAX - - CP.flags = int(HondaFlags.BOSCH) - 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.10 assert get_civic_bosch_modified_torque_lpf_tau(0.02, -0.01, 8.0) == 0.28 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index c9f92a351..468b91a33 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -14,25 +14,25 @@ def civic_bosch_modified_lateral_testing_ground_active() -> bool: def get_civic_bosch_modified_pid_output_scale(desired_angle_deg: float, desired_angle_delta_deg: float, v_ego: float) -> float: abs_angle = abs(desired_angle_deg) - if abs_angle < 8.0: - return 1.0 - - speed_weight = min(max((v_ego - 2.0) / 8.0, 0.0), 1.0) - angle_weight = min(max((abs_angle - 8.0) / 32.0, 0.0), 1.0) + speed_weight = min(max((v_ego - 4.0) / 10.0, 0.0), 1.0) + center_weight = min(max((4.0 - abs_angle) / 4.0, 0.0), 1.0) + angle_weight = min(max((abs_angle - 8.0) / 24.0, 0.0), 1.0) phase = desired_angle_deg * desired_angle_delta_deg is_left = desired_angle_deg > 0.0 - base_scale = 0.02 if is_left else 0.05 - turn_in_scale = 0.02 if is_left else 0.03 - unwind_scale = 0.00 if is_left else 0.02 + center_taper = 0.06 + base_scale = 0.08 if is_left else 0.10 + turn_in_scale = 0.08 if is_left else 0.10 + unwind_scale = 0.10 if is_left else 0.14 - scale = 1.0 + (speed_weight * angle_weight * base_scale) + scale = 1.0 - (speed_weight * center_weight * center_taper) + scale += speed_weight * angle_weight * base_scale if phase > 0.2: scale += speed_weight * angle_weight * turn_in_scale elif phase < -0.2: - scale += speed_weight * angle_weight * unwind_scale + scale -= speed_weight * angle_weight * unwind_scale - return scale + return max(scale, 0.84) class LatControlPID(LatControl): diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 9b16263aa..ec74862ba 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -211,19 +211,19 @@ IONIQ_6_FF_CUTOFF = 0.48 IONIQ_6_FF_CUTOFF_WIDTH = 0.12 IONIQ_6_TRANSITION_SPEED = 10.0 IONIQ_6_PHASE_SCALE = 0.10 -IONIQ_6_TURN_IN_BOOST_LEFT = 0.96 -IONIQ_6_TURN_IN_BOOST_RIGHT = 1.00 +IONIQ_6_TURN_IN_BOOST_LEFT = 1.02 +IONIQ_6_TURN_IN_BOOST_RIGHT = 1.12 IONIQ_6_UNWIND_TAPER_LEFT = 1.52 IONIQ_6_UNWIND_TAPER_RIGHT = 2.92 IONIQ_6_FRICTION_MULT = 0.995 IONIQ_6_FRICTION_LAT_RISE = 0.20 IONIQ_6_FRICTION_JERK_RISE = 0.24 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.29 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.40 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.31 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.46 IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.60 IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 3.50 -IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.14 -IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.22 +IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.16 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.28 IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.38 IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 3.00 IONIQ_6_CENTER_TAPER_MAX = 0.046 diff --git a/selfdrive/controls/tests/test_conditional_experimental_mode.py b/selfdrive/controls/tests/test_conditional_experimental_mode.py index 690adfb8b..88cbb49d0 100644 --- a/selfdrive/controls/tests/test_conditional_experimental_mode.py +++ b/selfdrive/controls/tests/test_conditional_experimental_mode.py @@ -57,8 +57,8 @@ def run_stop_light_detector(cem, v_ego, *, steps: int, tracking_lead: bool = Fal def make_update_sm(*, standstill: bool): return { - "carState": SimpleNamespace(standstill=standstill, leftBlinker=False, rightBlinker=False), - "starpilotCarState": SimpleNamespace(trafficModeEnabled=False), + "carState": SimpleNamespace(standstill=standstill, leftBlinker=False, rightBlinker=False, gasPressed=False), + "starpilotCarState": SimpleNamespace(trafficModeEnabled=False, dashboardStopSign=0, accelPressed=False), } @@ -206,6 +206,21 @@ def test_standstill_update_can_activate_exp_from_red_light_detection(monkeypatch assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] +def test_standstill_update_can_activate_exp_from_dashboard_stop_sign(monkeypatch): + cem = make_cem(model_length=80.0, model_stopped=False) + toggles = make_update_toggles() + sm = make_update_sm(standstill=True) + sm["starpilotCarState"].dashboardStopSign = 1 + + monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None) + + cem.update(0.0, sm, toggles) + + assert cem.experimental_mode + assert cem.standstill_stop_reason == "sign" + assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] + + def test_standstill_green_light_clears_exp_immediately(monkeypatch): cem = make_cem(model_length=80.0, model_stopped=False) toggles = make_update_toggles() @@ -259,6 +274,43 @@ def test_standstill_force_stop_keeps_exp_on_even_if_red_light_latch_clears(monke assert cem.status_value == conditional_experimental_mode_module.CEStatus["STOP_LIGHT"] +def test_standstill_stop_sign_latches_until_pedal_even_after_force_stop_ends(monkeypatch): + cem = make_cem(model_length=80.0, model_stopped=False) + toggles = make_update_toggles() + sm = make_update_sm(standstill=True) + sm["starpilotCarState"].dashboardStopSign = 1 + + monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None) + cem.update(0.0, sm, toggles) + + assert cem.experimental_mode + assert cem.standstill_stop_reason == "sign" + + sm["starpilotCarState"].dashboardStopSign = 0 + cem.update(0.0, sm, toggles) + + assert cem.experimental_mode + assert cem.standstill_stop_reason == "sign" + + +def test_standstill_stop_sign_releases_on_pedal(monkeypatch): + cem = make_cem(model_length=80.0, model_stopped=False) + toggles = make_update_toggles() + sm = make_update_sm(standstill=True) + sm["starpilotCarState"].dashboardStopSign = 1 + + monkeypatch.setattr(cem, "stop_sign_and_light", lambda *args, **kwargs: None) + cem.update(0.0, sm, toggles) + assert cem.experimental_mode + + sm["starpilotCarState"].dashboardStopSign = 0 + sm["carState"].gasPressed = True + cem.update(0.0, sm, toggles) + + assert not cem.experimental_mode + assert cem.standstill_stop_reason is None + + def test_slow_lead_holds_through_tracking_flap_for_high_confidence_vision_lead(): v_ego = 35 * CV.MPH_TO_MS cem = make_cem( diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index b5d745713..b0ff78626 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -380,11 +380,13 @@ class TestLatControl: assert lac_log.active def test_civic_bosch_modified_pid_scale_curve(self): - assert get_civic_bosch_modified_pid_output_scale(0.0, 0.0, 12.0) == 1.0 + assert get_civic_bosch_modified_pid_output_scale(0.0, 0.0, 12.0) < 1.0 assert get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(20.0, 0.0, 12.0) assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(-20.0, 0.0, 12.0) - assert get_civic_bosch_modified_pid_output_scale(-20.0, 0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0) - assert get_civic_bosch_modified_pid_output_scale(-20.0, 0.5, 4.0) < get_civic_bosch_modified_pid_output_scale(-20.0, 0.5, 12.0) + assert get_civic_bosch_modified_pid_output_scale(20.0, -0.5, 12.0) < get_civic_bosch_modified_pid_output_scale(20.0, 0.0, 12.0) + assert get_civic_bosch_modified_pid_output_scale(-20.0, 0.5, 12.0) < get_civic_bosch_modified_pid_output_scale(-20.0, 0.0, 12.0) + assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0) + assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 4.0) < get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0) def test_civic_bosch_modified_pid_testing_ground_update_path(self, monkeypatch): controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(HONDA.HONDA_CIVIC_BOSCH) diff --git a/starpilot/controls/lib/conditional_experimental_mode.py b/starpilot/controls/lib/conditional_experimental_mode.py index fab55711f..849a11957 100644 --- a/starpilot/controls/lib/conditional_experimental_mode.py +++ b/starpilot/controls/lib/conditional_experimental_mode.py @@ -89,6 +89,7 @@ class ConditionalExperimentalMode: self.stop_light_detected = False self.stop_light_model_detected = False self.stop_approach_hold_until = 0.0 + self.standstill_stop_reason = None self.prev_experimental_mode = False # For hysteresis self.mode_hold_until = 0.0 self.mode_false_since = 0.0 @@ -97,6 +98,9 @@ class ConditionalExperimentalMode: now = time.monotonic() standstill = bool(sm["carState"].standstill) + if not standstill: + self.standstill_stop_reason = None + self.status_value = CEStatus["OFF"] if self.params.get_bool("SafeMode") else restore_persisted_ce_state(self.params, self.params_memory) if not is_manual_ce_status(self.status_value) and not standstill: @@ -120,14 +124,10 @@ class ConditionalExperimentalMode: self.mode_false_since = 0.0 # Keep the stop-light path live at standstill so EXP stays pinned for a red - # light / stop sign, but can immediately release to CHILL when the model - # clears the stop (green light / open path). + # light / stop sign. Stop signs latch until pedal, while stop lights can + # immediately release to CHILL when the model clears the stop. self.stop_sign_and_light(v_ego, sm, starpilot_toggles.conditional_model_stop_time) - standstill_stop_hold = ( - self.stop_light_detected or - getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False) or - getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False) - ) + standstill_stop_hold = self.get_standstill_stop_hold(sm) self.experimental_mode = standstill_stop_hold self.prev_experimental_mode = self.experimental_mode @@ -141,6 +141,31 @@ class ConditionalExperimentalMode: self.stop_light_detected &= not is_manual_ce_status(self.status_value) self.stop_light_filter.x = 0 + def get_standstill_stop_hold(self, sm): + dash_stop_sign = ( + bool(getattr(self.starpilot_planner.starpilot_vcruise, "stop_sign_confirmed", False)) or + bool(getattr(sm["starpilotCarState"], "dashboardStopSign", 0) > 0) + ) + force_stop_active = bool(getattr(self.starpilot_planner.starpilot_vcruise, "forcing_stop", False)) + pedal_override = bool(getattr(sm["carState"], "gasPressed", False) or getattr(sm["starpilotCarState"], "accelPressed", False)) + + if pedal_override or not bool(sm["carState"].standstill): + self.standstill_stop_reason = None + return False + + if dash_stop_sign: + self.standstill_stop_reason = "sign" + elif self.stop_light_detected or force_stop_active: + if self.standstill_stop_reason is None: + self.standstill_stop_reason = "light" + elif self.standstill_stop_reason == "light": + self.standstill_stop_reason = None + + if self.standstill_stop_reason == "sign": + return True + + return bool(self.stop_light_detected or force_stop_active) + def check_conditions(self, v_ego, sm, starpilot_toggles): below_speed = starpilot_toggles.conditional_limit > v_ego >= 1 and not self.starpilot_planner.starpilot_following.following_lead below_speed_with_lead = starpilot_toggles.conditional_limit_lead > v_ego >= 1 and self.starpilot_planner.starpilot_following.following_lead